diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 675e7fd566..5359ad16e7 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -165,7 +165,7 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { // @Param: SPOOL_TIME // @DisplayName: Spool up time // @Description: Time in seconds to spool up the motors from zero to min throttle. - // @Range: 0 2 + // @Range: 0.05 2 // @Units: s // @Increment: 0.1 // @User: Advanced @@ -222,6 +222,15 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { // @User: Advanced AP_GROUPINFO("OPTIONS", 43, AP_MotorsMulticopter, _options, 0), + // @Param: SPOOL_TIM_DN + // @DisplayName: Spool down time + // @Description: Time taken to spool down the motors from min to zero throttle. If set to 0 then SPOOL_TIME is used instead. + // @Range: 0 2 + // @Units: s + // @Increment: 0.001 + // @User: Advanced + AP_GROUPINFO("SPOOL_TIM_DN", 44, AP_MotorsMulticopter, _spool_down_time, 0), + AP_GROUPEND }; @@ -477,6 +486,7 @@ void AP_MotorsMulticopter::update_throttle_hover(float dt) // run spool logic void AP_MotorsMulticopter::output_logic() { + const constexpr float minimum_spool_time = 0.05f; if (armed()) { if (_disarm_disable_pwm && (_disarm_safe_timer < _safe_time)) { _disarm_safe_timer += _dt; @@ -493,12 +503,11 @@ void AP_MotorsMulticopter::output_logic() _spool_state = SpoolState::SHUT_DOWN; } - if (_spool_up_time < 0.05) { + if (_spool_up_time < minimum_spool_time) { // prevent float exception - _spool_up_time.set(0.05); + _spool_up_time.set(minimum_spool_time); } - const float spool_step = _dt / _spool_up_time; switch (_spool_state) { case SpoolState::SHUT_DOWN: // Motors should be stationary. @@ -539,7 +548,9 @@ void AP_MotorsMulticopter::output_logic() // set and increment ramp variables switch (_spool_desired) { - case DesiredSpoolState::SHUT_DOWN: + case DesiredSpoolState::SHUT_DOWN: { + const float spool_time = _spool_down_time > minimum_spool_time ? _spool_down_time : _spool_up_time; + const float spool_step = _dt / spool_time; _spin_up_ratio -= spool_step; // constrain ramp value and update mode if (_spin_up_ratio <= 0.0f) { @@ -547,8 +558,10 @@ void AP_MotorsMulticopter::output_logic() _spool_state = SpoolState::SHUT_DOWN; } break; + } - case DesiredSpoolState::THROTTLE_UNLIMITED: + case DesiredSpoolState::THROTTLE_UNLIMITED: { + const float spool_step = _dt / _spool_up_time; _spin_up_ratio += spool_step; // constrain ramp value and update mode if (_spin_up_ratio >= 1.0f) { @@ -559,15 +572,19 @@ void AP_MotorsMulticopter::output_logic() } } break; - - case DesiredSpoolState::GROUND_IDLE: + } + case DesiredSpoolState::GROUND_IDLE: { + const float spool_up_step = _dt / _spool_up_time; + const float spool_down_time = _spool_down_time > minimum_spool_time ? _spool_down_time : _spool_up_time; + const float spool_down_step = _dt / spool_down_time; float spin_up_armed_ratio = 0.0f; if (thr_lin.get_spin_min() > 0.0f) { spin_up_armed_ratio = _spin_arm / thr_lin.get_spin_min(); } - _spin_up_ratio += constrain_float(spin_up_armed_ratio - _spin_up_ratio, -spool_step, spool_step); + _spin_up_ratio += constrain_float(spin_up_armed_ratio - _spin_up_ratio, -spool_down_step, spool_up_step); break; } + } _throttle_thrust_max = 0.0f; // initialise motor failure variables @@ -575,7 +592,8 @@ void AP_MotorsMulticopter::output_logic() _thrust_boost_ratio = 0.0f; break; } - case SpoolState::SPOOLING_UP: + case SpoolState::SPOOLING_UP: { + const float spool_step = _dt / _spool_up_time; // Maximum throttle should move from minimum to maximum. // Servos should exhibit normal flight behavior. @@ -608,8 +626,10 @@ void AP_MotorsMulticopter::output_logic() _thrust_boost = false; _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - spool_step); break; + } - case SpoolState::THROTTLE_UNLIMITED: + case SpoolState::THROTTLE_UNLIMITED: { + const float spool_step = _dt / _spool_up_time; // Throttle should exhibit normal flight behavior. // Servos should exhibit normal flight behavior. @@ -636,6 +656,7 @@ void AP_MotorsMulticopter::output_logic() _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - spool_step); } break; + } case SpoolState::SPOOLING_DOWN: // Maximum throttle should move from maximum to minimum. @@ -656,6 +677,8 @@ void AP_MotorsMulticopter::output_logic() // set and increment ramp variables _spin_up_ratio = 1.0f; + const float spool_time = _spool_down_time > minimum_spool_time ? _spool_down_time : _spool_up_time; + const float spool_step = _dt / spool_time; _throttle_thrust_max -= spool_step; // constrain ramp value and update mode diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index f2b8b1dc03..f6e07e204a 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -169,6 +169,7 @@ protected: // time to spool motors to min throttle AP_Float _spool_up_time; + AP_Float _spool_down_time; // scaling for booster motor throttle AP_Float _boost_scale;