Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Add a seprate spool down time, if it's 0 use spool up time

This commit is contained in:
Michael du Breuil 2022-03-22 16:12:28 -07:00 committed by Randy Mackay
parent 0cc9484b2d
commit 9e32456003
2 changed files with 35 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -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

1
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -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;