// 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
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)
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;
return;
}
// we may be crashing
crash_counter++;
// check if crashing for 2 seconds
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) {
crashed = true;
}
}
if (crashed) {
AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK,
LogErrorCode::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();
}
}
}
}
void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
{
switch ((Failsafe_Action)action) {
case Failsafe_Action_None:
break;
case Failsafe_Action_SmartRTL:
if (set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
break;
}
FALLTHROUGH;
case Failsafe_Action_RTL:
if (set_mode(mode_rtl, MODE_REASON_FAILSAFE)) {
break;
}
FALLTHROUGH;
case Failsafe_Action_Hold:
set_mode(mode_hold, MODE_REASON_FAILSAFE);
break;
case Failsafe_Action_SmartRTL_Hold:
if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
set_mode(mode_hold, MODE_REASON_FAILSAFE);
}
break;
case Failsafe_Action_Terminate:
#if ADVANCED_FAILSAFE == ENABLED
char battery_type_str[17];
snprintf(battery_type_str, 17, "%s battery", type_str);
afs.gcs_terminate(true, battery_type_str);
#else
disarm_motors();
#endif // ADVANCED_FAILSAFE == ENABLED
break;
}
}
/*
this failsafe_check function is called from the core timer interrupt
at 1kHz.
*/
void Rover::failsafe_check()
{
static uint16_t last_ticks;
static uint32_t last_timestamp;
const uint32_t tnow = AP_HAL::micros();
const uint16_t ticks = scheduler.ticks();
if (ticks != last_ticks) {
// the main loop is running, all is OK
last_ticks = ticks;
last_timestamp = tnow;
return;
}
if (tnow - last_timestamp > 200000) {
// we have gone at least 0.2 seconds since the main loop
// ran. That means we're in trouble, or perhaps are in
// an initialisation routine or log erase. disarm the motors
// To-Do: log error to dataflash
if (arming.is_armed()) {
// disarm motors
disarm_motors();
}
}
}
/*
check for driver input on rudder/steering stick for arming/disarming
*/
void Rover::rudder_arm_disarm_check()
{
// In Rover we need to check that its set to the throttle trim and within the DZ
// if throttle is not within trim dz, then pilot cannot rudder arm/disarm
if (!channel_throttle->in_trim_dz()) {
rudder_arm_timer = 0;
return;
}
// if not in a manual throttle mode then disallow rudder arming/disarming
if (control_mode->auto_throttle()) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_steer->get_control_in() > 4000) {
const uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
// time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (!motor_active() & !g2.motors.have_skid_steering()) {
// when armed and motor not active (not moving), full left rudder starts disarming counter
// This is disabled for skid steering otherwise when tring to turn a skid steering rover around
// the rover would disarm
if (channel_steer->get_control_in() < -4000) {
const uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
// time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
/*
check for driver input on rudder/steering stick for arming/disarming
*/
void Rover::rudder_arm_disarm_check()
{
// In Rover we need to check that its set to the throttle trim and within the DZ
// if throttle is not within trim dz, then pilot cannot rudder arm/disarm
if (!channel_throttle->in_trim_dz()) {
rudder_arm_timer = 0;
return;
}
// check if arming/disarming allowed from this mode
if (!control_mode->allows_arming_from_transmitter()) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_steer->get_control_in() > 4000) {
const uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < ARM_DELAY_MS) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
// time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (!g2.motors.active()) {
// when armed and motor not active (not moving), full left rudder starts disarming counter
if (channel_steer->get_control_in() < -4000) {
const uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < ARM_DELAY_MS) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
// time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
/*
If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
only called from AP_Landing, when the landing library is ready to disarm
*/
void Plane::disarm_if_autoland_complete()
{
if (landing.get_disarm_delay() > 0 &&
!is_flying() &&
arming.arming_required() != AP_Arming::NO &&
arming.is_armed()) {
/* we have auto disarm enabled. See if enough time has passed */
if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) {
if (disarm_motors()) {
gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed");
}
}
}
}
/*
If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
*/
void Plane::disarm_if_autoland_complete()
{
if (g.land_disarm_delay > 0 &&
auto_state.land_complete &&
!is_flying() &&
arming.arming_required() != AP_Arming::NO &&
arming.is_armed()) {
/* we have auto disarm enabled. See if enough time has passed */
if (millis() - auto_state.last_flying_ms >= g.land_disarm_delay*1000UL) {
if (disarm_motors()) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Auto-Disarmed"));
}
}
}
}
// 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
if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (control_mode != GUIDED && control_mode != AUTO)) {
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
((100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))) < CRASH_CHECK_THROTTLE_MIN)) {
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");
// change mode to hold and disarm
set_mode(HOLD);
if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
disarm_motors();
}
}
}
// 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, 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;
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)) {
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");
// 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();
}
}
}
/*
* Determine if we have crashed
*/
void Plane::crash_detection_update(void)
{
if (control_mode != AUTO || !g.crash_detection_enable)
{
// crash detection is only available in AUTO mode
crash_state.debounce_timer_ms = 0;
crash_state.is_crashed = false;
return;
}
uint32_t now_ms = AP_HAL::millis();
bool crashed_near_land_waypoint = false;
bool crashed = false;
bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
(now_ms - auto_state.started_flying_in_auto_ms >= 2500);
if (!is_flying() && been_auto_flying)
{
switch (flight_stage)
{
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
case AP_SpdHgtControl::FLIGHT_NORMAL:
if (!in_preLaunch_flight_stage()) {
crashed = true;
}
// TODO: handle auto missions without NAV_TAKEOFF mission cmd
break;
case AP_SpdHgtControl::FLIGHT_VTOL:
// we need a totally new method for this
crashed = false;
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
crashed = true;
// when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL
// so ground crashes most likely can not be triggered from here. However,
// a crash into a tree would be caught here.
break;
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
// We should be nice and level-ish in this flight stage. If not, we most
// likely had a crazy landing. Throttle is inhibited already at the flare
// but go ahead and notify GCS and perform any additional post-crash actions.
// Declare a crash if we are oriented more that 60deg in pitch or roll
if (!crash_state.checkedHardLanding && // only check once
(fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) {
crashed = true;
// did we "crash" within 75m of the landing location? Probably just a hard landing
crashed_near_land_waypoint =
get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75;
// trigger hard landing event right away, or never again. This inhibits a false hard landing
// event when, for example, a minute after a good landing you pick the plane up and
// this logic is still running and detects the plane is on its side as you carry it.
crash_state.debounce_timer_ms = now_ms + CRASH_DETECTION_DELAY_MS;
}
crash_state.checkedHardLanding = true;
break;
default:
break;
} // switch
} else {
crash_state.checkedHardLanding = false;
}
if (!crashed) {
// reset timer
crash_state.debounce_timer_ms = 0;
} else if (crash_state.debounce_timer_ms == 0) {
// start timer
crash_state.debounce_timer_ms = now_ms;
} else if ((now_ms - crash_state.debounce_timer_ms >= CRASH_DETECTION_DELAY_MS) && !crash_state.is_crashed) {
crash_state.is_crashed = true;
if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
if (crashed_near_land_waypoint) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken");
} else {
gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken");
}
}
else {
if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
disarm_motors();
}
auto_state.land_complete = true;
if (crashed_near_land_waypoint) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected");
} else {
gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected");
}
}
}
}
/*
check for pilot input on rudder stick for arming/disarming
*/
void Plane::rudder_arm_disarm_check()
{
AP_Arming::ArmingRudder arming_rudder = arming.rudder_arming();
if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) {
//parameter disallows rudder arming/disabling
return;
}
// if throttle is not down, then pilot cannot rudder arm/disarm
if (channel_throttle->control_in != 0){
rudder_arm_timer = 0;
return;
}
// if not in a manual throttle mode then disallow rudder arming/disarming
if (auto_throttle_mode ) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_rudder->control_in > 4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM && !is_flying()) {
// when armed and not flying, full left rudder starts disarming counter
if (channel_rudder->control_in < -4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
/*
* Determine if we have crashed
*/
void Plane::crash_detection_update(void)
{
if (control_mode != AUTO || !aparm.crash_detection_enable)
{
// crash detection is only available in AUTO mode
crash_state.debounce_timer_ms = 0;
crash_state.is_crashed = false;
return;
}
uint32_t now_ms = AP_HAL::millis();
bool crashed_near_land_waypoint = false;
bool crashed = false;
bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
(now_ms - auto_state.started_flying_in_auto_ms >= 2500);
if (!is_flying() && arming.is_armed())
{
if (landing.is_expecting_impact()) {
// We should be nice and level-ish in this flight stage. If not, we most
// likely had a crazy landing. Throttle is inhibited already at the flare
// but go ahead and notify GCS and perform any additional post-crash actions.
// Declare a crash if we are oriented more that 60deg in pitch or roll
if (!crash_state.checkedHardLanding && // only check once
been_auto_flying &&
(labs(ahrs.roll_sensor) > 6000 || labs(ahrs.pitch_sensor) > 6000)) {
crashed = true;
// did we "crash" within 75m of the landing location? Probably just a hard landing
crashed_near_land_waypoint =
current_loc.get_distance(mission.get_current_nav_cmd().content.location) < 75;
// trigger hard landing event right away, or never again. This inhibits a false hard landing
// event when, for example, a minute after a good landing you pick the plane up and
// this logic is still running and detects the plane is on its side as you carry it.
crash_state.debounce_timer_ms = now_ms;
crash_state.debounce_time_total_ms = 0; // no debounce
}
crash_state.checkedHardLanding = true;
} else if (landing.is_on_approach()) {
// when altitude gets low, we automatically flare so ground crashes
// most likely can not be triggered from here. However,
// a crash into a tree would be caught here.
if (been_auto_flying) {
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
} else {
switch (flight_stage)
{
case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF:
if (g.takeoff_throttle_min_accel > 0 &&
!throttle_suppressed) {
// if you have an acceleration holding back throttle, but you met the
// accel threshold but still not fying, then you either shook/hit the
// plane or it was a failed launch.
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
// TODO: handle auto missions without NAV_TAKEOFF mission cmd
break;
case AP_Vehicle::FixedWing::FLIGHT_NORMAL:
if (!in_preLaunch_flight_stage() && been_auto_flying) {
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
break;
case AP_Vehicle::FixedWing::FLIGHT_VTOL:
// we need a totally new method for this
crashed = false;
break;
default:
break;
} // switch
}
} else {
crash_state.checkedHardLanding = false;
}
// if we have no GPS lock and we don't have a functional airspeed
// sensor then don't do crash detection
if (gps.status() < AP_GPS::GPS_OK_FIX_3D && (!airspeed.use() || !airspeed.healthy())) {
crashed = false;
}
if (!crashed) {
// reset timer
crash_state.debounce_timer_ms = 0;
} else if (crash_state.debounce_timer_ms == 0) {
// start timer
crash_state.debounce_timer_ms = now_ms;
} else if ((now_ms - crash_state.debounce_timer_ms >= crash_state.debounce_time_total_ms) && !crash_state.is_crashed) {
crash_state.is_crashed = true;
if (aparm.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
if (crashed_near_land_waypoint) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken");
} else {
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken");
}
}
else {
if (aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
disarm_motors();
}
if (crashed_near_land_waypoint) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected");
} else {
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash detected");
}
}
}
}
/*
check for pilot input on rudder stick for arming/disarming
*/
void Plane::rudder_arm_disarm_check()
{
AP_Arming::ArmingRudder arming_rudder = arming.get_rudder_arming_type();
if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) {
//parameter disallows rudder arming/disabling
return;
}
// if throttle is not down, then pilot cannot rudder arm/disarm
if (get_throttle_input() != 0){
rudder_arm_timer = 0;
return;
}
// if not in a manual throttle mode and not in CRUISE or FBWB
// modes then disallow rudder arming/disarming
if (auto_throttle_mode &&
(control_mode != CRUISE && control_mode != FLY_BY_WIRE_B)) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_rudder->get_control_in() > 4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if ((arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM) && !is_flying()) {
// when armed and not flying, full left rudder starts disarming counter
if (channel_rudder->get_control_in() < -4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
/*
* Determine if we have crashed
*/
void Plane::crash_detection_update(void)
{
if (control_mode != AUTO)
{
// crash detection is only available in AUTO mode
crash_state.debounce_timer_ms = 0;
return;
}
uint32_t now_ms = hal.scheduler->millis();
bool auto_launch_detected;
bool crashed_near_land_waypoint = false;
bool crashed = false;
bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
(now_ms - auto_state.started_flying_in_auto_ms >= 2500);
if (!is_flying())
{
switch (flight_stage)
{
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0);
if (been_auto_flying || // failed hand launch
auto_launch_detected) { // threshold of been_auto_flying may not be met on auto-launches
// has launched but is no longer flying. That's a crash on takeoff.
crashed = true;
}
break;
case AP_SpdHgtControl::FLIGHT_NORMAL:
if (been_auto_flying) {
crashed = true;
}
// TODO: handle auto missions without NAV_TAKEOFF mission cmd
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
if (been_auto_flying) {
crashed = true;
}
// when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL
// so ground crashes most likely can not be triggered from here. However,
// a crash into a tree, for example, would be.
break;
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
// We should be nice and level-ish in this flight stage. If not, we most
// likely had a crazy landing. Throttle is inhibited already at the flare
// but go ahead and notify GCS and perform any additional post-crash actions.
// Declare a crash if we are oriented more that 60deg in pitch or roll
if (been_auto_flying &&
!crash_state.checkHardLanding && // only check once
(fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) {
crashed = true;
// did we "crash" within 75m of the landing location? Probably just a hard landing
crashed_near_land_waypoint =
get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75;
// trigger hard landing event right away, or never again. This inhibits a false hard landing
// event when, for example, a minute after a good landing you pick the plane up and
// this logic is still running and detects the plane is on its side as you carry it.
crash_state.debounce_timer_ms = now_ms + 2500;
}
crash_state.checkHardLanding = true;
break;
} // switch
} else {
crash_state.checkHardLanding = false;
}
if (!crashed) {
// reset timer
crash_state.debounce_timer_ms = 0;
} else if (crash_state.debounce_timer_ms == 0) {
// start timer
crash_state.debounce_timer_ms = now_ms;
} else if ((now_ms - crash_state.debounce_timer_ms >= 2500) && !crash_state.is_crashed) {
crash_state.is_crashed = true;
if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
if (crashed_near_land_waypoint) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected - no action taken"));
} else {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected - no action taken"));
}
}
else {
if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
disarm_motors();
}
auto_state.land_complete = true;
if (crashed_near_land_waypoint) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected"));
} else {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected"));
}
}
}
}
