Line data Source code
1 :
2 : #include "AP_Motors_Thrust_Linearization.h"
3 : #include "AP_Motors_Class.h"
4 : #include <AP_BattMonitor/AP_BattMonitor.h>
5 : #include <AP_Baro/AP_Baro.h>
6 : #include <AP_AHRS/AP_AHRS.h>
7 : #include <AP_Vehicle/AP_Vehicle_Type.h>
8 :
9 : #define AP_MOTORS_BATT_VOLT_FILT_HZ 0.5 // battery voltage filtered at 0.5hz
10 :
11 : #if APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
12 : // Example does not instantiate baro so cannot do density compensation
13 : #define AP_MOTORS_DENSITY_COMP 0
14 : #else
15 : #ifndef AP_MOTORS_DENSITY_COMP
16 : #define AP_MOTORS_DENSITY_COMP 1
17 : #endif
18 : #endif
19 :
20 : #if APM_BUILD_TYPE(APM_BUILD_Heli)
21 : // defaults to no linearisation to not break users existing setups
22 : #define THRST_LIN_THST_EXPO_DEFAULT 0.0f // set to 0 for linear and 1 for second order approximation
23 : #define THRST_LIN_SPIN_MIN_DEFAULT 0.0f // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range
24 : #define THRST_LIN_SPIN_MAX_DEFAULT 1.0f // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
25 : #define THRST_LIN_BAT_VOLT_MAX_DEFAULT 0.0f // voltage limiting max default
26 : #define THRST_LIN_BAT_VOLT_MIN_DEFAULT 0.0f // voltage limiting min default (voltage dropping below this level will have no effect)
27 : #else
28 : #define THRST_LIN_THST_EXPO_DEFAULT 0.65f // set to 0 for linear and 1 for second order approximation
29 : #define THRST_LIN_SPIN_MIN_DEFAULT 0.15f // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range
30 : #define THRST_LIN_SPIN_MAX_DEFAULT 0.95f // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
31 : #define THRST_LIN_BAT_VOLT_MAX_DEFAULT 0.0f // voltage limiting max default
32 : #define THRST_LIN_BAT_VOLT_MIN_DEFAULT 0.0f // voltage limiting min default (voltage dropping below this level will have no effect)
33 : #endif
34 :
35 :
36 : extern const AP_HAL::HAL& hal;
37 :
38 : const AP_Param::GroupInfo Thrust_Linearization::var_info[] = {
39 :
40 : // @Param: THST_EXPO
41 : // @DisplayName: Thrust Curve Expo
42 : // @Description: motor thrust curve exponent (0.0 for linear to 1.0 for second order curve)
43 : // @Range: -1 1
44 : // @User: Standard
45 : AP_GROUPINFO("THST_EXPO", 1, Thrust_Linearization, curve_expo, THRST_LIN_THST_EXPO_DEFAULT),
46 :
47 : // @Param: SPIN_MIN
48 : // @DisplayName: Motor Spin minimum
49 : // @Description: Point at which the thrust starts expressed as a number from 0 to 1 in the entire output range. Should be higher than MOT_SPIN_ARM.
50 : // @Values: 0.0:Low, 0.15:Default, 0.3:High
51 : // @Range: 0.0 0.3
52 : // @User: Standard
53 : AP_GROUPINFO("SPIN_MIN", 2, Thrust_Linearization, spin_min, THRST_LIN_SPIN_MIN_DEFAULT),
54 :
55 : // @Param: SPIN_MAX
56 : // @DisplayName: Motor Spin maximum
57 : // @Description: Point at which the thrust saturates expressed as a number from 0 to 1 in the entire output range
58 : // @Values: 0.9:Low, 0.95:Default, 1.0:High
59 : // @Range: 0.9 1.0
60 : // @User: Standard
61 : AP_GROUPINFO("SPIN_MAX", 3, Thrust_Linearization, spin_max, THRST_LIN_SPIN_MAX_DEFAULT),
62 :
63 : // @Param: BAT_IDX
64 : // @DisplayName: Battery compensation index
65 : // @Description: Which battery monitor should be used for doing compensation
66 : // @Values: 0:First battery, 1:Second battery
67 : // @Range: 0 15
68 : // @User: Standard
69 : AP_GROUPINFO("BAT_IDX", 4, Thrust_Linearization, batt_idx, 0),
70 :
71 : // @Param: BAT_V_MAX
72 : // @DisplayName: Battery voltage compensation maximum voltage
73 : // @Description: Battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.2 * cell count, 0 = Disabled
74 : // @Range: 6 53
75 : // @Units: V
76 : // @User: Standard
77 : AP_GROUPINFO("BAT_V_MAX", 5, Thrust_Linearization, batt_voltage_max, THRST_LIN_BAT_VOLT_MAX_DEFAULT),
78 :
79 : // @Param: BAT_V_MIN
80 : // @DisplayName: Battery voltage compensation minimum voltage
81 : // @Description: Battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.3 * cell count, 0 = Disabled
82 : // @Range: 6 42
83 : // @Units: V
84 : // @User: Standard
85 : AP_GROUPINFO("BAT_V_MIN", 6, Thrust_Linearization, batt_voltage_min, THRST_LIN_BAT_VOLT_MIN_DEFAULT),
86 :
87 : AP_GROUPEND
88 : };
89 :
90 1282 : Thrust_Linearization::Thrust_Linearization(AP_Motors& _motors) :
91 1282 : lift_max(1.0),
92 1282 : motors(_motors)
93 : {
94 : // setup battery voltage filtering
95 1282 : batt_voltage_filt.set_cutoff_frequency(AP_MOTORS_BATT_VOLT_FILT_HZ);
96 1282 : batt_voltage_filt.reset(1.0);
97 :
98 : #if APM_BUILD_TYPE(APM_BUILD_Heli)
99 : AP_Param::setup_object_defaults(this, var_info);
100 : #endif
101 1282 : }
102 :
103 : // converts desired thrust to linearized actuator output in a range of 0~1
104 82722549 : float Thrust_Linearization::thrust_to_actuator(float thrust_in) const
105 : {
106 82722549 : thrust_in = constrain_float(thrust_in, 0.0, 1.0);
107 82722549 : return spin_min + (spin_max - spin_min) * apply_thrust_curve_and_volt_scaling(thrust_in);
108 : }
109 :
110 : // inverse of above, tested with AP_Motors/examples/expo_inverse_test
111 : // used to calculate equivelent motor throttle level to direct ouput, used in tailsitter transtions
112 176306 : float Thrust_Linearization::actuator_to_thrust(float actuator) const
113 : {
114 176306 : actuator = (actuator - spin_min) / (spin_max - spin_min);
115 176306 : return constrain_float(remove_thrust_curve_and_volt_scaling(actuator), 0.0, 1.0);
116 : }
117 :
118 : // apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
119 82722549 : float Thrust_Linearization::apply_thrust_curve_and_volt_scaling(float thrust) const
120 : {
121 82722549 : float battery_scale = 1.0;
122 82722549 : if (is_positive(batt_voltage_filt.get())) {
123 82722549 : battery_scale = 1.0 / batt_voltage_filt.get();
124 : }
125 : // apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0
126 82722549 : float thrust_curve_expo = constrain_float(curve_expo, -1.0, 1.0);
127 82722549 : if (is_zero(thrust_curve_expo)) {
128 : // zero expo means linear, avoid floating point exception for small values
129 1141090 : return lift_max * thrust * battery_scale;
130 : }
131 81581459 : float throttle_ratio = ((thrust_curve_expo - 1.0) + safe_sqrt((1.0 - thrust_curve_expo) * (1.0 - thrust_curve_expo) + 4.0 * thrust_curve_expo * lift_max * thrust)) / (2.0 * thrust_curve_expo);
132 81581459 : return constrain_float(throttle_ratio * battery_scale, 0.0, 1.0);
133 : }
134 :
135 : // inverse of above, tested with AP_Motors/examples/expo_inverse_test
136 : // used to calculate equivelent motor throttle level to direct ouput, used in tailsitter transtions
137 176306 : float Thrust_Linearization::remove_thrust_curve_and_volt_scaling(float throttle) const
138 : {
139 176306 : float battery_scale = 1.0;
140 176306 : if (is_positive(batt_voltage_filt.get())) {
141 176306 : battery_scale = 1.0 / batt_voltage_filt.get();
142 : }
143 : // apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0
144 176306 : float thrust_curve_expo = constrain_float(curve_expo, -1.0, 1.0);
145 176306 : if (is_zero(thrust_curve_expo)) {
146 : // zero expo means linear, avoid floating point exception for small values
147 0 : return throttle / (lift_max * battery_scale);
148 : }
149 176306 : float thrust = ((throttle / battery_scale) * (2.0 * thrust_curve_expo)) - (thrust_curve_expo - 1.0);
150 176306 : thrust = (thrust * thrust) - ((1.0 - thrust_curve_expo) * (1.0 - thrust_curve_expo));
151 176306 : thrust /= 4.0 * thrust_curve_expo * lift_max;
152 176306 : return constrain_float(thrust, 0.0, 1.0);
153 : }
154 :
155 : // update_lift_max from battery voltage - used for voltage compensation
156 36253665 : void Thrust_Linearization::update_lift_max_from_batt_voltage()
157 : {
158 : #if AP_BATTERY_ENABLED
159 : // sanity check battery_voltage_min is not too small
160 : // if disabled or misconfigured exit immediately
161 36253665 : float _batt_voltage = motors.has_option(AP_Motors::MotorOptions::BATT_RAW_VOLTAGE) ? AP::battery().voltage(batt_idx) : AP::battery().voltage_resting_estimate(batt_idx);
162 :
163 36253665 : if ((batt_voltage_max <= 0) || (batt_voltage_min >= batt_voltage_max) || (_batt_voltage < 0.25 * batt_voltage_min)) {
164 10027668 : batt_voltage_filt.reset(1.0);
165 10027668 : lift_max = 1.0;
166 10027668 : return;
167 : }
168 :
169 26225997 : batt_voltage_min.set(MAX(batt_voltage_min, batt_voltage_max * 0.6));
170 :
171 : // constrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
172 26225997 : _batt_voltage = constrain_float(_batt_voltage, batt_voltage_min, batt_voltage_max);
173 :
174 26225997 : if (!motors.has_option(AP_Motors::MotorOptions::BATT_RAW_VOLTAGE)) {
175 : // filter at 0.5 Hz
176 26225997 : batt_voltage_filt.apply(_batt_voltage / batt_voltage_max, motors.get_dt_s());
177 : } else {
178 : // reset is equivalent to no filtering
179 0 : batt_voltage_filt.reset(_batt_voltage / batt_voltage_max);
180 : }
181 :
182 : // calculate lift max
183 26225997 : float thrust_curve_expo = constrain_float(curve_expo, -1.0, 1.0);
184 26225997 : lift_max = batt_voltage_filt.get() * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt.get() * batt_voltage_filt.get();
185 : #endif
186 : }
187 :
188 : // return gain scheduling gain based on voltage and air density
189 67539996 : float Thrust_Linearization::get_compensation_gain() const
190 : {
191 : // avoid divide by zero
192 67539996 : if (get_lift_max() <= 0.0) {
193 0 : return 1.0;
194 : }
195 :
196 67539996 : float ret = 1.0 / get_lift_max();
197 :
198 : #if AP_MOTORS_DENSITY_COMP == 1
199 : // air density ratio is increasing in density / decreasing in altitude
200 67539996 : const float air_density_ratio = AP::ahrs().get_air_density_ratio();
201 67539996 : if (air_density_ratio > 0.3 && air_density_ratio < 1.5) {
202 67539996 : ret *= 1.0 / constrain_float(air_density_ratio, 0.5, 1.25);
203 : }
204 : #endif
205 67539996 : return ret;
206 : }
|
