Copter: move check for position up

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);

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)

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

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

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;
-    }
 }
 
 // drift_run - runs the drift controller

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;
-    }
 }
 
 

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

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);

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

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();
 

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();