From 7cde90553d29641e5e6e3f7b28bf70cccee3b05c Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 30 Dec 2015 16:15:57 -0800 Subject: [PATCH] Plane: new param CRASH_ACC_THRESH @Description: X-Axis deceleration threshold to notify the crash detector that there was a possible impact which helps disarm the motor quickly after a crash. This value should be much higher than normal negative x-axis forces during normal flight, check flight log files to determine the average IMU.x values for your aircraft and motor type. Higher value means less sensative (triggers on higher impact). For electric planes that don't vibrate much during fight a value of 25 is good (that's about 2.5G). For petrol/nitro planes you'll want a higher value. Set to 0 to disable the collision detector. --- ArduPlane/Parameters.cpp | 8 ++++++++ ArduPlane/Parameters.h | 2 ++ ArduPlane/Plane.h | 2 +- ArduPlane/is_flying.cpp | 29 ++++++++++------------------- 4 files changed, 21 insertions(+), 20 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 026fa323f7..8a5231d452 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -958,6 +958,14 @@ const AP_Param::Info Plane::var_info[] = { // @User: Standard GSCALAR(trim_rc_at_start, "TRIM_RC_AT_START", 0), + // @Param: CRASH_ACC_THRESH + // @DisplayName: Crash Deceleration Threshold + // @Description: X-Axis deceleration threshold to notify the crash detector that there was a possible impact which helps disarm the motor quickly after a crash. This value should be much higher than normal negative x-axis forces during normal flight, check flight log files to determine the average IMU.x values for your aircraft and motor type. Higher value means less sensative (triggers on higher impact). For electric planes that don't vibrate much during fight a value of 25 is good (that's about 2.5G). For petrol/nitro planes you'll want a higher value. Set to 0 to disable the collision detector. + // @Units: m/s/s + // @Values: 10 127 + // @User: Advanced + GSCALAR(crash_accel_threshold, "CRASH_ACC_THRESH", 0), + // @Param: CRASH_DETECT // @DisplayName: Crash Detection // @Description: Automatically detect a crash during AUTO flight and perform the bitmask selected action(s). Disarm will turn off motor for saftey and to help against burning out ESC and motor. Setting the mode to manual will help save the servos from burning out by overexerting if the aircraft crashed in an odd orientation such as upsidedown. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 2b4536f3d7..4651abc107 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -146,6 +146,7 @@ public: k_param_parachute, k_param_arming = 100, k_param_parachute_channel, + k_param_crash_accel_threshold, // 105: Extra parameters k_param_fence_retalt = 105, @@ -347,6 +348,7 @@ public: AP_Int8 rtl_autoland; AP_Int8 trim_rc_at_start; + AP_Int8 crash_accel_threshold; AP_Int8 crash_detection_enable; // Feed-forward gains diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 2c81310475..303a5e84ee 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -507,7 +507,7 @@ private: // debounce timer uint32_t debounce_timer_ms; - // length of time impact_detected has been true. times out after a few seconds. used to clamp is_flying_prob + // length of time impact_detected has been true. Times out after a few seconds. Used to clip isFlyingProbability uint32_t impact_timer_ms; } crash_state; diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 86742c67bc..eb355f0d0f 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -53,26 +53,18 @@ void Plane::update_is_flying_5Hz(void) // will not change the is_flying state, anything above 0.1 // is "true", it just allows it to decay faster once we decide we // aren't flying using the normal schemes - float accel_x = ins.get_accel_peak_hold_neg().x; - - if (accel_x < -20) { + if (g.crash_accel_threshold == 0) { + crash_state.impact_detected = false; + } else if (ins.get_accel_peak_hold_neg().x < -(g.crash_accel_threshold)) { // large deceleration detected, lets lower confidence VERY quickly - if (isFlyingProbability > 0.25f) { - isFlyingProbability = 0.25f; - crash_state.impact_detected = true; - crash_state.impact_timer_ms = now_ms; - } - } else if (accel_x < -10) { - // medium deceleration detected, lets lower confidence - if (isFlyingProbability > 0.5f) { - isFlyingProbability = 0.5f; - crash_state.impact_detected = true; - crash_state.impact_timer_ms = now_ms; + crash_state.impact_detected = true; + crash_state.impact_timer_ms = now_ms; + if (isFlyingProbability > 0.2f) { + isFlyingProbability = 0.2f; } - } - - if (crash_state.impact_detected && + } else if (crash_state.impact_detected && (now_ms - crash_state.impact_timer_ms > IS_FLYING_IMPACT_TIMER_MS)) { + // no impacts seen in a while, clear the flag so we stop clipping isFlyingProbability crash_state.impact_detected = false; } @@ -123,8 +115,7 @@ void Plane::update_is_flying_5Hz(void) } if (!crash_state.impact_detected || !is_flying_bool) { - // when impact is detected, enforce a clamp. Only allow isFlyingProbability to go down, not up. - + // when impact is detected, enforce a clip. Only allow isFlyingProbability to go down, not up. // low-pass the result. // coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%) isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);