diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index e22c2150c1..0bd3d94533 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -8,13 +8,6 @@ // althold_init - initialise althold controller bool Copter::althold_init(bool ignore_checks) { -#if FRAME_CONFIG == HELI_FRAME - // do not allow helis to enter Alt Hold if the Rotor Runup is not complete - if (!ignore_checks && !motors->rotor_runup_complete()){ - return false; - } -#endif - // initialize vertical speeds and leash lengths pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control->set_accel_z(g.pilot_accel_z); diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index e4f4225223..f8fc21f47c 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -7,13 +7,6 @@ // loiter_init - initialise loiter controller bool Copter::loiter_init(bool ignore_checks) { -#if FRAME_CONFIG == HELI_FRAME - // do not allow helis to enter Loiter if the Rotor Runup is not complete - if (!ignore_checks && !motors->rotor_runup_complete()){ - return false; - } -#endif - if (position_ok() || ignore_checks) { // set target to current position diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index 2ff9b19261..4ef6a43591 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -75,13 +75,6 @@ static struct { // poshold_init - initialise PosHold controller bool Copter::poshold_init(bool ignore_checks) { -#if FRAME_CONFIG == HELI_FRAME - // do not allow helis to enter Pos Hold if the Rotor Runup is not complete - if (!ignore_checks && !motors->rotor_runup_complete()){ - return false; - } -#endif - // fail to initialise PosHold mode if no GPS lock if (!position_ok() && !ignore_checks) { return false; diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 84977d417e..1c420b586b 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -24,6 +24,15 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) return true; } + +#if FRAME_CONFIG == HELI_FRAME + // do not allow helis to enter a non-manual throttle mode if the + // rotor runup is not complete + if (!ignore_checks && !mode_has_manual_throttle(mode) && !motors->rotor_runup_complete()){ + goto failed; + } +#endif + switch (mode) { case ACRO: #if FRAME_CONFIG == HELI_FRAME @@ -114,6 +123,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) break; } +#if FRAME_CONFIG == HELI_FRAME +failed: +#endif + // update flight mode if (success) { // perform any cleanup required by previous flight mode