diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index ed14c5cac5375..85c7120251550 100644 --- a/APMrover2/Parameters.cpp +++ b/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 }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 71a93100b036b..f3ff2c7ae4beb 100644 --- a/APMrover2/Parameters.h +++ b/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; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 508a264620bbd..3a42fc8ac8e78 100644 --- a/APMrover2/Rover.h +++ b/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(); diff --git a/APMrover2/crash_check.cpp b/APMrover2/crash_check.cpp index 7431efaf52ebe..0c2c87fd36eed 100644 --- a/APMrover2/crash_check.cpp +++ b/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; +} diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 343d55a189f83..1b10e89d847b5 100644 --- a/APMrover2/defines.h +++ b/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 {