From 96d4335765077002ffd389bfffafd1723b7929bb Mon Sep 17 00:00:00 2001 From: Ebin Date: Wed, 27 Jun 2018 19:23:30 +0530 Subject: [PATCH] Rover: Crash check based on angle for all rover frames --- APMrover2/Parameters.cpp | 14 +++++++---- APMrover2/Parameters.h | 4 ++-- APMrover2/crash_check.cpp | 49 ++++++++++++++++++++++----------------- 3 files changed, 39 insertions(+), 28 deletions(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 8a9794487d..da5df82edc 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -556,14 +556,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 5), - // @Param: BAL_PITCH_CRASH - // @DisplayName: BalanceBot Crash Pitch - // @Description: Pitch angle for crash check + // @Param: CRASH_ANGLE + // @DisplayName: Crash Angle + // @Description: Pitch/Roll angle limit in degrees for crash check. Zero disables check // @Units: deg // @Range: 0 60 - // @Increment: 0.1 + // @Increment: 1 // @User: Standard - AP_GROUPINFO("BAL_PITCH_CRASH", 22, ParametersG2, bal_pitch_crash, 30), + AP_GROUPINFO("CRASH_ANGLE", 22, ParametersG2, crash_angle, 0), AP_GROUPEND @@ -642,6 +642,10 @@ void Rover::load_parameters(void) SRV_Channels::set_default_function(CH_1, SRV_Channel::k_steering); SRV_Channels::set_default_function(CH_3, SRV_Channel::k_throttle); + if (is_balancebot()) { + g2.crash_angle.set_default(30); + } + const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old, Parameters::k_param_rc_2_old, Parameters::k_param_rc_3_old, Parameters::k_param_rc_4_old, Parameters::k_param_rc_5_old, Parameters::k_param_rc_6_old, diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 98773b22fe..8f55458352 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -345,8 +345,8 @@ public: // pitch angle at 100% throttle AP_Float bal_pitch_max; - // balance bot pitch for crash check - AP_Float bal_pitch_crash; + // pitch/roll angle for crash check + AP_Int8 crash_angle; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/crash_check.cpp b/APMrover2/crash_check.cpp index 6bbc30e920..6c8ecbe330 100644 --- a/APMrover2/crash_check.cpp +++ b/APMrover2/crash_check.cpp @@ -11,30 +11,25 @@ static const float CRASH_CHECK_VEL_MIN = 0.08f; // vehicle must have a velo void Rover::crash_check() { static uint16_t crash_counter; // number of iterations vehicle may have been crashed + bool crashed = false; //stores crash state - if (is_balancebot() && arming.is_armed()) { - // Crashed if pitch > bal_pitch_crash - if (fabsf(ahrs.pitch) > radians(g2.bal_pitch_crash)) { - // log an error in the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); - // send message to gcs - gcs().send_text(MAV_SEVERITY_EMERGENCY, "Balance Bot crashed"); - disarm_motors(); - } - } - - // return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode - if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (!control_mode->is_autopilot_mode())) { + // 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; 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; + } + // TODO : Check if min vel can be calculated // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise; - if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) || // Check velocity + if (!crashed && ((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)) { + (fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN))) { crash_counter = 0; return; } @@ -44,14 +39,26 @@ void Rover::crash_check() // check if crashing for 2 seconds if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) { + crashed = true; + } + + if (crashed){ // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); - // 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 (is_balancebot()) { + // send message to gcs + 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(); + } } + } }