ardupilot/APMrover2/crash_check.cpp

47 lines
1.8 KiB
C++
Raw Normal View History

2016-11-21 12:08:24 -04:00
#include "Rover.h"
// Code to detect a crash or block
static const uint16_t CRASH_CHECK_TRIGGER_SEC = 2; // 2 seconds blocked indicates a crash
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
2016-11-21 12:08:24 -04:00
// 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
// return immediately if disarmed, or crash checking disabled or in HOLD mode
2017-07-18 23:19:08 -03:00
if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (!control_mode->is_autopilot_mode())) {
2016-11-21 12:08:24 -04:00
crash_counter = 0;
return;
}
// 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
(fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) || // Check turn speed
(fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN)) {
2016-11-21 12:08:24 -04:00
crash_counter = 0;
return;
}
// we may be crashing
crash_counter++;
// check if crashing for 2 seconds
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) {
// 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");
2016-11-21 12:08:24 -04:00
// change mode to hold and disarm
2017-07-18 23:19:08 -03:00
set_mode(mode_hold);
2016-11-21 12:08:24 -04:00
if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
disarm_motors();
}
}
}