From 3907466f9dc89b88480cb7af00b453e5eb95d2eb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 1 Feb 2019 10:17:00 +1100
Subject: [PATCH] Plane: added TKOFF_TIMEOUT parameter

this allows for a timeout on auto-takeoff. This can be combined with
other takeoff parameters to allow the user to abort a hand-launch
after the motor has started without GCS interaction.

The 4m/s threshold is a bit arbitrary. We could make that a parameter
in the future if it is needed
---
 ArduPlane/Parameters.cpp     |  9 +++++++++
 ArduPlane/Parameters.h       |  1 +
 ArduPlane/Plane.h            |  1 +
 ArduPlane/commands_logic.cpp | 19 +++++++++++++++++++
 ArduPlane/takeoff.cpp        | 11 ++++++++---
 5 files changed, 38 insertions(+), 3 deletions(-)

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index 987f099e0e..3ddf025b54 100644
--- a/ArduPlane/Parameters.cpp
+++ b/ArduPlane/Parameters.cpp
@@ -1210,6 +1210,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Increment: 1
     // @User: Advanced
     AP_GROUPINFO("DSPOILER_CROW_W2", 18, ParametersG2, crow_flap_weight2, 0),
+
+    // @Param: TKOFF_TIMEOUT
+    // @DisplayName: Takeoff timeout
+    // @Description: This is the timeout for an automatic takeoff. If this is non-zero and the aircraft does not reach a ground speed of at least 4 m/s within this number of seconds then the takeoff is aborted and the vehicle disarmed. If the value is zero then no timeout applies.
+    // @Range: 0 120
+    // @Increment: 1
+    // @Units: s
+    // @User: User
+    AP_GROUPINFO("TKOFF_TIMEOUT", 19, ParametersG2, takeoff_timeout, 0),
     
     AP_GROUPEND
 };
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index b9cafad1a2..64b68431f5 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -551,6 +551,7 @@ public:
 #endif // ENABLE_SCRIPTING
 
     AP_Int8 takeoff_throttle_accel_count;
+    AP_Int8 takeoff_timeout;
 
 #if LANDING_GEAR_ENABLED == ENABLED
     AP_LandingGear landing_gear;
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 1b3dbfc8ad..9f284c64df 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -439,6 +439,7 @@ private:
         bool launchTimerStarted;
         uint8_t accel_event_counter;
         uint32_t accel_event_ms;
+        uint32_t start_time_ms;
     } takeoff_state;
     
     // ground steering controller state
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index bc4a309408..c80fdedb11 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/ArduPlane/commands_logic.cpp
@@ -546,6 +546,25 @@ bool Plane::verify_takeoff()
         nav_controller->update_level_flight();        
     }
 
+    // check for optional takeoff timeout
+    if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) {
+        const float ground_speed = gps.ground_speed();
+        const float takeoff_min_ground_speed = 4;
+        if (!hal.util->get_soft_armed()) {
+            return false;
+        }
+        if (ground_speed >= takeoff_min_ground_speed) {
+            takeoff_state.start_time_ms = 0;
+        } else {
+            uint32_t now = AP_HAL::millis();
+            if (now - takeoff_state.start_time_ms > (uint32_t)(1000U * g2.takeoff_timeout)) {
+                gcs().send_text(MAV_SEVERITY_INFO, "Takeoff timeout at %.1f m/s", ground_speed);
+                plane.disarm_motors();
+                mission.reset();
+            }
+        }
+    }
+
     // see if we have reached takeoff altitude
     int32_t relative_alt_cm = adjusted_relative_altitude_cm();
     if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) {
diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp
index af90fa4741..356e624395 100644
--- a/ArduPlane/takeoff.cpp
+++ b/ArduPlane/takeoff.cpp
@@ -15,11 +15,15 @@ bool Plane::auto_takeoff_check(void)
     uint32_t now = millis();
     uint16_t wait_time_ms = MIN(uint16_t(g.takeoff_throttle_delay)*100,12700);
 
+    // reset all takeoff state if disarmed
+    if (!hal.util->get_soft_armed()) {
+        memset(&takeoff_state, 0, sizeof(takeoff_state));
+        return false;
+    }
+
     // Reset states if process has been interrupted
     if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) {
-	    takeoff_state.launchTimerStarted = false;
-	    takeoff_state.last_tkoff_arm_time = 0;
-        takeoff_state.last_check_ms = now;
+        memset(&takeoff_state, 0, sizeof(takeoff_state));
         return false;
     }
 
@@ -92,6 +96,7 @@ bool Plane::auto_takeoff_check(void)
         gcs().send_text(MAV_SEVERITY_INFO, "Triggered AUTO. GPS speed = %.1f", (double)gps.ground_speed());
         takeoff_state.launchTimerStarted = false;
         takeoff_state.last_tkoff_arm_time = 0;
+        takeoff_state.start_time_ms = now;
         steer_state.locked_course_err = 0; // use current heading without any error offset
         return true;
     }