diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 7dd6d57c30..0ad251519f 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -25,6 +25,12 @@ bool Copter::auto_init(bool ignore_checks) if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; + // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce change of flips) + if (motors.armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) { + gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd"); + return false; + } + // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check if (auto_yaw_mode == AUTO_YAW_ROI) { @@ -37,10 +43,6 @@ bool Copter::auto_init(bool ignore_checks) // clear guided limits guided_limit_clear(); - // Deny switching to auto mode if land is completed and motors are armed but the next command in the mission is lateral flying - if (motors.armed() && ap.land_complete && !mission.check_takeoff_cmd()) { - return false; - } // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); return true;