From bf5f51a616b66851cced1814fb3771c1b6cd1e14 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 12 Dec 2017 21:53:56 +1100
Subject: [PATCH] Copter: factor out takeoff_trigger from althold, sport and
 loiter

---
 ArduCopter/mode.cpp         | 20 ++++++++++++++++++++
 ArduCopter/mode.h           |  3 +++
 ArduCopter/mode_althold.cpp |  9 +--------
 ArduCopter/mode_loiter.cpp  |  9 +--------
 ArduCopter/mode_sport.cpp   |  9 +--------
 5 files changed, 26 insertions(+), 24 deletions(-)

diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
index f2ac4e566e..13b244ef7f 100644
--- a/ArduCopter/mode.cpp
+++ b/ArduCopter/mode.cpp
@@ -242,3 +242,23 @@ void Copter::Mode::update_navigation()
     // run autopilot to make high level decisions about control modes
     run_autopilot();
 }
+
+
+bool Copter::Mode::takeoff_triggered(const float target_climb_rate) const
+{
+    if (!ap.land_complete) {
+        // can't take off if we're already flying
+        return false;
+    }
+    if (target_climb_rate <= 0.0f) {
+        // can't takeoff unless we want to go up...
+        return false;
+    }
+#if FRAME_CONFIG == HELI_FRAME
+    if (!motors->rotor_runup_complete()) {
+        // hold heli on the ground until rotor speed runup has finished
+        return false;
+    }
+#endif
+    return true;
+}
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index fdc598626c..17a32c8dd9 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -72,6 +72,9 @@ protected:
     ap_t &ap;
     takeoff_state_t &takeoff_state;
 
+    // takeoff support
+    bool takeoff_triggered(float target_climb_rate) const;
+
     // gnd speed limit required to observe optical flow sensor limits
     float &ekfGndSpdLimit;
 
diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp
index 38c9ee4f06..1eac191cc8 100644
--- a/ArduCopter/mode_althold.cpp
+++ b/ArduCopter/mode_althold.cpp
@@ -49,17 +49,10 @@ void Copter::ModeAltHold::run()
     float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
     target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
 
-#if FRAME_CONFIG == HELI_FRAME
-    // helicopters are held on the ground until rotor speed runup has finished
-    bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors->rotor_runup_complete());
-#else
-    bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
-#endif
-
     // Alt Hold State Machine Determination
     if (!motors->armed() || !motors->get_interlock()) {
         althold_state = AltHold_MotorStopped;
-    } else if (takeoff_state.running || takeoff_triggered) {
+    } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
         althold_state = AltHold_Takeoff;
     } else if (!ap.auto_armed || ap.land_complete) {
         althold_state = AltHold_Landed;
diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp
index ce241ce228..425cbf3a3a 100644
--- a/ArduCopter/mode_loiter.cpp
+++ b/ArduCopter/mode_loiter.cpp
@@ -101,17 +101,10 @@ void Copter::ModeLoiter::run()
         wp_nav->loiter_soften_for_landing();
     }
 
-#if FRAME_CONFIG == HELI_FRAME
-    // helicopters are held on the ground until rotor speed runup has finished
-    bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors->rotor_runup_complete());
-#else
-    bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
-#endif
-
     // Loiter State Machine Determination
     if (!motors->armed() || !motors->get_interlock()) {
         loiter_state = Loiter_MotorStopped;
-    } else if (takeoff_state.running || takeoff_triggered) {
+    } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
         loiter_state = Loiter_Takeoff;
     } else if (!ap.auto_armed || ap.land_complete) {
         loiter_state = Loiter_Landed;
diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp
index 82dc45fbf8..bd031bbf5b 100644
--- a/ArduCopter/mode_sport.cpp
+++ b/ArduCopter/mode_sport.cpp
@@ -71,17 +71,10 @@ void Copter::ModeSport::run()
     float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
     target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
 
-#if FRAME_CONFIG == HELI_FRAME
-    // helicopters are held on the ground until rotor speed runup has finished
-    bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors->rotor_runup_complete());
-#else
-    bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
-#endif
-
     // State Machine Determination
     if (!motors->armed() || !motors->get_interlock()) {
         sport_state = Sport_MotorStopped;
-    } else if (takeoff_state.running || takeoff_triggered) {
+    } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
         sport_state = Sport_Takeoff;
     } else if (!ap.auto_armed || ap.land_complete) {
         sport_state = Sport_Landed;