diff --git a/ArduCopter/control_acro.cpp b/ArduCopter/control_acro.cpp index db4042d83b..52a565bdc7 100644 --- a/ArduCopter/control_acro.cpp +++ b/ArduCopter/control_acro.cpp @@ -9,8 +9,11 @@ // acro_init - initialise acro controller bool Copter::acro_init(bool ignore_checks) { - // always successfully enter acro - return true; + // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high + if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) { + return false; + } + return true; } // acro_run - runs the acro controller diff --git a/ArduCopter/control_stabilize.cpp b/ArduCopter/control_stabilize.cpp index a78e057cd0..20232b44d5 100644 --- a/ArduCopter/control_stabilize.cpp +++ b/ArduCopter/control_stabilize.cpp @@ -9,11 +9,13 @@ // stabilize_init - initialise stabilize controller bool Copter::stabilize_init(bool ignore_checks) { + // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high + if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) { + return false; + } // set target altitude to zero for reporting - // To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error? pos_control.set_alt_target(0); - // stabilize should never be made to fail return true; }