Plane: added TKOFF_ACCEL_CNT for multi-shake to start

this allows you to setup shake to start with a lower accel threshold,
but with multiple fwd/back movements needed.

This implements https://github.com/ArduPilot/ardupilot/issues/2221

ArduPlane/Parameters.cpp

@@ -128,7 +128,7 @@ const AP_Param::Info Plane::var_info[] = {
 
     // @Param: TKOFF_THR_MINACC
     // @DisplayName: Takeoff throttle min acceleration
-    // @Description: Minimum forward acceleration in m/s/s before arming the ground speed check in auto-takeoff. This is meant to be used for hand launches. Setting this value to 0 disables the acceleration test which means the ground speed check will always be armed which could allow GPS velocity jumps to start the engine. For hand launches and bungee launches this should be set to around 15.
+    // @Description: Minimum forward acceleration in m/s/s before arming the ground speed check in auto-takeoff. This is meant to be used for hand launches. Setting this value to 0 disables the acceleration test which means the ground speed check will always be armed which could allow GPS velocity jumps to start the engine. For hand launches and bungee launches this should be set to around 15. Also see TKOFF_ACCEL_CNT paramter for control of full "shake to arm".
     // @Units: m/s/s
     // @Range: 0 30
     // @Increment: 0.1
@@ -1182,6 +1182,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(scripting, "SCR_", 14, ParametersG2, AP_Scripting),
 #endif
 
+    // @Param: TKOFF_ACCEL_CNT
+    // @DisplayName: Takeoff throttle acceleration count
+    // @Description: This is the number of acceleration events to require for arming with TKOFF_THR_MINACC. The default is 1, which means a single forward acceleration above TKOFF_THR_MINACC will arm. By setting this higher than 1 you can require more forward/backward movements to arm.
+    // @Range: 1 10
+    // @User: User
+    AP_GROUPINFO("TKOFF_ACCEL_CNT", 15, ParametersG2, takeoff_throttle_accel_count, 1),
+
     AP_GROUPEND
 };
 

ArduPlane/Parameters.h

@@ -549,6 +549,8 @@ public:
 #ifdef ENABLE_SCRIPTING
     AP_Scripting scripting;
 #endif // ENABLE_SCRIPTING
+
+    AP_Int8 takeoff_throttle_accel_count;
 };
 
 extern const AP_Param::Info var_info[];

ArduPlane/Plane.h

@@ -430,6 +430,8 @@ private:
         uint32_t last_check_ms;
         uint32_t last_report_ms;
         bool launchTimerStarted;
+        uint8_t accel_event_counter;
+        uint32_t accel_event_ms;
     } takeoff_state;
     
     // ground steering controller state

ArduPlane/takeoff.cpp

@@ -31,11 +31,28 @@ bool Plane::auto_takeoff_check(void)
         return false;
     }
 
-    // Check for launch acceleration if set. NOTE: relies on TECS 50Hz processing
-    if (!takeoff_state.launchTimerStarted &&
-        !is_zero(g.takeoff_throttle_min_accel) &&
-        SpdHgt_Controller->get_VXdot() < g.takeoff_throttle_min_accel) {
-        goto no_launch;
+    if (!takeoff_state.launchTimerStarted && !is_zero(g.takeoff_throttle_min_accel)) {
+        // we are requiring an X acceleration event to launch
+        float xaccel = SpdHgt_Controller->get_VXdot();
+        if (g2.takeoff_throttle_accel_count <= 1) {
+            if (xaccel < g.takeoff_throttle_min_accel) {
+                goto no_launch;
+            }
+        } else {
+            // we need multiple accel events
+            if (now - takeoff_state.accel_event_ms > 500) {
+                takeoff_state.accel_event_counter = 0;
+            }
+            bool odd_event = ((takeoff_state.accel_event_counter & 1) != 0);
+            bool got_event = (odd_event?xaccel < -g.takeoff_throttle_min_accel : xaccel > g.takeoff_throttle_min_accel);
+            if (got_event) {
+                takeoff_state.accel_event_counter++;
+                takeoff_state.accel_event_ms = now;
+            }
+            if (takeoff_state.accel_event_counter < g2.takeoff_throttle_accel_count) {
+                goto no_launch;
+            }
+        }
     }
 
     // we've reached the acceleration threshold, so start the timer
@@ -64,6 +81,7 @@ bool Plane::auto_takeoff_check(void)
             ahrs.pitch_sensor >= 4500 ||
             (!fly_inverted() && labs(ahrs.roll_sensor) > 3000)) {
             gcs().send_text(MAV_SEVERITY_WARNING, "Bad launch AUTO");
+            takeoff_state.accel_event_counter = 0;
             goto no_launch;
         }
     }