Copter: Heli: Handle GROUND_IDLE in Stab and Acro

This commit is contained in:
Leonard Hall
2026-01-31 14:07:54 +10:30
committed by Randy Mackay
parent 0bd8848f3e
commit fe93a08b03
2 changed files with 4 additions and 0 deletions

View File

@@ -37,6 +37,8 @@ void ModeAcro_Heli::run()
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else if (!motors->get_interlock()) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
// heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

View File

@@ -44,6 +44,8 @@ void ModeStabilize_Heli::run()
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else if (!motors->get_interlock()) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
// heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);