Copter: clean up do_user_takeoff

ArduCopter/mode.h

@@ -134,6 +134,10 @@ protected:
     ap_t ≈
     takeoff_state_t &takeoff_state;
 
+    // takeoff support
+    bool takeoff_triggered(float target_climb_rate) const;
+    virtual bool do_user_takeoff_start(float takeoff_alt_cm);
+
     // gnd speed limit required to observe optical flow sensor limits
     float &ekfGndSpdLimit;
 
@@ -783,7 +787,7 @@ public:
     void limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
     bool limit_check();
 
-    bool takeoff_start(float final_alt_above_home);
+    bool do_user_takeoff_start(float final_alt_above_home) override;
 
     GuidedMode mode() const { return guided_mode; }
 

ArduCopter/mode_guided.cpp

@@ -50,8 +50,8 @@ bool Copter::ModeGuided::init(bool ignore_checks)
 }
 
 
-// guided_takeoff_start - initialises waypoint controller to implement take-off
-bool Copter::ModeGuided::takeoff_start(float final_alt_above_home)
+// do_user_takeoff_start - initialises waypoint controller to implement take-off
+bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home)
 {
     guided_mode = Guided_TakeOff;
 

ArduCopter/takeoff.cpp

@@ -5,40 +5,44 @@
 //   A safe takeoff speed is calculated and used to calculate a time_ms
 //   the pos_control target is then slowly increased until time_ms expires
 
+bool Copter::Mode::do_user_takeoff_start(float takeoff_alt_cm)
+{
+    takeoff_timer_start(takeoff_alt_cm);
+    return true;
+}
+
 // initiate user takeoff - called when MAVLink TAKEOFF command is received
 bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
 {
-    if (copter.motors->armed() && ap.land_complete && has_user_takeoff(must_navigate) && takeoff_alt_cm > copter.current_loc.alt) {
+    if (!copter.motors->armed()) {
+        return false;
+    }
+    if (!ap.land_complete) {
+        // can't takeoff again!
+        return false;
+    }
+    if (!has_user_takeoff(must_navigate)) {
+        // this mode doesn't support user takeoff
+        return false;
+    }
+    if (takeoff_alt_cm <= copter.current_loc.alt) {
+        // can't takeoff downwards...
+        return false;
+    }
 
 #if FRAME_CONFIG == HELI_FRAME
-        // Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
-        if (!copter.motors->rotor_runup_complete()) {
-            return false;
-        }
+    // Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
+    if (!copter.motors->rotor_runup_complete()) {
+        return false;
+    }
 #endif
 
-        switch(copter.control_mode) {
-#if MODE_GUIDED_ENABLED == ENABLED
-            case GUIDED:
-                if (copter.mode_guided.takeoff_start(takeoff_alt_cm)) {
-                    copter.set_auto_armed(true);
-                    return true;
-                }
-                return false;
-#endif
-            case LOITER:
-            case POSHOLD:
-            case ALT_HOLD:
-            case SPORT:
-            case FLOWHOLD:
-                copter.set_auto_armed(true);
-                takeoff_timer_start(takeoff_alt_cm);
-                return true;
-            default:
-                return false;
-        }
+    if (!do_user_takeoff_start(takeoff_alt_cm)) {
+        return false;
     }
-    return false;
+
+    copter.set_auto_armed(true);
+    return true;
 }
 
 // start takeoff to specified altitude above home in centimeters