Copter: move check for high-throttle on mode change up

ArduCopter/mode.cpp

@@ -200,6 +200,22 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
     }
 #endif
 
+#if FRAME_CONFIG != HELI_FRAME
+    // ensure vehicle doesn't leap off the ground if a user switches
+    // into a manual throttle mode from a non-manual-throttle mode
+    // (e.g. user arms in guided, raises throttle to 1300 (not enough to
+    // trigger auto takeoff), then switches into manual):
+    if (!ignore_checks &&
+        ap.land_complete &&
+        new_flightmode->has_manual_throttle() &&
+        !copter.flightmode->has_manual_throttle() &&
+        new_flightmode->get_pilot_desired_throttle() > copter.get_non_takeoff_throttle()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: throttle too high");
+        Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
+        return false;
+    }
+#endif
+
     if (!ignore_checks &&
         new_flightmode->requires_GPS() &&
         !copter.position_ok()) {

ArduCopter/mode.h

@@ -79,7 +79,9 @@ public:
 
 protected:
 
-    virtual bool init(bool ignore_checks) = 0;
+    virtual bool init(bool ignore_checks) {
+        return true;
+    }
     virtual void run() = 0;
 
     virtual bool is_autopilot() const { return false; }
@@ -210,7 +212,6 @@ public:
     // inherit constructor
     using Copter::Mode::Mode;
 
-    virtual bool init(bool ignore_checks) override;
     virtual void run() override;
 
     bool is_autopilot() const override { return false; }
@@ -1040,7 +1041,6 @@ public:
     // inherit constructor
     using Copter::Mode::Mode;
 
-    virtual bool init(bool ignore_checks) override;
     virtual void run() override;
 
     bool requires_GPS() const override { return false; }

ArduCopter/mode_acro.cpp

@@ -8,17 +8,6 @@
  * Init and run calls for acro flight mode
  */
 
-bool Copter::ModeAcro::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 && !copter.flightmode->has_manual_throttle() &&
-           (get_pilot_desired_throttle() > copter.get_non_takeoff_throttle())) {
-       return false;
-   }
-
-   return true;
-}
-
 void Copter::ModeAcro::run()
 {
     float target_roll, target_pitch, target_yaw;

ArduCopter/mode_stabilize.cpp

@@ -4,18 +4,6 @@
  * Init and run calls for stabilize flight mode
  */
 
-// stabilize_init - initialise stabilize controller
-bool Copter::ModeStabilize::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 && !copter.flightmode->has_manual_throttle() &&
-            (get_pilot_desired_throttle() > get_non_takeoff_throttle())) {
-        return false;
-    }
-
-    return true;
-}
-
 // stabilize_run - runs the main stabilize controller
 // should be called at 100hz or more
 void Copter::ModeStabilize::run()