APM:Libraries
AP_MotorsMulticopter.cpp
Go to the documentation of this file.
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 /*
17  * AP_MotorsMulticopter.cpp - ArduCopter multicopter motors library
18  * Code by Randy Mackay and Robert Lefebvre. DIYDrones.com
19  *
20  */
21 
22 #include "AP_MotorsMulticopter.h"
23 #include <AP_HAL/AP_HAL.h>
25 
26 extern const AP_HAL::HAL& hal;
27 
28 // parameters for the motor class
30  // 0 was used by TB_RATIO
31  // 1,2,3 were used by throttle curve
32  // 5 was SPIN_ARMED
33 
34  // @Param: YAW_HEADROOM
35  // @DisplayName: Matrix Yaw Min
36  // @Description: Yaw control is given at least this pwm in microseconds range
37  // @Range: 0 500
38  // @Units: PWM
39  // @User: Advanced
40  AP_GROUPINFO("YAW_HEADROOM", 6, AP_MotorsMulticopter, _yaw_headroom, AP_MOTORS_YAW_HEADROOM_DEFAULT),
41 
42  // 7 was THR_LOW_CMP
43 
44  // @Param: THST_EXPO
45  // @DisplayName: Thrust Curve Expo
46  // @Description: Motor thrust curve exponent (from 0 for linear to 1.0 for second order curve)
47  // @Range: 0.25 0.8
48  // @User: Advanced
49  AP_GROUPINFO("THST_EXPO", 8, AP_MotorsMulticopter, _thrust_curve_expo, AP_MOTORS_THST_EXPO_DEFAULT),
50 
51  // @Param: SPIN_MAX
52  // @DisplayName: Motor Spin maximum
53  // @Description: Point at which the thrust saturates expressed as a number from 0 to 1 in the entire output range
54  // @Values: 0.9:Low, 0.95:Default, 1.0:High
55  // @User: Advanced
57 
58  // @Param: BAT_VOLT_MAX
59  // @DisplayName: Battery voltage compensation maximum voltage
60  // @Description: Battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.4 * cell count, 0 = Disabled
61  // @Range: 6 35
62  // @Units: V
63  // @User: Advanced
64  AP_GROUPINFO("BAT_VOLT_MAX", 10, AP_MotorsMulticopter, _batt_voltage_max, AP_MOTORS_BAT_VOLT_MAX_DEFAULT),
65 
66  // @Param: BAT_VOLT_MIN
67  // @DisplayName: Battery voltage compensation minimum voltage
68  // @Description: Battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.5 * cell count, 0 = Disabled
69  // @Range: 6 35
70  // @Units: V
71  // @User: Advanced
72  AP_GROUPINFO("BAT_VOLT_MIN", 11, AP_MotorsMulticopter, _batt_voltage_min, AP_MOTORS_BAT_VOLT_MIN_DEFAULT),
73 
74  // @Param: BAT_CURR_MAX
75  // @DisplayName: Motor Current Max
76  // @Description: Maximum current over which maximum throttle is limited (0 = Disabled)
77  // @Range: 0 200
78  // @Units: A
79  // @User: Advanced
80  AP_GROUPINFO("BAT_CURR_MAX", 12, AP_MotorsMulticopter, _batt_current_max, AP_MOTORS_BAT_CURR_MAX_DEFAULT),
81 
82  // 13, 14 were used by THR_MIX_MIN, THR_MIX_MAX
83 
84  // @Param: PWM_TYPE
85  // @DisplayName: Output PWM type
86  // @Description: This selects the output PWM type, allowing for normal PWM continuous output, OneShot, brushed or DShot motor output
87  // @Values: 0:Normal,1:OneShot,2:OneShot125,3:Brushed,4:DShot150,5:DShot300,6:DShot600,7:DShot1200
88  // @User: Advanced
89  // @RebootRequired: True
90  AP_GROUPINFO("PWM_TYPE", 15, AP_MotorsMulticopter, _pwm_type, PWM_TYPE_NORMAL),
91 
92  // @Param: PWM_MIN
93  // @DisplayName: PWM output miniumum
94  // @Description: This sets the min PWM output value in microseconds that will ever be output to the motors, 0 = use input RC3_MIN
95  // @Units: PWM
96  // @Range: 0 2000
97  // @User: Advanced
98  AP_GROUPINFO("PWM_MIN", 16, AP_MotorsMulticopter, _pwm_min, 0),
99 
100  // @Param: PWM_MAX
101  // @DisplayName: PWM output maximum
102  // @Description: This sets the max PWM value in microseconds that will ever be output to the motors, 0 = use input RC3_MAX
103  // @Units: PWM
104  // @Range: 0 2000
105  // @User: Advanced
106  AP_GROUPINFO("PWM_MAX", 17, AP_MotorsMulticopter, _pwm_max, 0),
107 
108  // @Param: SPIN_MIN
109  // @DisplayName: Motor Spin minimum
110  // @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.
111  // @Values: 0.0:Low, 0.15:Default, 0.3:High
112  // @User: Advanced
113  AP_GROUPINFO("SPIN_MIN", 18, AP_MotorsMulticopter, _spin_min, AP_MOTORS_SPIN_MIN_DEFAULT),
114 
115  // @Param: SPIN_ARM
116  // @DisplayName: Motor Spin armed
117  // @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.
118  // @Values: 0.0:Low, 0.1:Default, 0.2:High
119  // @User: Advanced
120  AP_GROUPINFO("SPIN_ARM", 19, AP_MotorsMulticopter, _spin_arm, AP_MOTORS_SPIN_ARM_DEFAULT),
121 
122  // @Param: BAT_CURR_TC
123  // @DisplayName: Motor Current Max Time Constant
124  // @Description: Time constant used to limit the maximum current
125  // @Range: 0 10
126  // @Units: s
127  // @User: Advanced
128  AP_GROUPINFO("BAT_CURR_TC", 20, AP_MotorsMulticopter, _batt_current_time_constant, AP_MOTORS_BAT_CURR_TC_DEFAULT),
129 
130  // @Param: THST_HOVER
131  // @DisplayName: Thrust Hover Value
132  // @Description: Motor thrust needed to hover expressed as a number from 0 to 1
133  // @Range: 0.2 0.8
134  // @User: Advanced
135  AP_GROUPINFO("THST_HOVER", 21, AP_MotorsMulticopter, _throttle_hover, AP_MOTORS_THST_HOVER_DEFAULT),
136 
137  // @Param: HOVER_LEARN
138  // @DisplayName: Hover Value Learning
139  // @Description: Enable/Disable automatic learning of hover throttle
140  // @Values: 0:Disabled, 1:Learn, 2:LearnAndSave
141  // @User: Advanced
142  AP_GROUPINFO("HOVER_LEARN", 22, AP_MotorsMulticopter, _throttle_hover_learn, HOVER_LEARN_AND_SAVE),
143 
144  // @Param: SAFE_DISARM
145  // @DisplayName: Motor PWM output disabled when disarmed
146  // @Description: Disables motor PWM output when disarmed
147  // @Values: 0:PWM enabled while disarmed, 1:PWM disabled while disarmed
148  // @User: Advanced
149  AP_GROUPINFO("SAFE_DISARM", 23, AP_MotorsMulticopter, _disarm_disable_pwm, 0),
150 
151  // @Param: YAW_SV_ANGLE
152  // @DisplayName: Yaw Servo Max Lean Angle
153  // @Description: Yaw servo's maximum lean angle
154  // @Range: 5 80
155  // @Units: deg
156  // @Increment: 1
157  // @User: Standard
158  AP_GROUPINFO_FRAME("YAW_SV_ANGLE", 35, AP_MotorsMulticopter, _yaw_servo_angle_max_deg, 30, AP_PARAM_FRAME_TRICOPTER),
159 
160  // @Param: SPOOL_TIME
161  // @DisplayName: Spool up time
162  // @Description: Time in seconds to spool up the motors from zero to min throttle.
163  // @Range: 0 2
164  // @Units: s
165  // @Increment: 0.1
166  // @User: Advanced
167  AP_GROUPINFO("SPOOL_TIME", 36, AP_MotorsMulticopter, _spool_up_time, AP_MOTORS_SPOOL_UP_TIME_DEFAULT),
168 
169  // @Param: BOOST_SCALE
170  // @DisplayName: Motor boost scale
171  // @Description: This is a scaling factor for vehicles with a vertical booster motor used for extra lift. It is used with electric multicopters that have an internal combusion booster motor for longer endurance. The output to the BoostThrottle servo function is set to the current motor thottle 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.
172  // @Range: 0 5
173  // @Increment: 0.1
174  // @User: Advanced
175  AP_GROUPINFO("BOOST_SCALE", 37, AP_MotorsMulticopter, _boost_scale, 0),
176 
177  // 38 RESERVED for BAT_POW_MAX
178 
179  // @Param: BAT_IDX
180  // @DisplayName: Battery compensation index
181  // @Description: Which battery monitor should be used for doing compensation
182  // @Values: 0:First battery, 1:Second battery
183  // @User: Advanced
184  AP_GROUPINFO("BAT_IDX", 39, AP_MotorsMulticopter, _batt_idx, 0),
185 
187 };
188 
189 // Constructor
190 AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) :
191  AP_Motors(loop_rate, speed_hz),
192  _spool_mode(SHUT_DOWN),
193  _spin_up_ratio(0.0f),
194  _lift_max(1.0f),
195  _throttle_limit(1.0f),
196  _throttle_thrust_max(0.0f),
197  _disarm_safety_timer(0)
198 {
200 
201  // disable all motors by default
202  memset(motor_enabled, false, sizeof(motor_enabled));
203 
204  // setup battery voltage filtering
207 
208  // default throttle range
209  _throttle_radio_min = 1100;
210  _throttle_radio_max = 1900;
211 };
212 
213 // output - sends commands to the motors
215 {
216  // update throttle filter
218 
219  // calc filtered battery voltage and lift_max
221 
222  // run spool logic
223  output_logic();
224 
225  // calculate thrust
227 
228  // apply any thrust compensation for the frame
230 
231  // convert rpy_thrust values to pwm
233 
234  // output any booster throttle
236 };
237 
238 // output booster throttle, if any
240 {
241  if (_boost_scale > 0) {
242  float throttle = constrain_float(get_throttle() * _boost_scale, 0, 1);
244  }
245 }
246 
247 
248 // sends minimum values out to the motors
250 {
253  output();
254 }
255 
256 // update the throttle input filter
258 {
259  if (armed()) {
261  // constrain filtered throttle
262  if (_throttle_filter.get() < 0.0f) {
264  }
265  if (_throttle_filter.get() > 1.0f) {
267  }
268  } else {
270  }
271 }
272 
273 // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
275 {
277 
278  if (_batt_current_max <= 0 || // return maximum if current limiting is disabled
279  !_flags.armed || // remove throttle limit if disarmed
280  !battery.has_current(_batt_idx)) { // no current monitoring is available
281  _throttle_limit = 1.0f;
282  return 1.0f;
283  }
284 
285  float _batt_resistance = battery.get_resistance(_batt_idx);
286 
287  if (is_zero(_batt_resistance)) {
288  _throttle_limit = 1.0f;
289  return 1.0f;
290  }
291 
292  float _batt_current = battery.current_amps(_batt_idx);
293 
294  // calculate the maximum current to prevent voltage sag below _batt_voltage_min
295  float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx)-_batt_voltage_min)/_batt_resistance);
296 
297  float batt_current_ratio = _batt_current/batt_current_max;
298 
299  float loop_interval = 1.0f/_loop_rate;
300  _throttle_limit += (loop_interval/(loop_interval+_batt_current_time_constant))*(1.0f - batt_current_ratio);
301 
302  // throttle limit drops to 20% between hover and full throttle
304 
305  // limit max throttle
307 }
308 
309 // apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
311 {
312  float throttle_ratio = thrust;
313  // apply thrust curve - domain 0.0 to 1.0, range 0.0 to 1.0
314  float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
315  if (fabsf(thrust_curve_expo) < 0.001) {
316  // zero expo means linear, avoid floating point exception for small values
317  return thrust;
318  }
319  if(!is_zero(_batt_voltage_filt.get())) {
320  throttle_ratio = ((thrust_curve_expo-1.0f) + safe_sqrt((1.0f-thrust_curve_expo)*(1.0f-thrust_curve_expo) + 4.0f*thrust_curve_expo*_lift_max*thrust))/(2.0f*thrust_curve_expo*_batt_voltage_filt.get());
321  } else {
322  throttle_ratio = ((thrust_curve_expo-1.0f) + safe_sqrt((1.0f-thrust_curve_expo)*(1.0f-thrust_curve_expo) + 4.0f*thrust_curve_expo*_lift_max*thrust))/(2.0f*thrust_curve_expo);
323  }
324 
325  return constrain_float(throttle_ratio, 0.0f, 1.0f);
326 }
327 
328 // update_lift_max from battery voltage - used for voltage compensation
330 {
331  // sanity check battery_voltage_min is not too small
332  // if disabled or misconfigured exit immediately
333  float _batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(_batt_idx);
334  if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f*_batt_voltage_min)) {
336  _lift_max = 1.0f;
337  return;
338  }
339 
340  _batt_voltage_min = MAX(_batt_voltage_min, _batt_voltage_max * 0.6f);
341 
342  // contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
343  _batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max);
344 
345  // filter at 0.5 Hz
346  float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate/_batt_voltage_max, 1.0f/_loop_rate);
347 
348  // calculate lift max
349  float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
350  _lift_max = batt_voltage_filt*(1-thrust_curve_expo) + thrust_curve_expo*batt_voltage_filt*batt_voltage_filt;
351 }
352 
354 {
355  // avoid divide by zero
356  if (_lift_max <= 0.0f) {
357  return 1.0f;
358  }
359 
360  float ret = 1.0f / _lift_max;
361 
362 #if AP_MOTORS_DENSITY_COMP == 1
363  // air density ratio is increasing in density / decreasing in altitude
364  if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) {
365  ret *= 1.0f / constrain_float(_air_density_ratio,0.5f,1.25f);
366  }
367 #endif
368  return ret;
369 }
370 
371 int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
372 {
373  thrust_in = constrain_float(thrust_in, 0.0f, 1.0f);
375 }
376 
378 {
380 }
381 // get minimum or maximum pwm value that can be output to motors
383 {
384  // return _pwm_min if both PWM_MIN and PWM_MAX parameters are defined and valid
385  if ((_pwm_min > 0) && (_pwm_max > 0) && (_pwm_max > _pwm_min)) {
386  return _pwm_min;
387  }
388  return _throttle_radio_min;
389 }
390 
391 // get maximum pwm value that can be output to motors
393 {
394  // return _pwm_max if both PWM_MIN and PWM_MAX parameters are defined and valid
395  if ((_pwm_min > 0) && (_pwm_max > 0) && (_pwm_max > _pwm_min)) {
396  return _pwm_max;
397  }
398  return _throttle_radio_max;
399 }
400 
401 // set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
402 // also sets throttle channel minimum and maximum pwm
403 void AP_MotorsMulticopter::set_throttle_range(int16_t radio_min, int16_t radio_max)
404 {
405  // sanity check
406  if (radio_max <= radio_min) {
407  return;
408  }
409 
410  _throttle_radio_min = radio_min;
411  _throttle_radio_max = radio_max;
412 
414 }
415 
416 // update the throttle input filter. should be called at 100hz
418 {
420  // we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial.
422  }
423 }
424 
425 // run spool logic
427 {
428  if (_flags.armed) {
429  _disarm_safety_timer = 100;
430  } else if (_disarm_safety_timer != 0) {
432  }
433 
434  // force desired and current spool mode if disarmed or not interlocked
435  if (!_flags.armed || !_flags.interlock) {
438  }
439 
440  if (_spool_up_time < 0.05) {
441  // prevent float exception
442  _spool_up_time.set(0.05);
443  }
444 
445  switch (_spool_mode) {
446  case SHUT_DOWN:
447  // Motors should be stationary.
448  // Servos set to their trim values or in a test condition.
449 
450  // set limits flags
451  limit.roll_pitch = true;
452  limit.yaw = true;
453  limit.throttle_lower = true;
454  limit.throttle_upper = true;
455 
456  // make sure the motors are spooling in the correct direction
459  break;
460  }
461 
462  // set and increment ramp variables
463  _spin_up_ratio = 0.0f;
464  _throttle_thrust_max = 0.0f;
465  break;
466 
467  case SPIN_WHEN_ARMED: {
468  // Motors should be stationary or at spin when armed.
469  // Servos should be moving to correct the current attitude.
470 
471  // set limits flags
472  limit.roll_pitch = true;
473  limit.yaw = true;
474  limit.throttle_lower = true;
475  limit.throttle_upper = true;
476 
477  // set and increment ramp variables
478  float spool_step = 1.0f/(_spool_up_time*_loop_rate);
480  _spin_up_ratio -= spool_step;
481  // constrain ramp value and update mode
482  if (_spin_up_ratio <= 0.0f) {
483  _spin_up_ratio = 0.0f;
485  }
487  _spin_up_ratio += spool_step;
488  // constrain ramp value and update mode
489  if (_spin_up_ratio >= 1.0f) {
490  _spin_up_ratio = 1.0f;
492  }
493  } else { // _spool_desired == SPIN_WHEN_ARMED
494  float spin_up_armed_ratio = 0.0f;
495  if (_spin_min > 0.0f) {
496  spin_up_armed_ratio = _spin_arm / _spin_min;
497  }
498  _spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step);
499  }
500  _throttle_thrust_max = 0.0f;
501  break;
502  }
503  case SPOOL_UP:
504  // Maximum throttle should move from minimum to maximum.
505  // Servos should exhibit normal flight behavior.
506 
507  // initialize limits flags
508  limit.roll_pitch = false;
509  limit.yaw = false;
510  limit.throttle_lower = false;
511  limit.throttle_upper = false;
512 
513  // make sure the motors are spooling in the correct direction
516  break;
517  }
518 
519  // set and increment ramp variables
520  _spin_up_ratio = 1.0f;
522 
523  // constrain ramp value and update mode
527  } else if (_throttle_thrust_max < 0.0f) {
528  _throttle_thrust_max = 0.0f;
529  }
530  break;
531 
532  case THROTTLE_UNLIMITED:
533  // Throttle should exhibit normal flight behavior.
534  // Servos should exhibit normal flight behavior.
535 
536  // initialize limits flags
537  limit.roll_pitch = false;
538  limit.yaw = false;
539  limit.throttle_lower = false;
540  limit.throttle_upper = false;
541 
542  // make sure the motors are spooling in the correct direction
545  break;
546  }
547 
548  // set and increment ramp variables
549  _spin_up_ratio = 1.0f;
551  break;
552 
553  case SPOOL_DOWN:
554  // Maximum throttle should move from maximum to minimum.
555  // Servos should exhibit normal flight behavior.
556 
557  // initialize limits flags
558  limit.roll_pitch = false;
559  limit.yaw = false;
560  limit.throttle_lower = false;
561  limit.throttle_upper = false;
562 
563  // make sure the motors are spooling in the correct direction
566  break;
567  }
568 
569  // set and increment ramp variables
570  _spin_up_ratio = 1.0f;
572 
573  // constrain ramp value and update mode
574  if (_throttle_thrust_max <= 0.0f){
575  _throttle_thrust_max = 0.0f;
576  }
579  } else if (is_zero(_throttle_thrust_max)) {
581  }
582  break;
583  }
584 }
585 
586 // passes throttle directly to all motors for ESC calibration.
587 // 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()
589 {
590  if (armed()) {
591  uint16_t pwm_out = get_pwm_output_min() + constrain_float(throttle_input, 0.0f, 1.0f) * (get_pwm_output_max() - get_pwm_output_min());
592  // send the pilot's input directly to each enabled motor
593  for (uint16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
594  if (motor_enabled[i]) {
595  rc_write(i, pwm_out);
596  }
597  }
598  }
599 }
600 
601 // output a thrust to all motors that match a given motor mask. This
602 // is used to control tiltrotor motors in forward flight. Thrust is in
603 // the range 0 to 1
604 void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask)
605 {
606  for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
607  if (motor_enabled[i]) {
608  int16_t motor_out;
609  if (mask & (1U<<i)) {
610  motor_out = calc_thrust_to_pwm(thrust);
611  } else {
612  motor_out = get_pwm_output_min();
613  }
614  rc_write(i, motor_out);
615  }
616  }
617 }
618 
619 // save parameters as part of disarming
621 {
622  // save hover throttle
624  _throttle_hover.save();
625  }
626 }
void reset(T value)
int16_t get_pwm_output_min() const
static const struct AP_Param::GroupInfo var_info[]
#define AP_MOTORS_BAT_VOLT_MAX_DEFAULT
int16_t get_pwm_output_max() const
float safe_sqrt(const T v)
Definition: AP_Math.cpp:64
virtual void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm)
Definition: RCOutput.h:111
struct AP_Motors::AP_Motors_flags _flags
#define AP_MOTORS_BAT_CURR_MAX_DEFAULT
#define AP_GROUPINFO(name, idx, clazz, element, def)
Definition: AP_Param.h:102
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: AC_PID_test.cpp:14
struct AP_Motors::AP_Motors_limit limit
#define AP_MOTORS_SPIN_MIN_DEFAULT
#define MIN(a, b)
Definition: usb_conf.h:215
virtual void thrust_compensation(void)
virtual void output_to_motors()=0
void set_cutoff_frequency(float cutoff_freq)
static auto MAX(const A &one, const B &two) -> decltype(one > two ? one :two)
Definition: AP_Math.h:202
float apply_thrust_curve_and_volt_scaling(float thrust) const
#define AP_MOTORS_BATT_VOLT_FILT_HZ
#define AP_MOTORS_YAW_HEADROOM_DEFAULT
bool armed() const
virtual void output_armed_stabilizing()=0
static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value)
float voltage_resting_estimate(uint8_t instance) const
uint16_t _loop_rate
virtual float get_throttle_hover() const
bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]
bool is_zero(const T fVal1)
Definition: AP_Math.h:40
#define f(i)
bool has_current(uint8_t instance) const
has_current - returns true if battery monitor instance provides current info
#define AP_MOTORS_SPIN_MAX_DEFAULT
void update_throttle_hover(float dt)
float current_amps(uint8_t instance) const
current_amps - returns the instantaneous current draw in amperes
AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz=AP_MOTORS_SPEED_DEFAULT)
#define AP_MOTORS_SPOOL_UP_TIME_DEFAULT
void set_throttle_range(int16_t radio_min, int16_t radio_max)
virtual void rc_write(uint8_t chan, uint16_t pwm)
AP_BattMonitor & battery()
const T & get() const
#define AP_MOTORS_THST_HOVER_MAX
LowPassFilterFloat _throttle_filter
#define AP_GROUPINFO_FRAME(name, idx, clazz, element, def, frame_flags)
Definition: AP_Param.h:96
float constrain_float(const float amt, const float low, const float high)
Definition: AP_Math.h:142
virtual void update_throttle_filter()
void set_throttle_passthrough_for_esc_calibration(float throttle_input)
virtual void set_desired_spool_state(enum spool_up_down_desired spool)
spool_up_down_mode _spool_mode
#define AP_MOTORS_THST_HOVER_MIN
spool_up_down_desired _spool_desired
AP_HAL::RCOutput * rcout
Definition: HAL.h:113
int16_t calc_spin_up_to_pwm() const
#define AP_MOTORS_THST_HOVER_DEFAULT
float _air_density_ratio
#define AP_MOTORS_BAT_VOLT_MIN_DEFAULT
Motor control class for Multicopters.
virtual float get_current_limit_max_throttle()
int16_t calc_thrust_to_pwm(float thrust_in) const
virtual void output_boost_throttle(void)
float get_throttle() const
#define AP_MOTORS_MAX_NUM_MOTORS
float voltage(uint8_t instance) const
voltage - returns battery voltage in millivolts
float get_resistance() const
LowPassFilterFloat _batt_voltage_filt
#define AP_PARAM_FRAME_TRICOPTER
Definition: AP_Param.h:81
#define AP_MOTORS_THST_EXPO_DEFAULT
#define AP_MOTORS_BAT_CURR_TC_DEFAULT
float _throttle_in
#define AP_GROUPEND
Definition: AP_Param.h:121
#define AP_MOTORS_SPIN_ARM_DEFAULT
#define AP_MOTORS_THST_HOVER_TC
vertical booster throttle
Definition: SRV_Channel.h:116
T apply(T sample, float dt)
virtual void output_motor_mask(float thrust, uint8_t mask)
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
Definition: AP_Param.cpp:1214