Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: Crash check based on angle for all rover frames

Browse Source
This commit is contained in:
Ebin 2018-06-27 19:23:30 +05:30 committed by Randy Mackay
parent fd92475ce9
commit 96d4335765
3 changed files with 39 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
APMrover2/Parameters.cpp
View File

@ -556,14 +556,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 5), AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 5),
// @Param: BAL_PITCH_CRASH // @Param: CRASH_ANGLE
// @DisplayName: BalanceBot Crash Pitch // @DisplayName: Crash Angle
// @Description: Pitch angle for crash check // @Description: Pitch/Roll angle limit in degrees for crash check. Zero disables check
// @Units: deg // @Units: deg
// @Range: 0 60 // @Range: 0 60
// @Increment: 0.1 // @Increment: 1
// @User: Standard // @User: Standard
AP_GROUPINFO("BAL_PITCH_CRASH", 22, ParametersG2, bal_pitch_crash, 30), AP_GROUPINFO("CRASH_ANGLE", 22, ParametersG2, crash_angle, 0),
AP_GROUPEND 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_1, SRV_Channel::k_steering);
SRV_Channels::set_default_function(CH_3, SRV_Channel::k_throttle); 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, 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_3_old, Parameters::k_param_rc_4_old,
Parameters::k_param_rc_5_old, Parameters::k_param_rc_6_old, Parameters::k_param_rc_5_old, Parameters::k_param_rc_6_old,

4
APMrover2/Parameters.h
View File

@ -345,8 +345,8 @@ public:
// pitch angle at 100% throttle // pitch angle at 100% throttle
AP_Float bal_pitch_max; AP_Float bal_pitch_max;
// balance bot pitch for crash check // pitch/roll angle for crash check
AP_Float bal_pitch_crash; AP_Int8 crash_angle;
}; };
extern const AP_Param::Info var_info[]; extern const AP_Param::Info var_info[];

49
APMrover2/crash_check.cpp
View File

@ -11,30 +11,25 @@ static const float CRASH_CHECK_VEL_MIN = 0.08f; // vehicle must have a velo
void Rover::crash_check() void Rover::crash_check()
{ {
static uint16_t crash_counter; // number of iterations vehicle may have been crashed 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()) { // return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode(if it is not a balance bot)
// Crashed if pitch > bal_pitch_crash if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || ((!control_mode->is_autopilot_mode()) && (!is_balancebot()))) {
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())) {
crash_counter = 0; crash_counter = 0;
return; 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 // TODO : Check if min vel can be calculated
// min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise; // 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(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; crash_counter = 0;
return; return;
} }
@ -44,14 +39,26 @@ void Rover::crash_check()
// check if crashing for 2 seconds // check if crashing for 2 seconds
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) { if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) {
crashed = true;
}
if (crashed){
// log an error in the dataflash // log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); 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"); if (is_balancebot()) {
// change mode to hold and disarm // send message to gcs
set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE); gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Disarming");
if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { disarm_motors();
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();
}
} }
} }
} }