diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index d0b59e0bae..98dd44dcc4 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -556,6 +556,7 @@ void AP_MotorsMulticopter::output_logic() _spool_up_time.set(0.05); } + const float spool_step = 1.0f / (_spool_up_time * _loop_rate); switch (_spool_state) { case SpoolState::SHUT_DOWN: // Motors should be stationary. @@ -595,7 +596,6 @@ void AP_MotorsMulticopter::output_logic() limit.throttle_upper = true; // set and increment ramp variables - float spool_step = 1.0f / (_spool_up_time * _loop_rate); switch (_spool_desired) { case DesiredSpoolState::SHUT_DOWN: _spin_up_ratio -= spool_step; @@ -649,7 +649,7 @@ void AP_MotorsMulticopter::output_logic() // set and increment ramp variables _spin_up_ratio = 1.0f; - _throttle_thrust_max += 1.0f / (_spool_up_time * _loop_rate); + _throttle_thrust_max += spool_step; // constrain ramp value and update mode if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) { @@ -661,7 +661,7 @@ void AP_MotorsMulticopter::output_logic() // initialise motor failure variables _thrust_boost = false; - _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0 / (_spool_up_time * _loop_rate)); + _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - spool_step); break; case SpoolState::THROTTLE_UNLIMITED: @@ -686,9 +686,9 @@ void AP_MotorsMulticopter::output_logic() _throttle_thrust_max = get_current_limit_max_throttle(); if (_thrust_boost && !_thrust_balanced) { - _thrust_boost_ratio = MIN(1.0, _thrust_boost_ratio + 1.0f / (_spool_up_time * _loop_rate)); + _thrust_boost_ratio = MIN(1.0, _thrust_boost_ratio + spool_step); } else { - _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0f / (_spool_up_time * _loop_rate)); + _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - spool_step); } break; @@ -711,7 +711,7 @@ void AP_MotorsMulticopter::output_logic() // set and increment ramp variables _spin_up_ratio = 1.0f; - _throttle_thrust_max -= 1.0f / (_spool_up_time * _loop_rate); + _throttle_thrust_max -= spool_step; // constrain ramp value and update mode if (_throttle_thrust_max <= 0.0f) { @@ -723,7 +723,7 @@ void AP_MotorsMulticopter::output_logic() _spool_state = SpoolState::GROUND_IDLE; } - _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0f / (_spool_up_time * _loop_rate)); + _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - spool_step); break; } }