Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: reuse spool_step for code refactoring

Browse Source
This commit is contained in:
chobitsfan 2022-07-09 13:15:43 +08:00 committed by Randy Mackay
parent d5f2a95417
commit a723abf986
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -556,6 +556,7 @@ void AP_MotorsMulticopter::output_logic()
_spool_up_time.set(0.05); _spool_up_time.set(0.05);
} }
const float spool_step = 1.0f / (_spool_up_time * _loop_rate);
switch (_spool_state) { switch (_spool_state) {
case SpoolState::SHUT_DOWN: case SpoolState::SHUT_DOWN:
// Motors should be stationary. // Motors should be stationary.
@ -595,7 +596,6 @@ void AP_MotorsMulticopter::output_logic()
limit.throttle_upper = true; limit.throttle_upper = true;
// set and increment ramp variables // set and increment ramp variables
float spool_step = 1.0f / (_spool_up_time * _loop_rate);
switch (_spool_desired) { switch (_spool_desired) {
case DesiredSpoolState::SHUT_DOWN: case DesiredSpoolState::SHUT_DOWN:
_spin_up_ratio -= spool_step; _spin_up_ratio -= spool_step;
@ -649,7 +649,7 @@ void AP_MotorsMulticopter::output_logic()
// set and increment ramp variables // set and increment ramp variables
_spin_up_ratio = 1.0f; _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 // constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) { 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 // initialise motor failure variables
_thrust_boost = false; _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; break;
case SpoolState::THROTTLE_UNLIMITED: case SpoolState::THROTTLE_UNLIMITED:
@ -686,9 +686,9 @@ void AP_MotorsMulticopter::output_logic()
_throttle_thrust_max = get_current_limit_max_throttle(); _throttle_thrust_max = get_current_limit_max_throttle();
if (_thrust_boost && !_thrust_balanced) { 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 { } 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; break;
@ -711,7 +711,7 @@ void AP_MotorsMulticopter::output_logic()
// set and increment ramp variables // set and increment ramp variables
_spin_up_ratio = 1.0f; _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 // constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f) { if (_throttle_thrust_max <= 0.0f) {
@ -723,7 +723,7 @@ void AP_MotorsMulticopter::output_logic()
_spool_state = SpoolState::GROUND_IDLE; _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; break;
} }
} }