From 748bfdc181c784b6f8c3ab2c239239eadc60cba8 Mon Sep 17 00:00:00 2001 From: TsuyoshiKawamura Date: Fri, 11 Jan 2019 18:16:21 +0900 Subject: [PATCH 1/2] Rover: two step crash check, first concept. --- APMrover2/Parameters.cpp | 14 ++++++ APMrover2/Parameters.h | 2 + APMrover2/Rover.h | 19 ++++++++ APMrover2/crash_check.cpp | 93 ++++++++++++++++++++++++++++++--------- 4 files changed, 108 insertions(+), 20 deletions(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index ed14c5cac5375a..e37cdd0d689445 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:Disable,1:Enable + // @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 71a93100b036b4..f3ff2c7ae4beb0 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 508a264620bbd1..9d77f0fbac70fb 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -377,6 +377,21 @@ 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 + }; + typedef struct { + uint16_t recovery_counter; + uint32_t detected_angle_ms; + uint32_t detected_vel_ms; + Crash_Stage stage; + } crashcheck_t; + crashcheck_t crashcheck; + private: // APMrover2.cpp @@ -412,6 +427,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 7431efaf52ebec..e3b24156a1c566 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,52 @@ 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; + + // 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() +{ + // TODO WAIT or BACK? or STEERING? + return true; +} + +bool Rover::is_recoverable_crash() +{ + return crashcheck.stage != CrashStage_Emergency; +} From d8248a96407294c752b60e6e784d9ffbd500e141 Mon Sep 17 00:00:00 2001 From: TsuyoshiKawamura Date: Sun, 13 Jan 2019 23:39:58 +0900 Subject: [PATCH 2/2] Rover: recovery action --- APMrover2/Parameters.cpp | 2 +- APMrover2/Rover.h | 8 +++++++- APMrover2/crash_check.cpp | 19 ++++++++++++++++++- APMrover2/defines.h | 1 + 4 files changed, 27 insertions(+), 3 deletions(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index e37cdd0d689445..85c71202515501 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -694,7 +694,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: CRASH_RECOVER_ACTION // @DisplayName: Crash Recover Action // @Description: Enable crash recovery action. Zero do nothing - // @Values: 0:Disable,1:Enable + // @Values: 0:Wait, 1:Back // @User: Standard AP_GROUPINFO("CRSH_RCVR_ACTION", 40, ParametersG2, crash_recover_action, 0), diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 9d77f0fbac70fb..3a42fc8ac8e78b 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -381,11 +381,17 @@ class Rover : public AP_HAL::HAL::Callbacks { enum Crash_Stage { CrashStage_None = 0, CrashStage_Detected = 1, - CrashStage_Recovery = 2, + 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; diff --git a/APMrover2/crash_check.cpp b/APMrover2/crash_check.cpp index e3b24156a1c566..0c2c87fd36eed5 100644 --- a/APMrover2/crash_check.cpp +++ b/APMrover2/crash_check.cpp @@ -88,6 +88,10 @@ void Rover::crash_action() 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; @@ -108,7 +112,20 @@ void Rover::crash_action() bool Rover::crash_recovery_action() { - // TODO WAIT or BACK? or STEERING? + 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; } diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 343d55a189f83b..1b10e89d847b51 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 {