From 6807b961e2a448e304f9b50df67d1a46af0c464f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 25 Mar 2016 18:04:52 +0900 Subject: [PATCH] AP_MotorsMulticopter: protect against div-by-zero if MOT_SPIN_ARMED is zero --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 31 ++++++++++++-------- libraries/AP_Motors/AP_MotorsMulticopter.h | 3 -- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index c69c15c4ac..34b18e6814 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -360,25 +360,30 @@ void AP_MotorsMulticopter::output_logic() // set and increment ramp variables float spool_step = 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate); - if(_spool_desired == DESIRED_SHUT_DOWN){ + if (_spool_desired == DESIRED_SHUT_DOWN){ _throttle_low_end_pct -= spool_step; - } else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED ){ + // constrain ramp value and update mode + if (_throttle_low_end_pct <= 0.0f) { + _throttle_low_end_pct = 0.0f; + _multicopter_flags.spool_mode = SHUT_DOWN; + } + } else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) { _throttle_low_end_pct += spool_step; - }else{ - _throttle_low_end_pct += constrain_float(spin_when_armed_low_end_pct()-_throttle_low_end_pct, -spool_step, spool_step); + // constrain ramp value and update mode + if (_throttle_low_end_pct >= 1.0f) { + _throttle_low_end_pct = 1.0f; + _multicopter_flags.spool_mode = SPOOL_UP; + } + } else { // _spool_desired == SPIN_WHEN_ARMED + float spin_when_armed_low_end_pct = 0.0f; + if (_min_throttle > 0) { + spin_when_armed_low_end_pct = (float)_spin_when_armed / _min_throttle; + } + _throttle_low_end_pct += constrain_float(spin_when_armed_low_end_pct-_throttle_low_end_pct, -spool_step, spool_step); } _throttle_thrust_max = 0.0f; _throttle_rpy_mix = 0.0f; _throttle_rpy_mix_desired = 0.0f; - - // constrain ramp value and update mode - if (_throttle_low_end_pct <= 0.0f) { - _throttle_low_end_pct = 0.0f; - _multicopter_flags.spool_mode = SHUT_DOWN; - } else if (_throttle_low_end_pct >= 1.0f) { - _throttle_low_end_pct = 1.0f; - _multicopter_flags.spool_mode = SPOOL_UP; - } break; } case SPOOL_UP: diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 991025e0b4..fd74a8850c 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -132,9 +132,6 @@ protected: // convert thrust (0~1) range back to pwm range int16_t calc_thrust_to_pwm(float thrust_in) const; - // spin when armed as a percentage of the 0~1 range from 0 to throttle_min - float spin_when_armed_low_end_pct() { return (float)_spin_when_armed.get() / _min_throttle; } - // flag bitmask struct { spool_up_down_mode spool_mode : 3; // motor's current spool mode