Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_MotorsMulticopter.h"
17 : #include <AP_HAL/AP_HAL.h>
18 : #include <AP_BattMonitor/AP_BattMonitor.h>
19 : #include <SRV_Channel/SRV_Channel.h>
20 : #include <AP_Logger/AP_Logger.h>
21 :
22 : #include <AP_Vehicle/AP_Vehicle_Type.h>
23 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
24 : #define AP_MOTORS_PARAM_PREFIX "Q_M_"
25 : #else
26 : #define AP_MOTORS_PARAM_PREFIX "MOT_"
27 : #endif
28 :
29 : extern const AP_HAL::HAL& hal;
30 :
31 : // parameters for the motor class
32 : const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
33 : // 0 was used by TB_RATIO
34 : // 1,2,3 were used by throttle curve
35 : // 5 was SPIN_ARMED
36 :
37 : // @Param: YAW_HEADROOM
38 : // @DisplayName: Matrix Yaw Min
39 : // @Description: Yaw control is given at least this pwm in microseconds range
40 : // @Range: 0 500
41 : // @Units: PWM
42 : // @Increment: 1
43 : // @User: Advanced
44 : AP_GROUPINFO("YAW_HEADROOM", 6, AP_MotorsMulticopter, _yaw_headroom, AP_MOTORS_YAW_HEADROOM_DEFAULT),
45 :
46 : // 7 was THR_LOW_CMP
47 :
48 : // @Param: THST_EXPO
49 : // @DisplayName: Thrust Curve Expo
50 : // @Description: Motor thrust curve exponent (0.0 for linear to 1.0 for second order curve)
51 : // @Range: -1.0 1.0
52 : // @User: Advanced
53 : AP_GROUPINFO("THST_EXPO", 8, AP_MotorsMulticopter, thr_lin.curve_expo, AP_MOTORS_THST_EXPO_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: Advanced
61 : AP_GROUPINFO("SPIN_MAX", 9, AP_MotorsMulticopter, thr_lin.spin_max, AP_MOTORS_SPIN_MAX_DEFAULT),
62 :
63 : // @Param: BAT_VOLT_MAX
64 : // @DisplayName: Battery voltage compensation maximum voltage
65 : // @Description: Battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.2 * cell count, 0 = Disabled
66 : // @Range: 6 53
67 : // @Units: V
68 : // @Increment: 0.1
69 : // @User: Advanced
70 : AP_GROUPINFO("BAT_VOLT_MAX", 10, AP_MotorsMulticopter, thr_lin.batt_voltage_max, AP_MOTORS_BAT_VOLT_MAX_DEFAULT),
71 :
72 : // @Param: BAT_VOLT_MIN
73 : // @DisplayName: Battery voltage compensation minimum voltage
74 : // @Description: Battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.3 * cell count, 0 = Disabled
75 : // @Range: 6 42
76 : // @Units: V
77 : // @Increment: 0.1
78 : // @User: Advanced
79 : AP_GROUPINFO("BAT_VOLT_MIN", 11, AP_MotorsMulticopter, thr_lin.batt_voltage_min, AP_MOTORS_BAT_VOLT_MIN_DEFAULT),
80 :
81 : // @Param: BAT_CURR_MAX
82 : // @DisplayName: Motor Current Max
83 : // @Description: Maximum current over which maximum throttle is limited (0 = Disabled)
84 : // @Range: 0 200
85 : // @Units: A
86 : // @Increment: 0.1
87 : // @User: Advanced
88 : AP_GROUPINFO("BAT_CURR_MAX", 12, AP_MotorsMulticopter, _batt_current_max, AP_MOTORS_BAT_CURR_MAX_DEFAULT),
89 :
90 : // 13, 14 were used by THR_MIX_MIN, THR_MIX_MAX
91 :
92 : // @Param: PWM_TYPE
93 : // @DisplayName: Output PWM type
94 : // @Description: This selects the output PWM type, allowing for normal PWM continuous output, OneShot, brushed or DShot motor output.PWMRange and PWMAngle are PWM special/rare cases for ESCs that dont calibrate normally (some Sub motors) or where each ESC must have its PWM range set individually using the Servo params instead of PWM_MIN/MAX parameters.
95 : // @Values: 0:Normal,1:OneShot,2:OneShot125,3:Brushed,4:DShot150,5:DShot300,6:DShot600,7:DShot1200,8:PWMRange,9:PWMAngle
96 : // @User: Advanced
97 : // @RebootRequired: True
98 : AP_GROUPINFO("PWM_TYPE", 15, AP_MotorsMulticopter, _pwm_type, float(PWMType::NORMAL)),
99 :
100 : // @Param: PWM_MIN
101 : // @DisplayName: PWM output minimum
102 : // @Description: This sets the min PWM output value in microseconds that will ever be output to the motors
103 : // @Units: PWM
104 : // @Range: 0 2000
105 : // @Increment: 1
106 : // @User: Advanced
107 : AP_GROUPINFO("PWM_MIN", 16, AP_MotorsMulticopter, _pwm_min, 1000),
108 :
109 : // @Param: PWM_MAX
110 : // @DisplayName: PWM output maximum
111 : // @Description: This sets the max PWM value in microseconds that will ever be output to the motors
112 : // @Units: PWM
113 : // @Range: 0 2000
114 : // @Increment: 1
115 : // @User: Advanced
116 : AP_GROUPINFO("PWM_MAX", 17, AP_MotorsMulticopter, _pwm_max, 2000),
117 :
118 : // @Param: SPIN_MIN
119 : // @DisplayName: Motor Spin minimum
120 : // @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.
121 : // @Values: 0.0:Low, 0.15:Default, 0.25:High
122 : // @Range: 0.0 0.25
123 : // @Increment: 0.01
124 : // @User: Advanced
125 : AP_GROUPINFO("SPIN_MIN", 18, AP_MotorsMulticopter, thr_lin.spin_min, AP_MOTORS_SPIN_MIN_DEFAULT),
126 :
127 : // @Param: SPIN_ARM
128 : // @DisplayName: Motor Spin armed
129 : // @Description: Point at which the motors start to spin expressed as a number from 0 to 1 in the entire output range. Should be lower than MOT_SPIN_MIN.
130 : // @Values: 0.0:Low, 0.1:Default, 0.2:High
131 : // @Range: 0.0 0.2
132 : // @Increment: 0.01
133 : // @User: Advanced
134 : AP_GROUPINFO("SPIN_ARM", 19, AP_MotorsMulticopter, _spin_arm, AP_MOTORS_SPIN_ARM_DEFAULT),
135 :
136 : // @Param: BAT_CURR_TC
137 : // @DisplayName: Motor Current Max Time Constant
138 : // @Description: Time constant used to limit the maximum current
139 : // @Range: 0 10
140 : // @Units: s
141 : // @Increment: 0.01
142 : // @User: Advanced
143 : AP_GROUPINFO("BAT_CURR_TC", 20, AP_MotorsMulticopter, _batt_current_time_constant, AP_MOTORS_BAT_CURR_TC_DEFAULT),
144 :
145 : // @Param: THST_HOVER
146 : // @DisplayName: Thrust Hover Value
147 : // @Description: Motor thrust needed to hover expressed as a number from 0 to 1
148 : // @Range: 0.125 0.6875
149 : // @User: Advanced
150 : AP_GROUPINFO("THST_HOVER", 21, AP_MotorsMulticopter, _throttle_hover, AP_MOTORS_THST_HOVER_DEFAULT),
151 :
152 : // @Param: HOVER_LEARN
153 : // @DisplayName: Hover Value Learning
154 : // @Description: Enable/Disable automatic learning of hover throttle
155 : // @Values{Copter}: 0:Disabled, 1:Learn, 2:Learn and Save
156 : // @Values{Sub}: 0:Disabled
157 : // @Values{Plane}: 0:Disabled, 1:Learn, 2:Learn and Save
158 : // @User: Advanced
159 : AP_GROUPINFO("HOVER_LEARN", 22, AP_MotorsMulticopter, _throttle_hover_learn, HOVER_LEARN_AND_SAVE),
160 :
161 : // @Param: SAFE_DISARM
162 : // @DisplayName: Motor PWM output disabled when disarmed
163 : // @Description: Disables motor PWM output when disarmed
164 : // @Values: 0:PWM enabled while disarmed, 1:PWM disabled while disarmed
165 : // @User: Advanced
166 : AP_GROUPINFO("SAFE_DISARM", 23, AP_MotorsMulticopter, _disarm_disable_pwm, 0),
167 :
168 : // @Param{Copter,Plane}: YAW_SV_ANGLE
169 : // @DisplayName: Yaw Servo Max Lean Angle
170 : // @Description: Yaw servo's maximum lean angle (Tricopter only)
171 : // @Range: 5 80
172 : // @Units: deg
173 : // @Increment: 1
174 : // @User: Standard
175 : AP_GROUPINFO_FRAME("YAW_SV_ANGLE", 35, AP_MotorsMulticopter, _yaw_servo_angle_max_deg, 30, AP_PARAM_FRAME_TRICOPTER),
176 :
177 : // @Param: SPOOL_TIME
178 : // @DisplayName: Spool up time
179 : // @Description: Time in seconds to spool up the motors from zero to min throttle.
180 : // @Range: 0.05 2
181 : // @Units: s
182 : // @Increment: 0.1
183 : // @User: Advanced
184 : AP_GROUPINFO("SPOOL_TIME", 36, AP_MotorsMulticopter, _spool_up_time, AP_MOTORS_SPOOL_UP_TIME_DEFAULT),
185 :
186 : // @Param: BOOST_SCALE
187 : // @DisplayName: Motor boost scale
188 : // @Description: Booster motor output scaling factor vs main throttle. The output to the BoostThrottle servo will be the main throttle times this scaling factor. A higher scaling factor will put more of the load on the booster motor. A value of 1 will set the BoostThrottle equal to the main throttle.
189 : // @Range: 0 5
190 : // @Increment: 0.1
191 : // @User: Advanced
192 : AP_GROUPINFO("BOOST_SCALE", 37, AP_MotorsMulticopter, _boost_scale, 0),
193 :
194 : // 38 RESERVED for BAT_POW_MAX
195 :
196 : // @Param: BAT_IDX
197 : // @DisplayName: Battery compensation index
198 : // @Description: Which battery monitor should be used for doing compensation
199 : // @Values: 0:First battery, 1:Second battery
200 : // @Range: 0 15
201 : // @Increment: 1
202 : // @User: Advanced
203 : AP_GROUPINFO("BAT_IDX", 39, AP_MotorsMulticopter, thr_lin.batt_idx, 0),
204 :
205 : // @Param: SLEW_UP_TIME
206 : // @DisplayName: Output slew time for increasing throttle
207 : // @Description: Time in seconds to slew output from zero to full. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
208 : // @Range: 0 0.5
209 : // @Units: s
210 : // @Increment: 0.001
211 : // @User: Advanced
212 : AP_GROUPINFO("SLEW_UP_TIME", 40, AP_MotorsMulticopter, _slew_up_time, AP_MOTORS_SLEW_TIME_DEFAULT),
213 :
214 : // @Param: SLEW_DN_TIME
215 : // @DisplayName: Output slew time for decreasing throttle
216 : // @Description: Time in seconds to slew output from full to zero. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
217 : // @Range: 0 0.5
218 : // @Units: s
219 : // @Increment: 0.001
220 : // @User: Advanced
221 : AP_GROUPINFO("SLEW_DN_TIME", 41, AP_MotorsMulticopter, _slew_dn_time, AP_MOTORS_SLEW_TIME_DEFAULT),
222 :
223 : // @Param: SAFE_TIME
224 : // @DisplayName: PWM re-enable delay after arming when disarmed pwm was disabled
225 : // @Description: Delay after arming before spool transitions are allowed, used to let ESCs/servos complete startup when PWM was disabled while disarmed.
226 : // @Range: 0 5
227 : // @Units: s
228 : // @Increment: 0.001
229 : // @User: Advanced
230 : AP_GROUPINFO("SAFE_TIME", 42, AP_MotorsMulticopter, _safe_time, AP_MOTORS_SAFE_TIME_DEFAULT),
231 :
232 : // @Param: OPTIONS
233 : // @DisplayName: Motor options
234 : // @Description: Motor options
235 : // @Bitmask: 0:Voltage compensation uses raw voltage
236 : // @User: Advanced
237 : AP_GROUPINFO("OPTIONS", 43, AP_MotorsMulticopter, _options, 0),
238 :
239 : // @Param: SPOOL_TIM_DN
240 : // @DisplayName: Spool down time
241 : // @Description: Time to ramp the throttle ceiling down toward zero. If 0, SPOOL_TIME is used.
242 : // @Range: 0 2
243 : // @Units: s
244 : // @Increment: 0.001
245 : // @User: Advanced
246 : AP_GROUPINFO("SPOOL_TIM_DN", 44, AP_MotorsMulticopter, _spool_down_time, 0),
247 :
248 : // @Param: IDLE_SEC
249 : // @DisplayName: Idle time
250 : // @Description: Delay after reaching ground idle when armed to allow ESC startup to complete.
251 : // @Range: 0 5
252 : // @Units: s
253 : // @Increment: 0.1
254 : // @User: Advanced
255 : AP_GROUPINFO("IDLE_SEC", 45, AP_MotorsMulticopter, _idle_time_delay_s, 0),
256 :
257 : AP_GROUPEND
258 : };
259 :
260 : // Constructor
261 1202 : AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t speed_hz) :
262 : AP_Motors(speed_hz),
263 1202 : _throttle_limit(1.0f)
264 : {
265 1202 : AP_Param::setup_object_defaults(this, var_info);
266 1202 : };
267 :
268 : // output - sends commands to the motors
269 36102663 : void AP_MotorsMulticopter::output()
270 : {
271 : // update throttle filter
272 36102663 : update_throttle_filter();
273 :
274 : // calc filtered battery voltage and lift_max
275 36102663 : thr_lin.update_lift_max_from_batt_voltage();
276 :
277 : // run spool logic
278 36102663 : output_logic();
279 :
280 : // calculate thrust
281 36102663 : output_armed_stabilizing();
282 :
283 : // apply any thrust compensation for the frame
284 36102663 : thrust_compensation();
285 :
286 : // convert rpy_thrust values to pwm
287 36102663 : output_to_motors();
288 :
289 : // output any booster throttle
290 36102663 : output_boost_throttle();
291 :
292 : // output raw roll/pitch/yaw/thrust
293 36102663 : output_rpyt();
294 :
295 : // check for any external limit flags
296 36102663 : update_external_limits();
297 :
298 : // clear mask of overridden motors
299 36102663 : _motor_mask_override = 0;
300 36102663 : };
301 :
302 : // update external limits from scripting
303 36102663 : void AP_MotorsMulticopter::update_external_limits()
304 : {
305 : #if AP_SCRIPTING_ENABLED
306 : // merge external (scripting) limit flags into the internal limit state
307 36102663 : limit.roll |= external_limits.roll;
308 36102663 : limit.pitch |= external_limits.pitch;
309 36102663 : limit.yaw |= external_limits.yaw;
310 36102663 : limit.throttle_lower |= external_limits.throttle_lower;
311 36102663 : limit.throttle_upper |= external_limits.throttle_upper;
312 : #endif
313 36102663 : }
314 :
315 : // output booster throttle, if any
316 36102663 : void AP_MotorsMulticopter::output_boost_throttle(void)
317 : {
318 36102663 : if (_boost_scale > 0) {
319 0 : float throttle = constrain_float(get_throttle() * _boost_scale, 0, 1);
320 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_boost_throttle, throttle * 1000);
321 : } else {
322 36102663 : SRV_Channels::set_output_scaled(SRV_Channel::k_boost_throttle, 0);
323 : }
324 36102663 : }
325 :
326 : // output roll/pitch/yaw/thrust
327 36102663 : void AP_MotorsMulticopter::output_rpyt(void)
328 : {
329 36102663 : SRV_Channels::set_output_scaled(SRV_Channel::k_roll_out, _roll_in_ff * 4500);
330 36102663 : SRV_Channels::set_output_scaled(SRV_Channel::k_pitch_out, _pitch_in_ff * 4500);
331 36102663 : SRV_Channels::set_output_scaled(SRV_Channel::k_yaw_out, _yaw_in_ff * 4500);
332 36102663 : SRV_Channels::set_output_scaled(SRV_Channel::k_thrust_out, get_throttle() * 1000);
333 36102663 : }
334 :
335 : // sends minimum values out to the motors
336 43981 : void AP_MotorsMulticopter::output_min()
337 : {
338 43981 : set_desired_spool_state(DesiredSpoolState::SHUT_DOWN);
339 43981 : _spool_state = SpoolState::SHUT_DOWN;
340 43981 : output();
341 43981 : }
342 :
343 : // update the throttle input filter
344 36102663 : void AP_MotorsMulticopter::update_throttle_filter()
345 : {
346 36102663 : const float last_thr = _throttle_filter.get();
347 :
348 36102663 : if (armed()) {
349 23838589 : _throttle_filter.apply(_throttle_in, _dt_s);
350 : // constrain filtered throttle
351 23838589 : if (_throttle_filter.get() < 0.0f) {
352 621537 : _throttle_filter.reset(0.0f);
353 : }
354 23838589 : if (_throttle_filter.get() > 1.0f) {
355 94339 : _throttle_filter.reset(1.0f);
356 : }
357 : } else {
358 : // when disarmed, reset filter to 0 so throttle slew resumes from zero on arm
359 12264074 : _throttle_filter.reset(0.0f);
360 : }
361 :
362 36102663 : float new_thr = _throttle_filter.get();
363 :
364 36102663 : if (!is_equal(last_thr, new_thr)) {
365 19213987 : _throttle_slew.update(new_thr, AP_HAL::micros());
366 : }
367 :
368 : // calculate slope normalized from per-micro
369 36102663 : const float rate = fabsf(_throttle_slew.slope() * 1e6);
370 36102663 : _throttle_slew_rate = _throttle_slew_filter.apply(rate, _dt_s);
371 36102663 : }
372 :
373 : // return current-limit as a normalised throttle [0..1] within the min..max range
374 : // returns 1.0 when current limiting is disabled, disarmed, or telemetry is unavailable
375 19631709 : float AP_MotorsMulticopter::get_current_limit_max_throttle()
376 : {
377 : #if AP_BATTERY_ENABLED
378 19631709 : AP_BattMonitor &battery = AP::battery();
379 :
380 19631709 : const uint8_t batt_idx = thr_lin.get_battery_index();
381 : float _batt_current;
382 :
383 : // early exits:
384 : // - current limiting disabled (param <= 0)
385 : // - disarmed (do not apply a throttle limit while disarmed)
386 : // - no current telemetry available
387 19631709 : if (_batt_current_max <= 0 || // return maximum if current limiting is disabled
388 19726566 : !armed() || // remove throttle limit if disarmed
389 94857 : !battery.current_amps(_batt_current, batt_idx)) { // no current monitoring is available
390 19536852 : _throttle_limit = 1.0f;
391 19536852 : return 1.0f;
392 : }
393 :
394 : // pack resistance for selected battery index
395 94857 : float _batt_resistance = battery.get_resistance(batt_idx);
396 :
397 : // if resistance is unknown/zero, do not attempt limiting
398 94857 : if (is_zero(_batt_resistance)) {
399 1912 : _throttle_limit = 1.0f;
400 1912 : return 1.0f;
401 : }
402 :
403 : // compute the maximum permissible current
404 : // the limit is the minimum of:
405 : // - user/parameter maximum (_batt_current_max)
406 : // - ohmic limit that avoids sagging below the configured minimum voltage:
407 : // I_max_ohmic = I_measured + (V_batt - V_min) / R_internal
408 : // calculate the maximum current to prevent voltage sag below _batt_voltage_min
409 92945 : float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(batt_idx) - thr_lin.get_battery_min_voltage()) / _batt_resistance);
410 :
411 : // ratio of measured current to the permissible current (1.0 means at the limit)
412 92945 : float batt_current_ratio = _batt_current / batt_current_max;
413 :
414 : // integrate throttle headroom based on current margin
415 : // batt_current_ratio = I_measured / I_max
416 : // - if ratio < 1 then (1 - ratio) > 0 and allowed headroom increases
417 : // - if ratio > 1 then (1 - ratio) < 0 and allowed headroom decreases
418 : // _batt_current_time_constant scales the step size; this is an integrator, not an RC toward a target
419 92945 : _throttle_limit += (_dt_s / (_dt_s + _batt_current_time_constant)) * (1.0f - batt_current_ratio);
420 :
421 : // constrain the headroom scaler to [0.2, 1.0]
422 : // ensures some minimum authority above hover remains available
423 92945 : _throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f);
424 :
425 : // map the limit into the pilot/controller throttle range
426 : // at hover (get_throttle_hover) the result is 1.0 (no reduction)
427 : // toward full throttle the available range is reduced by _throttle_limit
428 : // returns a normalised maximum throttle in [0..1]
429 92945 : return get_throttle_hover() + ((1.0 - get_throttle_hover()) * _throttle_limit);
430 : #else
431 : // no battery backend, no limiting
432 : return 1.0;
433 : #endif
434 : }
435 :
436 : #if HAL_LOGGING_ENABLED
437 : // 10hz logging of voltage scaling and max trust
438 757935 : void AP_MotorsMulticopter::Log_Write()
439 : {
440 757935 : const struct log_MotBatt pkt_mot {
441 : LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG),
442 757935 : time_us : AP_HAL::micros64(),
443 757935 : lift_max : thr_lin.get_lift_max(),
444 1515870 : bat_volt : thr_lin.batt_voltage_filt.get(),
445 757935 : th_limit : _throttle_limit,
446 757935 : th_average_max : _throttle_avg_max,
447 757935 : th_out : _throttle_out,
448 757935 : mot_fail_flags : (uint8_t)(_thrust_boost | (_thrust_balanced << 1U)),
449 757935 : };
450 757935 : AP::logger().WriteBlock(&pkt_mot, sizeof(pkt_mot));
451 757935 : }
452 : #endif
453 :
454 : // convert normalised actuator output (0..1) to pwm
455 : // in SHUT_DOWN: output 0 pwm when _disarm_disable_pwm && !armed(), else pwm_min
456 : // in all other states: map [0..1] linearly between pwm_min and pwm_max
457 141756127 : int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
458 : {
459 : float pwm_output;
460 141756127 : if (_spool_state == SpoolState::SHUT_DOWN) {
461 : // in shutdown mode, use PWM 0 or minimum PWM
462 58265736 : if (_disarm_disable_pwm && !armed()) {
463 0 : pwm_output = 0;
464 : } else {
465 58265736 : pwm_output = get_pwm_output_min();
466 : }
467 : } else {
468 : // in all other spool modes, covert to desired PWM
469 83490391 : pwm_output = get_pwm_output_min() + (get_pwm_output_max() - get_pwm_output_min()) * actuator;
470 : }
471 :
472 141756127 : return pwm_output;
473 : }
474 :
475 : // slew limiting on motor outputs:
476 : // - MOT_SLEW_UP_TIME = time to go 0 -> 1 (s); 0 disables up-slew limiting
477 : // - MOT_SLEW_DN_TIME = time to go 1 -> 0 (s); 0 disables down-slew limiting
478 : // - both times are constrained to 0..0.5 s
479 : // - no slew limiting while in SHUT_DOWN to allow immediate motor de-energisation
480 84198163 : void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float input)
481 : {
482 : /*
483 : If MOT_SLEW_UP_TIME is 0 (default), no slew limit is applied to increasing output.
484 : If MOT_SLEW_DN_TIME is 0 (default), no slew limit is applied to decreasing output.
485 : MOT_SLEW_UP_TIME and MOT_SLEW_DN_TIME are constrained to 0.0~0.5 for sanity.
486 : If spool mode is shutdown, no slew limit is applied to allow immediate disarming of motors.
487 : */
488 :
489 : // Output limits with no slew time applied
490 84198163 : float output_slew_limit_up = 1.0f;
491 84198163 : float output_slew_limit_dn = 0.0f;
492 :
493 : // If MOT_SLEW_UP_TIME is set, calculate the highest allowed new output value, constrained 0.0~1.0
494 84198163 : if (is_positive(_slew_up_time)) {
495 0 : float output_delta_up_max = _dt_s / (constrain_float(_slew_up_time, 0.0f, 0.5f));
496 0 : output_slew_limit_up = constrain_float(actuator_output + output_delta_up_max, 0.0f, 1.0f);
497 : }
498 :
499 : // If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0
500 84198163 : if (is_positive(_slew_dn_time)) {
501 0 : float output_delta_dn_max = _dt_s / (constrain_float(_slew_dn_time, 0.0f, 0.5f));
502 0 : output_slew_limit_dn = constrain_float(actuator_output - output_delta_dn_max, 0.0f, 1.0f);
503 : }
504 :
505 : // Constrain change in output to within the above limits
506 84198163 : actuator_output = constrain_float(input, output_slew_limit_dn, output_slew_limit_up);
507 84198163 : }
508 :
509 : // gradually increase actuator output to spin_min
510 : // equals _spin_up_ratio * spin_min (constrained to [0..1])
511 1043006 : float AP_MotorsMulticopter::actuator_spin_up_to_ground_idle() const
512 : {
513 1043006 : return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * thr_lin.get_spin_min();
514 : }
515 :
516 : // return throttle out for motor motor_num, returns true if value is valid false otherwise
517 1385396 : bool AP_MotorsMulticopter::get_thrust(uint8_t motor_num, float& thr_out) const
518 : {
519 1385396 : if (motor_num >= AP_MOTORS_MAX_NUM_MOTORS || !motor_enabled[motor_num]) {
520 1210304 : return false;
521 : }
522 :
523 : // Constrain to linearization range.
524 175092 : const float actuator = constrain_float(_actuator[motor_num], thr_lin.get_spin_min(), thr_lin.get_spin_max());
525 :
526 : // Remove linearization and compensation gain
527 175092 : thr_out = thr_lin.actuator_to_thrust(actuator) / thr_lin.get_compensation_gain();
528 175092 : return true;
529 : }
530 :
531 : // parameter checks for MOT_PWM_MIN/MAX, returns true if parameters are valid
532 100455 : bool AP_MotorsMulticopter::check_mot_pwm_params() const
533 : {
534 : // _pwm_min is a value greater than or equal to 1.
535 : // _pwm_max is greater than _pwm_min.
536 : // The values of _pwm_min and _pwm_max are positive values.
537 100455 : if (_pwm_min < 1 || _pwm_min >= _pwm_max) {
538 2 : return false;
539 : }
540 100453 : return true;
541 : }
542 :
543 : // update_throttle_range - update throttle endpoints
544 27173 : void AP_MotorsMulticopter::update_throttle_range()
545 : {
546 : // if all outputs are digital adjust the range. We also do this for type PWM_RANGE, as those use the
547 : // scaled output, which is then mapped to PWM via the SRV_Channel library
548 27173 : if (SRV_Channels::have_digital_outputs(get_motor_mask()) || (_pwm_type == PWMType::PWM_RANGE) || (_pwm_type == PWMType::PWM_ANGLE)) {
549 128 : _pwm_min.set_and_default(1000);
550 128 : _pwm_max.set_and_default(2000);
551 : }
552 :
553 27173 : hal.rcout->set_esc_scaling(get_pwm_output_min(), get_pwm_output_max());
554 27173 : }
555 :
556 : // update the throttle input filter. should be called at 100hz
557 1305938 : void AP_MotorsMulticopter::update_throttle_hover(float dt)
558 : {
559 1305938 : if (_throttle_hover_learn != HOVER_LEARN_DISABLED) {
560 : // we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial.
561 1300085 : _throttle_hover.set(constrain_float(_throttle_hover + (dt / (dt + AP_MOTORS_THST_HOVER_TC)) * (get_throttle() - _throttle_hover), AP_MOTORS_THST_HOVER_MIN, AP_MOTORS_THST_HOVER_MAX));
562 : }
563 1305938 : }
564 :
565 : // set_desired_spool_state - set desired spool state with safety constraints
566 29334859 : void AP_MotorsMulticopter::set_desired_spool_state(DesiredSpoolState desired)
567 : {
568 : // Safety constraint: disarmed or no interlock means motors must shut down
569 : // (multicopters don't use GROUND_IDLE like helis - props must not spin without interlock)
570 29334859 : if (!armed() || !get_interlock()) {
571 11082542 : _spool_desired = DesiredSpoolState::SHUT_DOWN;
572 11082542 : return;
573 : }
574 :
575 : // No safety constraints active - accept requested state
576 18252317 : _spool_desired = desired;
577 : }
578 :
579 : // run spool logic
580 : // advance the motor spool state machine once per cycle
581 : // enforce arming/interlock and disarm-pwm safe-time guards
582 : // shape two normalised scalars with monotonic ramps during normal operation
583 : // - _spin_up_ratio controls motor spin/idle shaping
584 : // - _throttle_thrust_max defines the moving throttle ceiling, limited by current draw
585 : // both values are normalised between 0 and 1
586 : // zero thrust while !armed() or !get_interlock()
587 : // ramps are limited by the configured spool times (see minimum_spool_time)
588 : // disarm or interlock drop immediately forces SHUT_DOWN for safety
589 : // pre-takeoff checks and idle-time delay may hold in GROUND_IDLE until cleared
590 : // (get_spoolup_block(), IDLE_TIME after reaching ground-idle spin)
591 36102663 : void AP_MotorsMulticopter::output_logic()
592 : {
593 : // Minimum allowable spool time [s].
594 : // Prevents divide-by-zero or excessively abrupt transitions when calculating
595 : // spool_step = dt / time. A value of 0.05 s (~20 Hz) ensures numerically
596 : // stable ramps while remaining perceptually instantaneous to the user.
597 36102663 : const constexpr float minimum_spool_time = 0.05f;
598 :
599 : // disarm-pwm safety window
600 : // when _disarm_disable_pwm is enabled, pwm outputs are off while disarmed
601 : // after arming, _safe_time provides a delay to let escs or servos detect
602 : // the restored pwm signal and complete their startup sequence
603 : // the timer only advances while armed and resets to 0 when disarmed
604 36102663 : if (armed()) {
605 23838589 : if (_disarm_disable_pwm && (_disarm_safe_timer < _safe_time)) {
606 0 : _disarm_safe_timer += _dt_s;
607 : } else {
608 23838589 : _disarm_safe_timer = _safe_time;
609 : }
610 : } else {
611 12264074 : _disarm_safe_timer = 0.0f;
612 : }
613 :
614 : // safety rule (always applies)
615 : // if not armed or interlock is false, force both desired and actual
616 : // spool states to SHUT_DOWN immediately with no ramping
617 : // ensures zero thrust and prevents any motor output until both armed() and get_interlock() are true
618 36102663 : if (!armed() || !get_interlock()) {
619 12659277 : _spool_desired = DesiredSpoolState::SHUT_DOWN;
620 12659277 : _spool_state = SpoolState::SHUT_DOWN;
621 : }
622 :
623 36102663 : if (_spool_up_time < minimum_spool_time) {
624 : // Enforce a lower bound on spool-up time for numerical stability and
625 : // predictability.
626 0 : _spool_up_time.set(minimum_spool_time);
627 : }
628 :
629 : // limits flags convention:
630 : // - SHUT_DOWN, GROUND_IDLE: limit.set_all(true) // no attitude/throttle authority
631 : // - SPOOLING_UP/DOWN, THROTTLE_UNLIMITED: set_all(false) // controller runs normally
632 36102663 : switch (_spool_state) {
633 14980496 : case SpoolState::SHUT_DOWN:
634 : // motors should be stationary
635 : // servos set to their trim values or in a test condition
636 :
637 : // zero energy/thrust, pwm disabled
638 : // leave only after safe-time has elapsed and a higher-energy state is requested
639 : // next state is GROUND_IDLE
640 :
641 : // set limits flags
642 : // keep all limits set while motors are stopped and thrust is disabled
643 14980496 : limit.set_all(true);
644 :
645 : // set ramp variables
646 : // zero both ramps in shut_down
647 14980496 : _spin_up_ratio = 0.0f;
648 14980496 : _throttle_thrust_max = 0.0f;
649 14980496 : _idle_time = 0.0;
650 :
651 : // initialise motor failure variables
652 : // disable thrust boost in shut_down
653 14980496 : _thrust_boost = false;
654 14980496 : _thrust_boost_ratio = 0.0f;
655 :
656 : // ensure motors only begin spooling after arming safety delay has expired
657 : // leave SHUT_DOWN only once the safe-time window has elapsed and
658 : // a higher-energy state is requested. this prevents any PWM output
659 : // until ESCs or servos have completed their start-up sequence.
660 14980496 : if (_spool_desired != DesiredSpoolState::SHUT_DOWN && _disarm_safe_timer >= _safe_time.get()) {
661 772 : _spool_state = SpoolState::GROUND_IDLE;
662 : }
663 14980496 : break;
664 :
665 263368 : case SpoolState::GROUND_IDLE: {
666 : // motors should be stationary or at ground idle
667 : // servos should be moving to correct the current attitude
668 :
669 : // pwm may be enabled at or near spin_min if required
670 : // throttle does not yet follow demand
671 : // this staging state ensures all spool-up checks pass before allowing thrust authority
672 :
673 : // set limits flags
674 : // keep all limits set while in a low-energy or gated state
675 263368 : limit.set_all(true);
676 :
677 263368 : float spin_up_ground_idle_ratio = 0.0f;
678 263368 : if ( is_positive(thr_lin.get_spin_min()) ) {
679 263368 : spin_up_ground_idle_ratio = _spin_arm / thr_lin.get_spin_min();
680 : }
681 :
682 263368 : if (_spin_up_ratio >= spin_up_ground_idle_ratio) {
683 : // Advance the idle-time delay only once the motors have reached ground-idle spin.
684 175421 : _idle_time = MIN(_idle_time_delay_s, _idle_time + _dt_s);
685 : }
686 :
687 : // set and increment ramp variables
688 263368 : switch (_spool_desired) {
689 9171 : case DesiredSpoolState::SHUT_DOWN: {
690 : // Down path: reduce spin toward 0 using SPOOL_TIM_DN if configured,
691 : // otherwise fall back to SPOOL_TIME for symmetry.
692 9171 : const float spool_time = _spool_down_time > minimum_spool_time ? _spool_down_time : _spool_up_time;
693 9171 : const float spool_step = _dt_s / spool_time;
694 9171 : _spin_up_ratio -= spool_step;
695 :
696 : // constrain ramp value and update mode
697 : // Drop back to SHUT_DOWN only after spin ratio hits zero.
698 9171 : if (_spin_up_ratio <= 0.0f) {
699 121 : _spin_up_ratio = 0.0f;
700 121 : _spool_state = SpoolState::SHUT_DOWN;
701 : }
702 9171 : break;
703 : }
704 :
705 116612 : case DesiredSpoolState::THROTTLE_UNLIMITED: {
706 : // Up path: ramp spin toward 1.0.
707 : // While the idle-time delay is running, hold spin at ground idle to allow ESC startup.
708 : // After the delay, continue ramping to 1.0 and proceed when spool-up checks are clear.
709 :
710 116612 : const float spool_step = _dt_s / _spool_up_time;
711 116612 : _spin_up_ratio += spool_step;
712 :
713 : // Hold at ground-idle spin until the configured idle-time delay has elapsed.
714 : // This allows ESCs to complete their startup sequence with PWM active at idle
715 : // before allowing further spool-up.
716 116612 : if (_idle_time < _idle_time_delay_s) {
717 0 : _spin_up_ratio = MIN(_spin_up_ratio, spin_up_ground_idle_ratio);
718 0 : break;
719 : }
720 :
721 : // wait for spin up to complete
722 116612 : if (_spin_up_ratio < 1.0f) {
723 112677 : _spin_up_complete = false;
724 : } else {
725 3935 : _spin_up_ratio = 1.0f;
726 3935 : if (!_spin_up_complete) {
727 : // Spin up is complete.
728 : // Enable spoolup block to hold the aircraft in GROUND_IDLE.
729 : // Main code should start checks when the block is enabled and remove the lock when ready.
730 737 : _spin_up_complete = true;
731 737 : set_spoolup_block(true);
732 : }
733 : }
734 116612 : if (_spin_up_complete && !get_spoolup_block()) {
735 : // only advance from ground idle when pre-takeoff checks have cleared
736 : // get_spoolup_block() is true while takeoff_check() is blocking spool-up
737 : // (e.g. esc telemetry inactive, rpm not in range, cpu overload, or disarmed)
738 : // must be false to proceed
739 734 : _spool_state = SpoolState::SPOOLING_UP;
740 : }
741 116612 : break;
742 : }
743 137585 : case DesiredSpoolState::GROUND_IDLE: {
744 : // slew limits for spin-up ratio while in ground idle
745 : // up uses _spool_up_time; down uses _spool_down_time (or up time if not set)
746 137585 : const float spool_up_step = _dt_s / _spool_up_time;
747 137585 : const float spool_down_time = _spool_down_time > minimum_spool_time ? _spool_down_time : _spool_up_time;
748 137585 : const float spool_down_step = _dt_s / spool_down_time;
749 :
750 : // target spin ratio while armed:
751 : // raise spin toward the armed idle (_spin_arm) as a fraction of spin_min
752 : // (keeps ratio normalized; if no spin_min, stay at 0)
753 :
754 : // asymmetrical slew toward target: limit decrease with spool_down_step and increase with spool_up_step
755 : // prevents step changes while allowing a different down vs up time constant
756 137585 : _spin_up_ratio += constrain_float(spin_up_ground_idle_ratio - _spin_up_ratio, -spool_down_step, spool_up_step);
757 137585 : break;
758 : }
759 : }
760 : // no throttle authority yet in ground idle
761 263368 : _throttle_thrust_max = 0.0f;
762 :
763 : // initialise motor failure variables
764 : // disable thrust boost in ground idle
765 263368 : _thrust_boost = false;
766 263368 : _thrust_boost_ratio = 0.0f;
767 263368 : break;
768 : }
769 10488 : case SpoolState::SPOOLING_UP: {
770 : // maximum throttle should move from minimum to maximum
771 : // spin is already 1.0 and ramp the throttle ceiling smoothly (avoid step changes)
772 10488 : const float spool_step = _dt_s / _spool_up_time;
773 :
774 : // initialize limits flags
775 : // clear all limits and allow normal attitude control during throttle ramp-up
776 10488 : limit.set_all(false);
777 :
778 : // make sure the motors are spooling in the correct direction
779 : // if desired target is below THROTTLE_UNLIMITED, reverse to SPOOLING_DOWN
780 10488 : if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) {
781 0 : _spool_state = SpoolState::SPOOLING_DOWN;
782 0 : break;
783 : }
784 :
785 : // set and increment ramp variables
786 : // hold spin at 1.0; increase throttle ceiling by bounded step
787 10488 : _spin_up_ratio = 1.0f;
788 10488 : _throttle_thrust_max += spool_step;
789 :
790 : // switch to THROTTLE_UNLIMITED once the moving ceiling no longer limits the commanded throttle
791 : // then set the ceiling to the steady-state current-limited maximum used in normal operation
792 10488 : if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
793 737 : _throttle_thrust_max = get_current_limit_max_throttle();
794 737 : _spool_state = SpoolState::THROTTLE_UNLIMITED;
795 9751 : } else if (_throttle_thrust_max < 0.0f) {
796 0 : _throttle_thrust_max = 0.0f;
797 : }
798 :
799 : // fade any thrust boost during spool-up
800 10488 : _thrust_boost = false;
801 10488 : _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - spool_step);
802 10488 : break;
803 : }
804 :
805 20763527 : case SpoolState::THROTTLE_UNLIMITED: {
806 : // minimum and maximum throttle at normal values
807 : // throttle follows controller demand, bounded by runtime current limiting
808 : // servos should exhibit normal flight behavior
809 :
810 : // step used to slew boost ratio (not used for throttle in this state)
811 20763527 : const float spool_step = _dt_s / _spool_up_time;
812 :
813 : // initialize limits flags
814 : // clear all limits and allow normal attitude control
815 20763527 : limit.set_all(false);
816 :
817 : // make sure the motors are spooling in the correct direction
818 : // if desired drops below THROTTLE_UNLIMITED, begin spooling down
819 20763527 : if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) {
820 500 : _spool_state = SpoolState::SPOOLING_DOWN;
821 500 : break;
822 : }
823 :
824 : // set steady-state scalars
825 : // hold spin at 1.0; throttle ceiling follows the steady-state current-limited maximum
826 20763027 : _spin_up_ratio = 1.0f;
827 20763027 : _throttle_thrust_max = get_current_limit_max_throttle();
828 :
829 : // manage thrust boost ratio smoothly:
830 : // - increase toward 1 when boost requested and not yet balanced
831 : // - otherwise decay toward 0
832 20763027 : if (_thrust_boost && !_thrust_balanced) {
833 0 : _thrust_boost_ratio = MIN(1.0, _thrust_boost_ratio + spool_step);
834 : } else {
835 20763027 : _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - spool_step);
836 : }
837 20763027 : break;
838 : }
839 :
840 84784 : case SpoolState::SPOOLING_DOWN:
841 : // maximum throttle should move from maximum to minimum
842 : // throttle ceiling ramps down; spin stays at 1.0 until we reach ground idle
843 : // servos should exhibit normal flight behavior
844 :
845 : // initialize limits flags
846 : // clear all limits; allow normal attitude control during throttle ramp-down
847 84784 : limit.set_all(false);
848 :
849 : // make sure the motors are spooling in the correct direction
850 : // if desired returns to THROTTLE_UNLIMITED, reverse to SPOOLING_UP
851 84784 : if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) {
852 5 : _spool_state = SpoolState::SPOOLING_UP;
853 5 : break;
854 : }
855 :
856 : // set and increment ramp variables
857 : // hold spin at 1.0 during throttle down-ramp; spin reduction happens in GROUND_IDLE
858 84779 : _spin_up_ratio = 1.0f;
859 :
860 : // choose down-ramp time: use SPOOL_TIM_DN when configured, else mirror SPOOL_TIME
861 84779 : const float spool_time = _spool_down_time > minimum_spool_time ? _spool_down_time : _spool_up_time;
862 84779 : const float spool_step = _dt_s / spool_time;
863 :
864 : // ramp the normalized throttle ceiling toward 0
865 84779 : _throttle_thrust_max -= spool_step;
866 :
867 : // constrain ramp value and update mode
868 : // clamp lower bound at 0
869 84779 : if (_throttle_thrust_max <= 0.0f) {
870 494 : _throttle_thrust_max = 0.0f;
871 : }
872 : // respect runtime current-limit ceiling
873 84779 : if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
874 0 : _throttle_thrust_max = get_current_limit_max_throttle();
875 : // transition to GROUND_IDLE when the ceiling is effectively zero
876 84779 : } else if (is_zero(_throttle_thrust_max)) {
877 494 : _spool_state = SpoolState::GROUND_IDLE;
878 : }
879 :
880 : // fade any thrust boost to zero during down-ramp
881 84779 : _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - spool_step);
882 84779 : break;
883 : }
884 36102663 : }
885 :
886 : // passes throttle directly to all motors for ESC calibration.
887 : // throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_output_min() and 1 will send get_pwm_output_max()
888 2263 : void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float throttle_input)
889 : {
890 2263 : if (armed()) {
891 2263 : uint16_t pwm_out = get_pwm_output_min() + constrain_float(throttle_input, 0.0f, 1.0f) * (get_pwm_output_max() - get_pwm_output_min());
892 : // send the pilot's input directly to each enabled motor
893 74679 : for (uint16_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
894 72416 : if (motor_enabled[i]) {
895 9052 : rc_write(i, pwm_out);
896 : }
897 : }
898 : // send pwm output to channels used by bicopter
899 2263 : SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, pwm_out);
900 2263 : SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, pwm_out);
901 : }
902 2263 : }
903 :
904 : // output a thrust to all motors that match a given motor mask. This
905 : // is used to control tiltrotor motors in forward flight. Thrust is in
906 : // the range 0 to 1
907 343285 : void AP_MotorsMulticopter::output_motor_mask(float thrust, uint32_t mask, float rudder_dt)
908 : {
909 343285 : const int16_t pwm_min = get_pwm_output_min();
910 343285 : const int16_t pwm_range = get_pwm_output_max() - pwm_min;
911 :
912 343285 : _motor_mask_override = mask;
913 :
914 11328405 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
915 10985120 : if (motor_enabled[i] && (mask & (1U << i)) != 0) {
916 : // only apply thrust and rudder mixing when armed and interlocked; otherwise zero throttle
917 763192 : if (armed() && get_interlock()) {
918 : /*
919 : apply rudder mixing differential thrust
920 : copter frame roll is plane frame yaw as this only
921 : apples to either tilted motors or tailsitters
922 : */
923 707772 : float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
924 707772 : set_actuator_with_slew(_actuator[i], thrust + diff_thrust);
925 : } else {
926 : // zero throttle
927 55420 : _actuator[i] = 0.0;
928 : }
929 763192 : int16_t pwm_output = pwm_min + pwm_range * _actuator[i];
930 763192 : rc_write(i, pwm_output);
931 : }
932 : }
933 343285 : }
934 :
935 : // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
936 : // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
937 32951 : uint32_t AP_MotorsMulticopter::get_motor_mask()
938 : {
939 32951 : return SRV_Channels::get_output_channel_mask(SRV_Channel::k_boost_throttle);
940 : }
941 :
942 : // save parameters as part of disarming
943 770 : void AP_MotorsMulticopter::save_params_on_disarm()
944 : {
945 : // save hover throttle
946 770 : if (_throttle_hover_learn == HOVER_LEARN_AND_SAVE) {
947 767 : _throttle_hover.save();
948 : }
949 770 : }
950 :
951 : // convert to PWM min and max in the motor lib
952 1001 : void AP_MotorsMulticopter::convert_pwm_min_max_param(int16_t radio_min, int16_t radio_max)
953 : {
954 1001 : if (_pwm_min.configured() || _pwm_max.configured()) {
955 898 : return;
956 : }
957 103 : _pwm_min.set_and_save(radio_min);
958 103 : _pwm_max.set_and_save(radio_max);
959 : }
960 :
961 100514 : bool AP_MotorsMulticopter::arming_checks(size_t buflen, char *buffer) const
962 : {
963 : // run base class checks
964 100514 : if (!AP_Motors::arming_checks(buflen, buffer)) {
965 56 : return false;
966 : }
967 :
968 : // Check output function is setup for each motor
969 3315114 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
970 3214656 : if (!motor_enabled[i]) {
971 2807459 : continue;
972 : }
973 : uint8_t chan;
974 407197 : SRV_Channel::Function function = SRV_Channels::get_motor_function(i);
975 407197 : if (!SRV_Channels::find_channel(function, chan)) {
976 0 : hal.util->snprintf(buffer, buflen, "no SERVOx_FUNCTION set to Motor%u", i + 1);
977 0 : return false;
978 : }
979 : }
980 :
981 : // Check param config
982 100458 : if (thr_lin.get_spin_min() > 0.3) {
983 2 : hal.util->snprintf(buffer, buflen, "%sSPIN_MIN too high %.2f > 0.3", AP_MOTORS_PARAM_PREFIX, thr_lin.get_spin_min());
984 2 : return false;
985 : }
986 100456 : if (_spin_arm > thr_lin.get_spin_min()) {
987 1 : hal.util->snprintf(buffer, buflen, "%sSPIN_ARM > %sSPIN_MIN", AP_MOTORS_PARAM_PREFIX, AP_MOTORS_PARAM_PREFIX);
988 1 : return false;
989 : }
990 100455 : if (!check_mot_pwm_params()) {
991 2 : hal.util->snprintf(buffer, buflen, "Check %sPWM_MIN and %sPWM_MAX", AP_MOTORS_PARAM_PREFIX, AP_MOTORS_PARAM_PREFIX);
992 2 : return false;
993 : }
994 :
995 100453 : return true;
996 : }
997 :
998 : // return raw throttle out fraction for motor motor_num, returns true if value is valid, false otherwise
999 0 : bool AP_MotorsMulticopter::get_raw_motor_throttle(uint8_t motor_num, float& thr_out) const
1000 : {
1001 0 : if (motor_num >= AP_MOTORS_MAX_NUM_MOTORS || !motor_enabled[motor_num]) {
1002 0 : return false;
1003 : }
1004 :
1005 0 : thr_out = constrain_float(_actuator[motor_num], 0.0f, 1.0f);
1006 0 : return true;
1007 : }
1008 :
1009 : #if APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
1010 : // Getters for AP_Motors example, not used by vehicles
1011 : float AP_MotorsMulticopter::get_throttle_avg_max() const
1012 : {
1013 : return _throttle_avg_max;
1014 : }
1015 :
1016 : int16_t AP_MotorsMulticopter::get_yaw_headroom() const
1017 : {
1018 : return _yaw_headroom;
1019 : }
1020 : #endif
|
