Rover: Two step crash check

APMrover2/Parameters.cpp

@@ -684,6 +684,20 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("MIS_DONE_BEHAVE", 38, ParametersG2, mis_done_behave, 0),
 
+    // @Param: CRASH_RECOVER_ENABLE
+    // @DisplayName: Crash Recover Enable
+    // @Description: Enable crash recovery. Zero disables recovery
+    // @Values: 0:Disable,1:Enable
+    // @User: Standard
+    AP_GROUPINFO("CRSH_RCVR_ENABLE", 39, ParametersG2, crash_recover_enable, 0),
+
+    // @Param: CRASH_RECOVER_ACTION
+    // @DisplayName: Crash Recover Action
+    // @Description: Enable crash recovery action. Zero do nothing
+    // @Values: 0:Wait, 1:Back
+    // @User: Standard
+    AP_GROUPINFO("CRSH_RCVR_ACTION", 40, ParametersG2, crash_recover_action, 0),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -356,6 +356,8 @@ class ParametersG2 {
 
     // pitch/roll angle for crash check
     AP_Int8 crash_angle;
+    AP_Int8 crash_recover_enable;
+    AP_Int8 crash_recover_action;
 
     // follow mode library
     AP_Follow follow;

APMrover2/Rover.h

@@ -377,6 +377,27 @@ class Rover : public AP_HAL::HAL::Callbacks {
         uint32_t auto_tack_start_ms;    // system time when tack was started in autonomous mode
     } sailboat;
 
+    // crash check
+    enum Crash_Stage {
+        CrashStage_None      = 0,
+        CrashStage_Detected  = 1,
+        CrashStage_Recovery  = 2,
+        CrashStage_Emergency = 3
+    };
+    enum Crash_Action {
+        CrashAction_Stay       = 0,
+        CrashAction_Back       = 1,
+        CrashAction_ChangeMode = 2
+    };
+    typedef struct {
+        uint16_t recovery_counter;
+        Mode *recovery_mode;
+        uint32_t detected_angle_ms;
+        uint32_t detected_vel_ms;
+        Crash_Stage stage;
+    } crashcheck_t;
+    crashcheck_t crashcheck;
+
 private:
 
     // APMrover2.cpp
@@ -412,6 +433,10 @@ class Rover : public AP_HAL::HAL::Callbacks {
 
     // crash_check.cpp
     void crash_check();
+    void crash_check_init();
+    void crash_action();
+    bool crash_recovery_action();
+    bool is_recoverable_crash();
 
     // cruise_learn.cpp
     void cruise_learn_start();

APMrover2/crash_check.cpp

@@ -5,46 +5,60 @@ static const uint16_t CRASH_CHECK_TRIGGER_SEC = 2;   // 2 seconds blocked indica
 static const float CRASH_CHECK_THROTTLE_MIN = 5.0f;  // vehicle must have a throttle greater that 5% to be considered crashed
 static const float CRASH_CHECK_VEL_MIN = 0.08f;      // vehicle must have a velocity under 0.08 m/s or rad/s to be considered crashed
 
+void Rover::crash_check_init()
+{
+    crashcheck.stage = CrashStage_None;
+    crashcheck.recovery_counter = 0;
+    crashcheck.detected_angle_ms = -1;
+    crashcheck.detected_vel_ms = -1;
+}
+
 // crash_check - disarms motors if a crash or block has been detected
 // crashes are detected by the vehicle being static (no speed) for more than CRASH_CHECK_TRIGGER_SEC and motor are running
 // called at 10Hz
 void Rover::crash_check()
 {
-    static uint16_t crash_counter;  // number of iterations vehicle may have been crashed
-    bool crashed = false; //stores crash state
-
     // return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode(if it is not a balance bot)
-    if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || ((!control_mode->is_autopilot_mode()) && (!is_balancebot()))) {
-        crash_counter = 0;
+    if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || crashcheck.stage != CrashStage_Recovery || ((!control_mode->is_autopilot_mode()) && (!is_balancebot()))) {
         return;
     }
 
     // Crashed if pitch/roll > crash_angle
     if ((g2.crash_angle != 0) && ((fabsf(ahrs.pitch) > radians(g2.crash_angle)) || (fabsf(ahrs.roll) > radians(g2.crash_angle)))) {
-        crashed = true;
+        if (crashcheck.detected_angle_ms != -1 && AP_HAL::micros() - crashcheck.detected_angle_ms >= 2000) {
+            crashcheck.stage = CrashStage_Emergency;
+        } else {
+            crashcheck.detected_angle_ms = AP_HAL::micros();
+            crashcheck.stage = CrashStage_Detected;
+        }
     }
 
     // TODO : Check if min vel can be calculated
     // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise;
 
     if (!is_balancebot()) {
-        if (!crashed && ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) ||        // Check velocity
+        if (crashcheck.detected_angle_ms == -1 && ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) ||  // Check velocity
             (fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) ||  // Check turn speed
             (fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN))) {
-            crash_counter = 0;
             return;
         }
 
-        // we may be crashing
-        crash_counter++;
-
         // check if crashing for 2 seconds
-        if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) {
-            crashed = true;
+        if (crashcheck.detected_angle_ms != -1 && AP_HAL::micros() - crashcheck.detected_vel_ms >= 2000) {
+            crashcheck.stage = CrashStage_Emergency;
+        } else {
+            crashcheck.detected_vel_ms = AP_HAL::micros();
+            crashcheck.stage = CrashStage_Detected;
         }
     }
 
-    if (crashed) {
+    crash_action();
+}
+
+void Rover::crash_action()
+{
+    switch (crashcheck.stage) {
+    case CrashStage_Detected:
         // log an error in the dataflash
         Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
 
@@ -53,13 +67,69 @@ void Rover::crash_check()
             gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Disarming");
             disarm_motors();
         } else {
-            // send message to gcs
-            gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
-            // change mode to hold and disarm
-            set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE);
-            if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
-                disarm_motors();
+            if (!g2.crash_recover_enable) {
+                // send message to gcs
+                gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
+                // change mode to hold and disarm
+                set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE);
+                if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
+                    disarm_motors();
+                }
+            } else {
+                // send message to gcs
+                gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to RECOVERY");
+                crashcheck.stage = CrashStage_Recovery;
             }
         }
+        break;
+
+    case CrashStage_Recovery:
+        crashcheck.recovery_counter += 1;
+        crashcheck.detected_angle_ms = -1;
+        crashcheck.detected_vel_ms = -1;
+
+        if (crashcheck.recovery_counter == 1) {
+            crashcheck.recovery_mode = control_mode;
+        }
+
+        // recvery action
+        if (!crash_recovery_action()) {
+            crashcheck.stage = CrashStage_Emergency;
+        }
+        break;
+
+    case CrashStage_Emergency:
+        // send message to gcs
+        gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to EMERGENCY");
+        // change mode to hold and disarm
+        set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE);
+        if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
+            disarm_motors();
+        }
+        break;
     }
 }
+
+bool Rover::crash_recovery_action()
+{
+    if (crashcheck.recovery_counter > 20) {
+        set_mode(*(crashcheck.recovery_mode), MODE_REASON_CRASH_RECOVERY);
+        crash_check_init();
+        return true;
+    }
+
+    switch (g2.crash_recover_action) {
+    case CrashAction_Stay:
+        set_mode(mode_hold, MODE_REASON_CRASH_RECOVERY);
+        break;
+    case CrashAction_Back:
+        // TODO vehicle go back
+        break;
+    }
+    return true;
+}
+
+bool Rover::is_recoverable_crash()
+{
+    return crashcheck.stage != CrashStage_Emergency;
+}

APMrover2/defines.h

@@ -122,6 +122,7 @@ enum mode_reason_t {
     MODE_REASON_MISSION_COMMAND,
     MODE_REASON_FENCE_BREACH,
     MODE_REASON_EKF_FAILSAFE,
+    MODE_REASON_CRASH_RECOVERY,
 };
 
 enum pilot_steer_type_t {