From 597e8769534aa38f37a7239fb1c47cb8791b60a1 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 12 Dec 2017 21:09:48 +1100
Subject: [PATCH] Copter: clean up do_user_takeoff

---
 ArduCopter/mode.h          |  6 +++-
 ArduCopter/mode_guided.cpp |  4 +--
 ArduCopter/takeoff.cpp     | 56 ++++++++++++++++++++------------------
 3 files changed, 37 insertions(+), 29 deletions(-)

diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 4315645d93..64d9986a7d 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -134,6 +134,10 @@ protected:
     ap_t &ap;
     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; }
 
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index 16933e6244..0eefda7d28 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/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;
 
diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp
index 88d6b88213..64ee1e8e6d 100644
--- a/ArduCopter/takeoff.cpp
+++ b/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