Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: improved crash detection logic and agility

Browse Source 
- 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"
This commit is contained in:
Tom Pittenger 2015-11-17 16:11:21 -08:00 committed by Andrew Tridgell
parent 3aaf2b1d2b
commit 21205f8b41
4 changed files with 93 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/Log.cpp
View File

@ -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" },

9
ArduPlane/Plane.h
View File

@ -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();

119
ArduPlane/is_flying.cpp
View File

@ -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);
}

1
ArduPlane/system.cpp
View File

@ -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