diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index ec9fe99e92..a8c0852e51 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -903,7 +903,7 @@ void Plane::update_is_flying_5Hz(void) // we are definitely not flying, or at least for not much longer! if (throttle_suppressed) { is_flying_bool = false; - auto_state.is_crashed = false; + crash_state.is_crashed = false; } break; @@ -919,7 +919,6 @@ void Plane::update_is_flying_5Hz(void) break; case AP_SpdHgtControl::FLIGHT_LAND_FINAL: - default: break; } // switch } @@ -985,13 +984,10 @@ bool Plane::is_flying(void) */ void Plane::crash_detection_update(void) { - static uint32_t crash_timer_ms = 0; - static bool checkHardLanding = false; - if (control_mode != AUTO) { // crash detection is only available in AUTO mode - crash_timer_ms = 0; + crash_state.debounce_timer_ms = 0; return; } @@ -1039,7 +1035,7 @@ 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 (been_auto_flying && - !checkHardLanding && // only check once + !crash_state.checkHardLanding && // only check once (fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) { crashed = true; @@ -1050,29 +1046,26 @@ 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_timer_ms = now_ms + 2500; + crash_state.debounce_timer_ms = now_ms + 2500; } - checkHardLanding = true; - break; - - default: + crash_state.checkHardLanding = true; break; } // switch } else { - checkHardLanding = false; + crash_state.checkHardLanding = false; } if (!crashed) { // reset timer - crash_timer_ms = 0; - auto_state.is_crashed = false; + crash_state.debounce_timer_ms = 0; + crash_state.is_crashed = false; - } else if (crash_timer_ms == 0) { + } else if (crash_state.debounce_timer_ms == 0) { // start timer - crash_timer_ms = now_ms; + crash_state.debounce_timer_ms = now_ms; - } else if ((now_ms - crash_timer_ms >= 2500) && !auto_state.is_crashed) { - auto_state.is_crashed = true; + } else if ((now_ms - crash_state.debounce_timer_ms >= 2500) && !crash_state.is_crashed) { + crash_state.is_crashed = true; if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { if (crashed_near_land_waypoint) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 85618ccf26..9d0aa17b47 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -13,7 +13,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan) if (failsafe.state != FAILSAFE_NONE) { system_status = MAV_STATE_CRITICAL; - } else if (plane.auto_state.is_crashed) { + } else if (plane.crash_state.is_crashed) { system_status = MAV_STATE_EMERGENCY; } else if (is_flying()) { system_status = MAV_STATE_ACTIVE; diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 495925e6a9..3216a1e656 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -331,7 +331,7 @@ void Plane::Log_Write_Status() ,is_flying_probability : isFlyingProbability ,armed : hal.util->get_soft_armed() ,safety : hal.util->safety_switch_state() - ,is_crashed : auto_state.is_crashed + ,is_crashed : crash_state.is_crashed ,is_still : plane.ins.is_still() }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c36a0ad5b5..173f5f7666 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -433,9 +433,6 @@ private: // movement until altitude is reached bool idle_mode:1; - // crash detection - bool is_crashed:1; - // used to 'wiggle' servos in idle mode to prevent them freezing // at high altitudes uint8_t idle_wiggle_stage; @@ -479,6 +476,18 @@ private: uint32_t started_flying_in_auto_ms; } auto_state; + 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; + + // crash detection. True when we are crashed + bool is_crashed:1; + + // debounce timer + uint32_t debounce_timer_ms; + } crash_state; + // true if we are in an auto-throttle mode, which means // we need to run the speed/height controller bool auto_throttle_mode; @@ -492,6 +501,9 @@ private: // 1 where 1 is 100% sure we're in flight float isFlyingProbability; + // previous value of is_flying() + bool previous_is_flying; + // Navigation control variables // The instantaneous desired bank angle. Hundredths of a degree int32_t nav_roll_cd; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 36197cc4b3..92268cf3ab 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -23,7 +23,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) auto_state.takeoff_complete = true; // if we are still executing mission commands then we must be traveling around still - auto_state.is_crashed = false; + crash_state.is_crashed = false; // if a go around had been commanded, clear it now. auto_state.commanded_go_around = false; @@ -42,7 +42,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) switch(cmd.id) { case MAV_CMD_NAV_TAKEOFF: - auto_state.is_crashed = false; + crash_state.is_crashed = false; do_takeoff(cmd); break; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 1a036c3d04..46bb0e8d7b 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -348,7 +348,7 @@ void Plane::set_mode(enum FlightMode mode) steer_state.locked_course_err = 0; // reset crash detection - auto_state.is_crashed = false; + crash_state.is_crashed = false; // set mode previous_mode = control_mode;