Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -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
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,

4
APMrover2/Parameters.h
View File

@ -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[];

37
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()
{
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,8 +39,18 @@ 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);
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
@ -54,4 +59,6 @@ void Rover::crash_check()
disarm_motors();
}
}
}
}