aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: improved crash detection

Browse Source 
- fixes bug where a bungee launch is configured but the aircraft gets bumped and triggers the prop to spin up. This will now detect that and "crash' and disable the motor
This commit is contained in:
Tom Pittenger 2016-03-28 14:52:30 -07:00
parent 2887e48178
commit 436062ef37
2 changed files with 26 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/Plane.h
View File

@ -515,6 +515,9 @@ private:
// debounce timer
uint32_t debounce_timer_ms;
// delay time for debounce to count to
uint32_t debounce_time_total_ms;
// 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;

30
ArduPlane/is_flying.cpp
View File

@ -6,7 +6,7 @@
is_flying and crash detection logic
*/
#define CRASH_DETECTION_DELAY_MS 300
#define CRASH_DETECTION_DELAY_MS 500
#define IS_FLYING_IMPACT_TIMER_MS 3000
/*
@ -197,25 +197,39 @@ void Plane::crash_detection_update(void)
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() && been_auto_flying)
if (!is_flying() && arming.is_armed())
{
switch (flight_stage)
{
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
case AP_SpdHgtControl::FLIGHT_NORMAL:
if (!in_preLaunch_flight_stage()) {
if (g.takeoff_throttle_min_accel > 0 &&
!throttle_suppressed) {
// if you have an acceleration holding back throttle, but you met the
// accel threshold but still not fying, then you either shook/hit the
// plane or it was a failed launch.
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
// TODO: handle auto missions without NAV_TAKEOFF mission cmd
break;
case AP_SpdHgtControl::FLIGHT_NORMAL:
if (!in_preLaunch_flight_stage() && been_auto_flying) {
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
break;
case AP_SpdHgtControl::FLIGHT_VTOL:
// we need a totally new method for this
crashed = false;
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
crashed = true;
if (been_auto_flying) {
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
// 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 would be caught here.
@ -228,8 +242,10 @@ void Plane::crash_detection_update(void)
// 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 (!crash_state.checkedHardLanding && // only check once
(fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) {
been_auto_flying &&
(labs(ahrs.roll_sensor) > 6000 || labs(ahrs.pitch_sensor) > 6000)) {
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
// did we "crash" within 75m of the landing location? Probably just a hard landing
crashed_near_land_waypoint =
@ -259,7 +275,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 >= CRASH_DETECTION_DELAY_MS) && !crash_state.is_crashed) {
} else if ((now_ms - crash_state.debounce_timer_ms >= crash_state.debounce_time_total_ms) && !crash_state.is_crashed) {
crash_state.is_crashed = true;
if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {