// 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); } } }
// 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 occured // 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; // This is how to handle a failsafe. // use the throttle failsafe setting to decide what to do switch(control_mode) { case STABILIZE: case ACRO: case SPORT: // if throttle is zero disarm motors if (ap.throttle_zero || ap.land_complete) { init_disarm_motors(); }else if(home_distance > wp_nav.get_wp_radius()) { // switch to RTL or if that fails, LAND set_mode_RTL_or_land_with_pause(); }else{ // We have no GPS or are very close to home so we will land set_mode_land_with_pause(); } break; case AUTO: // if mission has not started AND vehicle is landed, disarm motors if (!ap.auto_armed && ap.land_complete) { init_disarm_motors(); // if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) { if (home_distance > wp_nav.get_wp_radius()) { // switch to RTL or if that fails, LAND set_mode_RTL_or_land_with_pause(); }else{ // We are very close to home so we will land set_mode_land_with_pause(); } } // if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything break; default: // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold // if landed disarm if (ap.land_complete) { init_disarm_motors(); } else if (home_distance > wp_nav.get_wp_radius()) { // switch to RTL or if that fails, LAND set_mode_RTL_or_land_with_pause(); }else{ // We have no GPS or are very close to home so we will land set_mode_land_with_pause(); } break; } }