mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-02-06 05:54:05 +08:00
Copter: Heli: Handle GROUND_IDLE in Stab and Acro
This commit is contained in:
committed by
Randy Mackay
parent
0bd8848f3e
commit
fe93a08b03
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user