Plane: Auto Takeoff Check updates

Adds additional launch angle protection and protection against early
exit from auto causing unexpected motor start on re-entry

ArduPlane/Attitude.pde

@@ -434,85 +434,84 @@ static void throttle_slew_limit(int16_t last_throttle)
 }
 
 
-/*
-  check for automatic takeoff conditions being met
+/*   Check for automatic takeoff conditions being met using the following sequence:
+ *   1) Check for adequate GPS lock - if not return false
+ *   2) Check the gravity compensated longitudinal acceleration against the threshold and start the timer if true
+ *   3) Wait until the timer has reached the specified value (increments of 0.1 sec) and then check the GPS speed against the threshold
+ *   4) If the GPS speed is above the threshold and the attitude is within limits then return true and reset the timer
+ *   5) If the GPS speed and attitude within limits has not been achieved after 2.5 seconds, return false and reset the timer
+ *   6) If the time lapsed since the last timecheck is greater than 0.2 seconds, return false and reset the timer
+ *   NOTE : This function relies on the TECS 50Hz processing for its acceleration measure.
  */
 static bool auto_takeoff_check(void)
 {
-#if 1
+    // this is a more advanced check that relies on TECS
+    uint32_t now = hal.scheduler->millis();
+    static bool launchTimerStarted;
+    static uint32_t last_tkoff_arm_time;
+    static uint32_t last_check_ms;
+
+    // Reset states if process has been interrupted
+    if (last_check_ms && (now - last_check_ms) > 200) {
+        gcs_send_text_fmt(PSTR("Timer Interrupted AUTO"));
+	    launchTimerStarted = false;
+	    last_tkoff_arm_time = 0;
+        last_check_ms = now;
+        return false;
+    }
+
+    last_check_ms = now;
+
+    // Check for bad GPS
     if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) {
         // no auto takeoff without GPS lock
         return false;
     }
-    if (g_gps->ground_speed_cm < g.takeoff_throttle_min_speed*100.0f) {
-        // we haven't reached the minimum ground speed
-        return false;
+
+    // Check for launch acceleration or timer started. NOTE: relies on TECS 50Hz processing
+    if (!launchTimerStarted &&
+        g.takeoff_throttle_min_accel != 0.0 &&
+        SpdHgt_Controller->get_VXdot() < g.takeoff_throttle_min_accel) {
+        goto no_launch;
     }
 
-    if (g.takeoff_throttle_min_accel > 0.0f) {
-        float xaccel = ins.get_accel().x;
-        if (ahrs.pitch_sensor > -3000 && 
-            ahrs.pitch_sensor < 4500 &&
-            abs(ahrs.roll_sensor) < 3000 && 
-            xaccel >= g.takeoff_throttle_min_accel) {
-            // trigger with minimum acceleration when flat
-            // Thanks to Chris Miser for this suggestion
-            gcs_send_text_fmt(PSTR("Triggered AUTO xaccel=%.1f"), xaccel);
-            return true;
-        }
-        return false;
+    if (!launchTimerStarted) {
+        launchTimerStarted = true;
+        last_tkoff_arm_time = now;
+        gcs_send_text_fmt(PSTR("Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec"), 
+                          SpdHgt_Controller->get_VXdot(), 0.1f*float(min(g.takeoff_throttle_delay,25)));
     }
 
-    // we're good for takeoff
-    return true;
+    // Only perform velocity check if not timed out
+    if ((now - last_tkoff_arm_time) > 2500) {
+        gcs_send_text_fmt(PSTR("Timeout AUTO"));
+        goto no_launch;
+    }
 
-#else
-    // this is a more advanced check that relies on TECS
-    uint32_t now = hal.scheduler->micros();
-    static bool launchCountStarted;
-    static uint32_t last_tkoff_arm_time;
+    // Check aircraft attitude for bad launch
+    if (ahrs.pitch_sensor <= -3000 ||
+        ahrs.pitch_sensor >= 4500 ||
+        abs(ahrs.roll_sensor) > 3000) {
+        gcs_send_text_fmt(PSTR("Bad Launch AUTO"));
+        goto no_launch;
+    }
 
-    if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) 
-    {
-        // no auto takeoff without GPS lock
-        return false;
+    // Check ground speed and time delay
+    if (((g_gps->ground_speed_cm > g.takeoff_throttle_min_speed*100.0f || g.takeoff_throttle_min_speed == 0.0)) && 
+        ((now - last_tkoff_arm_time) >= min(uint16_t(g.takeoff_throttle_delay)*100,2500))) {
+        gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), g_gps->ground_speed_cm*0.01f);
+        launchTimerStarted = false;
+        last_tkoff_arm_time = 0;
+        return true;
     }
-    if (SpdHgt_Controller->get_VXdot() >= g.takeoff_throttle_min_accel || g.takeoff_throttle_min_accel == 0.0 || launchCountStarted)
-    {
-        if (!launchCountStarted) 
-        {
-		launchCountStarted = true;
-        last_tkoff_arm_time = now;
-        gcs_send_text_fmt(PSTR("Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec"), SpdHgt_Controller->get_VXdot(), 0.1f*float(min(g.takeoff_throttle_delay,15)));
-        }
- 		if ((now - last_tkoff_arm_time) <= 2500000)
-		{
-            
-			if ((g_gps->ground_speed > g.takeoff_throttle_min_speed*100.0f || g.takeoff_throttle_min_speed == 0.0) && ((now -last_tkoff_arm_time) >= min(uint32_t(g.takeoff_throttle_delay*100000),1500000)))
-            {
-                gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), g_gps->ground_speed*100.0f);
-			    launchCountStarted = false;
-		        last_tkoff_arm_time = 0;
-			    return true;
-		    }
-		    else
-			{
- 			    launchCountStarted = true;
- 			    return false;
-            }
-        }
-        else
-        {
-            gcs_send_text_fmt(PSTR("Timeout AUTO"));
-		    launchCountStarted = false;
-		    last_tkoff_arm_time = 0;
-	    	return false;
-        }
-    }         
-    launchCountStarted = false;
-	last_tkoff_arm_time = 0;
+
+    // we're not launching yet, but the timer is still going
+    return false;
+
+no_launch:
+    launchTimerStarted = false;
+    last_tkoff_arm_time = 0;
     return false;
-#endif
 }