2011-03-19 07:20:11 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2011-02-18 23:59:58 -04:00
2011-02-17 03:28:49 -04:00
/*
2012-08-16 21:50:02 -03:00
* This event will be called when the failsafe changes
* boolean failsafe reflects the current state
*/
2013-03-16 05:14:21 -03:00
static void failsafe_radio_on_event()
2011-02-17 03:28:49 -04:00
{
2012-11-13 10:43:54 -04:00
// if motors are not armed there is nothing to do
if( !motors.armed() ) {
return;
}
2012-08-16 21:50:02 -03:00
// This is how to handle a failsafe.
2012-11-11 09:11:12 -04:00
switch(control_mode) {
case STABILIZE:
case ACRO:
2014-07-31 02:58:34 -03:00
// if throttle is zero OR vehicle is landed disarm motors
2014-10-07 12:29:24 -03:00
if (ap.throttle_zero || ap.land_complete) {
2012-11-11 09:11:12 -04:00
init_disarm_motors();
2014-07-31 02:58:34 -03:00
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
2013-05-01 21:39:44 -03:00
}else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2014-07-31 02:58:34 -03:00
// if far from home then RTL
2014-01-21 22:42:16 -04:00
}else if(home_distance > wp_nav.get_wp_radius()) {
2014-12-14 23:48:54 -04:00
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
2014-07-31 02:58:34 -03:00
// We have no GPS or are very close to home so we will land
2012-11-11 09:11:12 -04:00
}else{
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2012-11-11 09:11:12 -04:00
}
break;
2014-07-31 02:58:34 -03:00
2012-11-11 09:11:12 -04:00
case AUTO:
2014-07-31 02:58:34 -03:00
// if mission has not started AND vehicle is landed, disarm motors
if (!ap.auto_armed && ap.land_complete) {
init_disarm_motors();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
} else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_RTL do RTL
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
2014-01-21 22:42:16 -04:00
if(home_distance > wp_nav.get_wp_radius()) {
2014-12-14 23:48:54 -04:00
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
2012-12-21 01:39:48 -04:00
}else{
// We are very close to home so we will land
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2012-12-21 01:39:48 -04:00
}
2013-11-18 02:51:17 -04:00
}
2014-07-31 02:58:34 -03:00
// failsafe_throttle must be FS_THR_ENABLED_CONTINUE_MISSION so no need to do anything
2013-11-18 02:51:17 -04:00
break;
2014-07-31 02:58:34 -03:00
2013-07-13 07:49:25 -03:00
case LAND:
// continue to land if battery failsafe is also active otherwise fall through to default handling
2013-11-16 01:46:57 -04:00
if (g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
2013-07-13 07:49:25 -03:00
break;
}
2014-07-17 00:10:45 -03:00
// no break
2012-11-11 09:11:12 -04:00
default:
2014-12-06 03:35:23 -04:00
// used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
2014-07-31 02:58:34 -03:00
// if landed disarm
if (ap.land_complete) {
init_disarm_motors();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
} else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2014-07-31 02:58:34 -03:00
// if far from home then RTL
2014-01-21 22:42:16 -04:00
}else if(home_distance > wp_nav.get_wp_radius()) {
2014-12-14 23:48:54 -04:00
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
2012-11-11 09:11:12 -04:00
}else{
2012-12-21 01:39:48 -04:00
// We have no GPS or are very close to home so we will land
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2012-11-11 09:11:12 -04:00
}
break;
2012-08-16 21:50:02 -03:00
}
2012-12-29 23:08:25 -04:00
// log the error to the dataflash
2013-03-16 05:14:21 -03:00
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_OCCURRED);
2012-12-29 23:08:25 -04:00
2011-03-13 03:25:38 -03:00
}
2011-02-17 03:28:49 -04:00
2012-11-11 09:11:12 -04:00
// failsafe_off_event - respond to radio contact being regained
// we must be in AUTO, LAND or RTL modes
// or Stabilize or ACRO mode but with motors disarmed
2013-03-16 05:14:21 -03:00
static void failsafe_radio_off_event()
2011-03-13 03:25:38 -03:00
{
2012-12-29 23:08:25 -04:00
// no need to do anything except log the error as resolved
2012-11-11 09:11:12 -04:00
// user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
2013-03-16 05:14:21 -03:00
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_RESOLVED);
2011-02-17 03:28:49 -04:00
}
2013-10-13 01:52:52 -03:00
static void failsafe_battery_event(void)
2011-02-17 03:28:49 -04:00
{
2013-10-01 10:34:44 -03:00
// return immediately if low battery event has already been triggered
2013-10-13 01:52:52 -03:00
if (failsafe.battery) {
2013-10-01 10:34:44 -03:00
return;
}
2012-11-13 10:43:54 -04:00
// failsafe check
2013-11-16 01:46:57 -04:00
if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
2012-11-13 10:43:54 -04:00
switch(control_mode) {
case STABILIZE:
case ACRO:
2014-07-31 02:58:34 -03:00
// if throttle is zero OR vehicle is landed disarm motors
2014-10-07 12:29:24 -03:00
if (ap.throttle_zero || ap.land_complete) {
2012-11-13 10:43:54 -04:00
init_disarm_motors();
}else{
2013-11-16 01:46:57 -04:00
// set mode to RTL or LAND
2014-01-21 22:42:16 -04:00
if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
2014-12-14 23:48:54 -04:00
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
2013-11-16 01:46:57 -04:00
}else{
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2013-11-16 01:46:57 -04:00
}
2012-11-13 10:43:54 -04:00
}
break;
2013-11-16 01:55:49 -04:00
case AUTO:
2014-07-31 02:58:34 -03:00
// if mission has not started AND vehicle is landed, disarm motors
if (!ap.auto_armed && ap.land_complete) {
init_disarm_motors();
2013-11-16 01:55:49 -04:00
// set mode to RTL or LAND
2014-07-31 02:58:34 -03:00
} else if (home_distance > wp_nav.get_wp_radius()) {
2014-12-14 23:48:54 -04:00
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
2014-07-31 02:58:34 -03:00
} else {
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2013-11-16 01:55:49 -04:00
}
break;
2013-11-16 01:46:57 -04:00
default:
2014-12-06 03:35:23 -04:00
// used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
2014-07-31 02:58:34 -03:00
// if landed disarm
if (ap.land_complete) {
init_disarm_motors();
2013-11-16 01:46:57 -04:00
// set mode to RTL or LAND
2014-07-31 02:58:34 -03:00
} else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
2014-12-14 23:48:54 -04:00
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
2014-07-31 02:58:34 -03:00
} else {
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2012-12-22 05:16:50 -04:00
}
2012-11-13 10:43:54 -04:00
break;
}
2012-06-04 22:31:40 -03:00
}
2012-11-13 10:43:54 -04:00
// set the low battery flag
2013-10-13 01:52:52 -03:00
set_failsafe_battery(true);
2012-11-13 10:43:54 -04:00
2012-12-29 23:08:25 -04:00
// warn the ground station and log to dataflash
2014-09-21 05:33:19 -03:00
gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
2013-03-16 05:14:21 -03:00
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
2012-12-29 23:08:25 -04:00
2011-02-17 03:28:49 -04:00
}
2013-03-16 05:27:46 -03:00
// failsafe_gps_check - check for gps failsafe
static void failsafe_gps_check()
{
uint32_t last_gps_update_ms;
2013-11-11 22:13:05 -04:00
// return immediately if gps failsafe is disabled or we have never had GPS lock
2015-02-09 07:25:04 -04:00
if (g.failsafe_gps_enabled == FS_GPS_DISABLED || !home_is_set()) {
2013-11-11 09:24:18 -04:00
// if we have just disabled the gps failsafe, ensure the gps failsafe event is cleared
if (failsafe.gps) {
failsafe_gps_off_event();
set_failsafe_gps(false);
}
2013-03-16 05:27:46 -03:00
return;
}
// calc time since last gps update
2013-09-22 10:21:57 -03:00
last_gps_update_ms = millis() - gps_glitch.last_good_update();
2013-03-16 05:27:46 -03:00
// check if all is well
if( last_gps_update_ms < FAILSAFE_GPS_TIMEOUT_MS) {
// check for recovery from gps failsafe
2013-09-26 05:54:33 -03:00
if( failsafe.gps ) {
2013-03-16 05:27:46 -03:00
failsafe_gps_off_event();
set_failsafe_gps(false);
}
return;
}
// do nothing if gps failsafe already triggered or motors disarmed
2013-09-26 05:54:33 -03:00
if( failsafe.gps || !motors.armed()) {
2013-03-16 05:27:46 -03:00
return;
}
// GPS failsafe event has occured
// update state, warn the ground station and log to dataflash
set_failsafe_gps(true);
2014-09-21 05:33:19 -03:00
gcs_send_text_P(SEVERITY_HIGH,PSTR("Lost GPS!"));
2013-03-16 05:27:46 -03:00
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_OCCURRED);
2013-11-16 00:55:59 -04:00
// take action based on flight mode and FS_GPS_ENABLED parameter
if (mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
if (g.failsafe_gps_enabled == FS_GPS_ALTHOLD && !failsafe.radio) {
set_mode(ALT_HOLD);
2014-12-14 23:48:54 -04:00
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
2013-11-16 00:55:59 -04:00
}else{
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2013-11-16 00:55:59 -04:00
}
2013-06-25 23:56:58 -03:00
}
2014-04-27 03:08:58 -03:00
// if flight mode is LAND ensure it's not the GPS controlled LAND
if (control_mode == LAND) {
land_do_not_use_GPS();
}
2013-03-16 05:27:46 -03:00
}
// failsafe_gps_off_event - actions to take when GPS contact is restored
static void failsafe_gps_off_event(void)
{
// log recovery of GPS in logs?
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_RESOLVED);
}
2011-02-17 03:28:49 -04:00
2013-04-29 09:30:22 -03:00
// failsafe_gcs_check - check for ground station failsafe
static void failsafe_gcs_check()
{
uint32_t last_gcs_update_ms;
2014-12-31 21:11:28 -04:00
// 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.
2015-01-02 13:46:41 -04:00
if ((!failsafe.gcs)&&(g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!failsafe.rc_override_active && control_mode != GUIDED))) {
2013-04-29 09:30:22 -03:00
return;
}
2013-07-20 08:44:56 -03:00
// calc time since last gcs update
2015-01-02 13:46:41 -04:00
// note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
2013-09-26 05:54:33 -03:00
last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
2013-04-29 09:30:22 -03:00
// check if all is well
2015-01-02 13:46:41 -04:00
if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
2013-07-20 08:44:56 -03:00
// check for recovery from gcs failsafe
2013-09-26 05:54:33 -03:00
if (failsafe.gcs) {
2013-04-29 09:30:22 -03:00
failsafe_gcs_off_event();
set_failsafe_gcs(false);
}
return;
}
2013-07-20 08:44:56 -03:00
// do nothing if gcs failsafe already triggered or motors disarmed
2015-01-02 13:46:41 -04:00
if (failsafe.gcs || !motors.armed()) {
2013-04-29 09:30:22 -03:00
return;
}
// GCS failsafe event has occured
// update state, log to dataflash
set_failsafe_gcs(true);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED);
2014-04-09 12:30:26 -03:00
// clear overrides so that RC control can be regained with radio.
hal.rcin->clear_overrides();
failsafe.rc_override_active = false;
2013-04-29 09:30:22 -03:00
// This is how to handle a failsafe.
// use the throttle failsafe setting to decide what to do
switch(control_mode) {
case STABILIZE:
case ACRO:
2013-08-04 03:56:35 -03:00
case SPORT:
2013-04-29 09:30:22 -03:00
// if throttle is zero disarm motors
2014-10-07 12:29:24 -03:00
if (ap.throttle_zero) {
2013-04-29 09:30:22 -03:00
init_disarm_motors();
2014-01-21 22:42:16 -04:00
}else if(home_distance > wp_nav.get_wp_radius()) {
2014-12-14 23:48:54 -04:00
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
2013-04-29 09:30:22 -03:00
}else{
// We have no GPS or are very close to home so we will land
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2013-04-29 09:30:22 -03:00
}
break;
case AUTO:
// if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
2014-01-21 22:42:16 -04:00
if (home_distance > wp_nav.get_wp_radius()) {
2014-12-14 23:48:54 -04:00
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
2013-04-29 09:30:22 -03:00
}else{
// We are very close to home so we will land
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2013-04-29 09:30:22 -03:00
}
}
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break;
default:
2014-01-21 22:42:16 -04:00
if(home_distance > wp_nav.get_wp_radius()) {
2014-12-14 23:48:54 -04:00
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
2013-04-29 09:30:22 -03:00
}else{
// We have no GPS or are very close to home so we will land
2014-07-06 06:16:27 -03:00
set_mode_land_with_pause();
2013-04-29 09:30:22 -03:00
}
break;
}
}
2013-07-20 08:44:56 -03:00
// failsafe_gcs_off_event - actions to take when GCS contact is restored
2013-04-29 09:30:22 -03:00
static void failsafe_gcs_off_event(void)
{
2013-07-20 08:44:56 -03:00
// log recovery of GCS in logs?
2013-04-29 09:30:22 -03:00
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
}
2014-12-14 23:48:54 -04:00
// set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
static void set_mode_RTL_or_land_with_pause()
{
// attempt to switch to RTL, if this fails then switch to Land
if (!set_mode(RTL)) {
// set mode to land will trigger mode change notification to pilot
set_mode_land_with_pause();
} else {
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
}
}
2014-01-20 01:05:31 -04:00
static void update_events()
2011-02-17 03:28:49 -04:00
{
2014-01-20 01:05:31 -04:00
ServoRelayEvents.update_events();
2011-02-17 03:28:49 -04:00
}