diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 850291f99e..48590837a9 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -200,6 +200,14 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) } #endif + if (!ignore_checks && + new_flightmode->requires_GPS() && + !copter.position_ok()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name()); + Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode); + return false; + } + if (!new_flightmode->init(ignore_checks)) { gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed"); Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index dab658a863..af727cb85d 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -22,7 +22,7 @@ // auto_init - initialise auto controller bool Copter::ModeAuto::init(bool ignore_checks) { - if ((copter.position_ok() && mission.num_commands() > 1) || ignore_checks) { + if (mission.num_commands() > 1 || ignore_checks) { _mode = Auto_Loiter; // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips) diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 08a925b900..ab0862bd27 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -9,8 +9,6 @@ // brake_init - initialise brake controller bool Copter::ModeBrake::init(bool ignore_checks) { - if (copter.position_ok() || ignore_checks) { - // set target to current position wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE); @@ -27,9 +25,6 @@ bool Copter::ModeBrake::init(bool ignore_checks) _timeout_ms = 0; return true; - }else{ - return false; - } } // brake_run - runs the brake controller diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index e046683559..b35235419f 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -9,7 +9,6 @@ // circle_init - initialise circle controller flight mode bool Copter::ModeCircle::init(bool ignore_checks) { - if (copter.position_ok() || ignore_checks) { pilot_yaw_override = false; // initialize speeds and accelerations @@ -22,9 +21,6 @@ bool Copter::ModeCircle::init(bool ignore_checks) copter.circle_nav->init(); return true; - }else{ - return false; - } } // circle_run - runs the circle flight mode diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 2f211d07dc..1d8f203df1 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -31,11 +31,7 @@ // drift_init - initialise drift controller bool Copter::ModeDrift::init(bool ignore_checks) { - if (copter.position_ok() || ignore_checks) { - return true; - }else{ - return false; - } + return true; } // drift_run - runs the drift controller diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 1476b9656e..6eaa3817b9 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -40,13 +40,9 @@ struct Guided_Limit { // guided_init - initialise guided controller bool Copter::ModeGuided::init(bool ignore_checks) { - if (copter.position_ok() || ignore_checks) { // start in position control mode pos_control_start(); return true; - }else{ - return false; - } } diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index f110e91375..e68f21bc2e 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -9,7 +9,6 @@ // loiter_init - initialise loiter controller bool Copter::ModeLoiter::init(bool ignore_checks) { - if (copter.position_ok() || ignore_checks) { if (!copter.failsafe.radio) { float target_roll, target_pitch; // apply SIMPLE mode transform to pilot inputs @@ -33,9 +32,6 @@ bool Copter::ModeLoiter::init(bool ignore_checks) } return true; - } else { - return false; - } } #if PRECISION_LANDING == ENABLED diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index a732084d90..ee24324347 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -74,11 +74,6 @@ static struct { // poshold_init - initialise PosHold controller bool Copter::ModePosHold::init(bool ignore_checks) { - // fail to initialise PosHold mode if no GPS lock - if (!copter.position_ok() && !ignore_checks) { - return false; - } - // initialize vertical speeds and acceleration pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_max_accel_z(g.pilot_accel_z); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 520025427d..51b4a788a8 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -12,15 +12,11 @@ // rtl_init - initialise rtl controller bool Copter::ModeRTL::init(bool ignore_checks) { - if (copter.position_ok() || ignore_checks) { // initialise waypoint and spline controller wp_nav->wp_and_spline_init(); build_path(!copter.failsafe.terrain); climb_start(); return true; - }else{ - return false; - } } // re-start RTL with terrain following disabled diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index 9d111ce676..c49e3f9a0b 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -11,7 +11,7 @@ bool Copter::ModeSmartRTL::init(bool ignore_checks) { - if ((copter.position_ok() || ignore_checks) && g2.smart_rtl.is_active()) { + if (g2.smart_rtl.is_active()) { // initialise waypoint and spline controller wp_nav->wp_and_spline_init(); diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 40b4292efa..c9c7d9c7b9 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -11,10 +11,6 @@ // initialise zigzag controller bool Copter::ModeZigZag::init(bool ignore_checks) { - if (!copter.position_ok() && !ignore_checks) { - return false; - } - // initialize's loiter position and velocity on xy-axes from current pos and velocity loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target();