Line data Source code
1 : #include "AC_InputManager_Heli.h"
2 : #include <AP_Math/AP_Math.h>
3 : #include <AP_HAL/AP_HAL.h>
4 : #include <GCS_MAVLink/GCS.h>
5 :
6 : extern const AP_HAL::HAL& hal;
7 :
8 : const AP_Param::GroupInfo AC_InputManager_Heli::var_info[] = {
9 :
10 : // parameters from parent vehicle
11 : AP_NESTEDGROUPINFO(AC_InputManager, 0),
12 :
13 : // Indicies 1-4 (STAB_COL_1 thru STAB_COL_4) have been replaced.
14 :
15 : // @Param: ACRO_COL_EXP
16 : // @DisplayName: Acro Mode Collective Expo
17 : // @Description: Used to soften collective pitch inputs near center point in Acro mode.
18 : // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
19 : // @Range: -0.5 0.95
20 : // @User: Advanced
21 : AP_GROUPINFO("ACRO_COL_EXP", 5, AC_InputManager_Heli, _acro_col_expo, 0),
22 :
23 : // @Param: STB_COL_1
24 : // @DisplayName: Stabilize Collective Low
25 : // @Description: Helicopter's minimum collective pitch setting at zero collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN.
26 : // @Range: 0 100
27 : // @Units: %
28 : // @Increment: 1
29 : // @User: Standard
30 : AP_GROUPINFO("STB_COL_1", 6, AC_InputManager_Heli, _heli_stab_col_min, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT),
31 :
32 : // @Param: STB_COL_2
33 : // @DisplayName: Stabilize Collective Mid-Low
34 : // @Description: Helicopter's collective pitch setting at mid-low (40%) collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN.
35 : // @Range: 0 100
36 : // @Units: %
37 : // @Increment: 1
38 : // @User: Standard
39 : AP_GROUPINFO("STB_COL_2", 7, AC_InputManager_Heli, _heli_stab_col_low, AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT),
40 :
41 : // @Param: STB_COL_3
42 : // @DisplayName: Stabilize Collective Mid-High
43 : // @Description: Helicopter's collective pitch setting at mid-high (60%) collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN.
44 : // @Range: 0 100
45 : // @Units: %
46 : // @Increment: 1
47 : // @User: Standard
48 : AP_GROUPINFO("STB_COL_3", 8, AC_InputManager_Heli, _heli_stab_col_high, AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT),
49 :
50 : // @Param: STB_COL_4
51 : // @DisplayName: Stabilize Collective High
52 : // @Description: Helicopter's maximum collective pitch setting at full collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN.
53 : // @Range: 0 100
54 : // @Units: %
55 : // @Increment: 1
56 : // @User: Standard
57 : AP_GROUPINFO("STB_COL_4", 9, AC_InputManager_Heli, _heli_stab_col_max, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT),
58 :
59 : AP_GROUPEND
60 : };
61 :
62 : // get_pilot_desired_collective - rescale's pilot collective pitch input in Stabilize and Acro modes
63 1059136 : float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in)
64 : {
65 : float slope_low, slope_high, slope_range, slope_run, scalar;
66 : float stab_col_out, acro_col_out;
67 :
68 : // calculate stabilize collective value which scales pilot input to reduced collective range
69 : // code implements a 3-segment curve with knee points at 40% and 60% throttle input
70 1059136 : if (control_in < 400){ // control_in ranges from 0 to 1000
71 917091 : slope_low = _heli_stab_col_min * 0.01f;
72 917091 : slope_high = _heli_stab_col_low * 0.01f;
73 917091 : slope_range = 0.4f;
74 917091 : slope_run = control_in * 0.001f;
75 142045 : } else if(control_in <600){ // control_in ranges from 0 to 1000
76 11001 : slope_low = _heli_stab_col_low * 0.01f;
77 11001 : slope_high = _heli_stab_col_high * 0.01f;
78 11001 : slope_range = 0.2f;
79 11001 : slope_run = (control_in - 400) * 0.001f; // control_in ranges from 0 to 1000
80 : } else {
81 131044 : slope_low = _heli_stab_col_high * 0.01f;
82 131044 : slope_high = _heli_stab_col_max * 0.01f;
83 131044 : slope_range = 0.4f;
84 131044 : slope_run = (control_in - 600) * 0.001f; // control_in ranges from 0 to 1000
85 : }
86 :
87 1059136 : scalar = (slope_high - slope_low)/slope_range;
88 1059136 : stab_col_out = slope_low + slope_run * scalar;
89 1059136 : stab_col_out = constrain_float(stab_col_out, 0.0f, 1.0f);
90 :
91 : //
92 : // calculate expo-scaled acro collective
93 : // range check expo
94 1059136 : if (_acro_col_expo > 1.0f) {
95 0 : _acro_col_expo.set(1.0f);
96 : }
97 :
98 1059136 : if (_acro_col_expo <= 0.0f) {
99 0 : acro_col_out = control_in * 0.001f; // control_in ranges from 0 to 1000
100 : } else {
101 : // expo variables
102 : float col_in, col_in3, col_out;
103 1059136 : col_in = (float)(control_in-500)/500.0f; // control_in ranges from 0 to 1000
104 1059136 : col_in3 = col_in*col_in*col_in;
105 1059136 : col_out = (_acro_col_expo * col_in3) + ((1.0f-_acro_col_expo)*col_in);
106 1059136 : acro_col_out = 0.5f + col_out*0.5f;
107 : }
108 1059136 : acro_col_out = constrain_float(acro_col_out, 0.0f, 1.0f);
109 :
110 : // ramp function
111 1059136 : if (is_positive(_ramp)) {
112 2964 : float dt = 1/(float)_loop_rate;
113 : // factor 2 to transition over a time span of 0.5s
114 2964 : _ramp -= 2*dt;
115 2964 : _ramp = constrain_float(_ramp, 0.0f, 1.0f);
116 : }
117 :
118 : //set Stabilize or Acro collective output
119 : float new_flightmode_col_output;
120 1059136 : if (_im_flags_heli.use_stab_col) {
121 1057936 : new_flightmode_col_output = stab_col_out;
122 : } else {
123 1200 : new_flightmode_col_output = acro_col_out;
124 : }
125 :
126 : // scale collective output smoothly between previous and current mode output
127 : float collective_out;
128 1059136 : collective_out = new_flightmode_col_output * (1.0 - _ramp) + _ramp * _old_flightmode_col_output;
129 1059136 : collective_out = constrain_float(collective_out, 0.0f, 1.0f);
130 :
131 1059136 : return collective_out;
132 : }
133 :
134 : // parameter_check - check if input manager specific parameters are sensible
135 2149 : bool AC_InputManager_Heli::parameter_check(char* fail_msg, uint8_t fail_msg_len) const
136 : {
137 :
138 : const struct StabCheck {
139 : const char *name;
140 : int16_t value;
141 2149 : } stab_checks[] = {
142 : {"IM_STB_COL_1", _heli_stab_col_min },
143 : {"IM_STB_COL_2", _heli_stab_col_low },
144 : {"IM_STB_COL_3", _heli_stab_col_high },
145 : {"IM_STB_COL_4", _heli_stab_col_max },
146 2149 : };
147 :
148 : // check values are within valid range
149 10745 : for (uint8_t i=0; i<ARRAY_SIZE(stab_checks); i++) {
150 8596 : const StabCheck check = stab_checks[i];
151 8596 : if ((check.value < 0) || (check.value > 100)){
152 0 : hal.util->snprintf(fail_msg, fail_msg_len, "%s out of range", check.name);
153 0 : return false;
154 : }
155 : }
156 : // check values are in correct order
157 8596 : for (uint8_t i=1; i<ARRAY_SIZE(stab_checks); i++) {
158 6447 : if ((stab_checks[i-1].value >= stab_checks[i].value)){
159 0 : hal.util->snprintf(fail_msg, fail_msg_len, "%s must be < %s", stab_checks[i-1].name, stab_checks[i].name);
160 0 : return false;
161 : }
162 : }
163 : // all other cases parameters are OK
164 2149 : return true;
165 : }
166 :
|
