diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 61eb1fa117..539c5019be 100644 --- a/ArduPlane/Parameters.cpp +++ b/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 }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 6b891f9469..10c8bd3a1d 100644 --- a/ArduPlane/Parameters.h +++ b/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[]; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 886144ccf3..ae99922f15 100644 --- a/ArduPlane/Plane.h +++ b/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 diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index f7eeec3b67..6536562585 100644 --- a/ArduPlane/takeoff.cpp +++ b/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; } }