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: Modify indentation (NFC)

Browse Source
This commit is contained in:
murata 2018-07-29 08:25:54 +09:00 committed by Randy Mackay
parent 037d7d2e74
commit c705d3d816
1 changed files with 42 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
APMrover2/crash_check.cpp
View File

@ -10,57 +10,56 @@ static const float CRASH_CHECK_VEL_MIN = 0.08f; // vehicle must have a velo
// called at 10Hz // called at 10Hz
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 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) // 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()))) { 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 (!is_balancebot()) {
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))) {
crash_counter = 0; crash_counter = 0;
return; return;
} }
// we may be crashing // Crashed if pitch/roll > crash_angle
crash_counter++; if ((g2.crash_angle != 0) && ((fabsf(ahrs.pitch) > radians(g2.crash_angle)) || (fabsf(ahrs.roll) > radians(g2.crash_angle)))) {
// check if crashing for 2 seconds
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) {
crashed = true; crashed = true;
} }
}
if (crashed) { // TODO : Check if min vel can be calculated
// log an error in the dataflash // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise;
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
if (is_balancebot()) { if (!is_balancebot()) {
// send message to gcs if (!crashed && ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) || // Check velocity
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Disarming"); (fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) || // Check turn speed
disarm_motors(); (fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN))) {
} else { crash_counter = 0;
// send message to gcs return;
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD"); }
// change mode to hold and disarm
set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE); // we may be crashing
if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { crash_counter++;
disarm_motors();
// 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
set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE);
if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
disarm_motors();
}
}
}
} }