/*
* This event will be called when the failsafe changes
* boolean failsafe reflects the current state
*/
void Copter::failsafe_radio_on_event()
{
// if motors are not armed there is nothing to do
if( !motors.armed() ) {
return;
}
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
// continue landing
} else {
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
} else {
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
}
}
}
// log the error to the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_OCCURRED);
}
void Copter::failsafe_battery_event(void)
{
// return immediately if low battery event has already been triggered
if (failsafe.battery) {
return;
}
// failsafe check
if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) {
set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
} else {
set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
}
}
}
// set the low battery flag
set_failsafe_battery(true);
// warn the ground station and log to dataflash
gcs_send_text(MAV_SEVERITY_WARNING,"Low battery");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
}
// failsafe_gcs_check - check for ground station failsafe
void Copter::failsafe_gcs_check()
{
uint32_t last_gcs_update_ms;
// return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs and we are not in guided mode
// this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
if ((!failsafe.gcs)&&(g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!failsafe.rc_override_active && control_mode != GUIDED))) {
return;
}
// calc time since last gcs update
// note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
// check if all is well
if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
// check for recovery from gcs failsafe
if (failsafe.gcs) {
failsafe_gcs_off_event();
set_failsafe_gcs(false);
}
return;
}
// do nothing if gcs failsafe already triggered or motors disarmed
if (failsafe.gcs || !motors->armed()) {
return;
}
// GCS failsafe event has occurred
// update state, log to dataflash
set_failsafe_gcs(true);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED);
// clear overrides so that RC control can be regained with radio.
hal.rcin->clear_overrides();
failsafe.rc_override_active = false;
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (control_mode == AUTO && g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(MODE_REASON_GCS_FAILSAFE);
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE);
} else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL
set_mode_RTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE);
}
}
}
// terrain failsafe action
void Copter::failsafe_terrain_on_event()
{
failsafe.terrain = true;
gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED);
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else if (control_mode == RTL) {
mode_rtl.restart_without_terrain();
} else {
set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE);
}
}
