From 21205f8b417b434ec4dbdbaab1e67e81d5659350 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 17 Nov 2015 16:11:21 -0800 Subject: [PATCH] Plane: improved crash detection logic and agility - inhibit crash detection warnings when disabled by param so now it can be completely disabled - reset is_crashed when disabled by param - fixed pre-takeoff detection bug by adding in_preLaunch_flight_stage() where we are actually in FLIGHT_NORMAL instead of FLIGHT_TAKEOFF during setup of bungee launches. This now detects if we're in that state - simplified the use of been_auto_flying to check across all flight stages. before it was excluded to handle hand-launches which can now be detected with in_preLaunch_flight_stage() - added impact detector timer to clamp is_flying a few seconds after an impact - logging new impact detector as "STAT.Hit" --- ArduPlane/Log.cpp | 4 +- ArduPlane/Plane.h | 9 ++- ArduPlane/is_flying.cpp | 119 +++++++++++++++++++++++++++------------- ArduPlane/system.cpp | 1 + 4 files changed, 93 insertions(+), 40 deletions(-) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c2b3505867..223745280b 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -310,6 +310,7 @@ struct PACKED log_Status { bool is_crashed; bool is_still; uint8_t stage; + bool impact; }; void Plane::Log_Write_Status() @@ -324,6 +325,7 @@ void Plane::Log_Write_Status() ,is_crashed : crash_state.is_crashed ,is_still : plane.ins.is_still() ,stage : flight_stage + ,impact : crash_state.impact_detected }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -489,7 +491,7 @@ static const struct LogStructure log_structure[] = { { LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP), "ATRP", "QBBcfff", "TimeUS,Type,State,Servo,Demanded,Achieved,P" }, { LOG_STATUS_MSG, sizeof(log_Status), - "STAT", "QBfBBBBB", "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage" }, + "STAT", "QBfBBBBBB", "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit" }, #if OPTFLOW == ENABLED { LOG_OPTFLOW_MSG, sizeof(log_Optflow), "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY" }, diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index a217a37072..2c81310475 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -496,13 +496,19 @@ private: struct { // on hard landings, only check once after directly a landing so you // don't trigger a crash when picking up the aircraft - bool checkHardLanding:1; + bool checkedHardLanding:1; // crash detection. True when we are crashed bool is_crashed:1; + // impact detection flag. Expires after a few seconds via impact_timer_ms + bool impact_detected:1; + // 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 + uint32_t impact_timer_ms; } crash_state; // true if we are in an auto-throttle mode, which means @@ -933,6 +939,7 @@ private: void update_aux(); void update_is_flying_5Hz(void); void crash_detection_update(void); + bool in_preLaunch_flight_stage(void); void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...); void handle_auto_mode(void); void calc_throttle(); diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 49b34ae714..86742c67bc 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -6,6 +6,9 @@ is_flying and crash detection logic */ +#define CRASH_DETECTION_DELAY_MS 300 +#define IS_FLYING_IMPACT_TIMER_MS 3000 + /* Do we think we are flying? Probabilistic method where a bool is low-passed and considered a probability. @@ -45,25 +48,50 @@ void Plane::update_is_flying_5Hz(void) make is_flying() more accurate during various auto modes */ + // Detect X-axis deceleration for probable ground impacts. + // Limit the max probability so it can decay faster. This + // 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) { + // 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; + } + } + + if (crash_state.impact_detected && + (now_ms - crash_state.impact_timer_ms > IS_FLYING_IMPACT_TIMER_MS)) { + crash_state.impact_detected = false; + } + switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_TAKEOFF: - // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so - // ensure we aren't showing a false positive. If the throttle is suppressed - // we are definitely not flying, or at least for not much longer! - if (throttle_suppressed) { + break; + + case AP_SpdHgtControl::FLIGHT_NORMAL: + if (in_preLaunch_flight_stage()) { + // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so + // ensure we aren't showing a false positive. is_flying_bool = false; crash_state.is_crashed = false; + auto_state.started_flying_in_auto_ms = 0; } break; - case AP_SpdHgtControl::FLIGHT_LAND_ABORT: - case AP_SpdHgtControl::FLIGHT_NORMAL: - // TODO: detect ground impacts - break; - case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: - // TODO: detect ground impacts if (fabsf(auto_state.sink_rate) > 0.2f) { is_flying_bool = true; } @@ -71,6 +99,16 @@ void Plane::update_is_flying_5Hz(void) case AP_SpdHgtControl::FLIGHT_LAND_FINAL: break; + + case AP_SpdHgtControl::FLIGHT_LAND_ABORT: + if (auto_state.sink_rate < -0.5f) { + // steep climb + is_flying_bool = true; + } + break; + + default: + break; } // switch } } else { @@ -84,9 +122,13 @@ void Plane::update_is_flying_5Hz(void) } } - // 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); + if (!crash_state.impact_detected || !is_flying_bool) { + // when impact is detected, enforce a clamp. 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); + } /* update last_flying_ms so we always know how long we have not @@ -139,50 +181,37 @@ bool Plane::is_flying(void) */ void Plane::crash_detection_update(void) { - if (control_mode != AUTO) + if (control_mode != AUTO || !g.crash_detection_enable) { // crash detection is only available in AUTO mode crash_state.debounce_timer_ms = 0; + crash_state.is_crashed = false; return; } uint32_t now_ms = AP_HAL::millis(); - bool auto_launch_detected; bool crashed_near_land_waypoint = false; bool crashed = false; bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) && (now_ms - auto_state.started_flying_in_auto_ms >= 2500); - if (!is_flying()) + if (!is_flying() && been_auto_flying) { switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_TAKEOFF: - auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0); - - if (been_auto_flying || // failed hand launch - auto_launch_detected) { // threshold of been_auto_flying may not be met on auto-launches - - // has launched but is no longer flying. That's a crash on takeoff. - crashed = true; - } - break; - - case AP_SpdHgtControl::FLIGHT_LAND_ABORT: case AP_SpdHgtControl::FLIGHT_NORMAL: - if (been_auto_flying) { + if (!in_preLaunch_flight_stage()) { crashed = true; } // TODO: handle auto missions without NAV_TAKEOFF mission cmd break; case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: - if (been_auto_flying) { - crashed = true; - } + crashed = true; // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL // so ground crashes most likely can not be triggered from here. However, - // a crash into a tree, for example, would be. + // a crash into a tree would be caught here. break; case AP_SpdHgtControl::FLIGHT_LAND_FINAL: @@ -190,8 +219,7 @@ void Plane::crash_detection_update(void) // likely had a crazy landing. Throttle is inhibited already at the flare // but go ahead and notify GCS and perform any additional post-crash actions. // Declare a crash if we are oriented more that 60deg in pitch or roll - if (been_auto_flying && - !crash_state.checkHardLanding && // only check once + if (!crash_state.checkedHardLanding && // only check once (fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) { crashed = true; @@ -202,13 +230,17 @@ void Plane::crash_detection_update(void) // trigger hard landing event right away, or never again. This inhibits a false hard landing // event when, for example, a minute after a good landing you pick the plane up and // this logic is still running and detects the plane is on its side as you carry it. - crash_state.debounce_timer_ms = now_ms + 2500; + crash_state.debounce_timer_ms = now_ms + CRASH_DETECTION_DELAY_MS; } - crash_state.checkHardLanding = true; + + crash_state.checkedHardLanding = true; + break; + + default: break; } // switch } else { - crash_state.checkHardLanding = false; + crash_state.checkedHardLanding = false; } if (!crashed) { @@ -219,7 +251,7 @@ void Plane::crash_detection_update(void) // start timer crash_state.debounce_timer_ms = now_ms; - } else if ((now_ms - crash_state.debounce_timer_ms >= 2500) && !crash_state.is_crashed) { + } else if ((now_ms - crash_state.debounce_timer_ms >= CRASH_DETECTION_DELAY_MS) && !crash_state.is_crashed) { crash_state.is_crashed = true; if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { @@ -243,3 +275,14 @@ void Plane::crash_detection_update(void) } } +/* + * return true if we are in a pre-launch phase of an auto-launch, typically used in bungee launches + */ +bool Plane::in_preLaunch_flight_stage(void) { + return (control_mode == AUTO && + throttle_suppressed && + flight_stage == AP_SpdHgtControl::FLIGHT_NORMAL && + mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF); +} + + diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 8c8d1d8db7..657ac37a20 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -342,6 +342,7 @@ void Plane::set_mode(enum FlightMode mode) // reset crash detection crash_state.is_crashed = false; + crash_state.impact_detected = false; // always reset this because we don't know who called set_mode. In evasion // behavior you should set this flag after set_mode so you know the evasion