aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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);
}
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;
}
}