// check for Failsafe conditions. This is called at 10Hz by the main
// ArduPlane code
void
APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms)
{
if (!_enable) {
return;
}
// we always check for fence breach
if(_enable_geofence_fs) {
if (geofence_breached || check_altlimit()) {
if (!_terminate) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Fence TERMINATE");
_terminate.set(1);
}
}
}
// check if RC termination is enabled
// check for RC failure in manual mode or RC failure when AFS_RC_MANUAL is 0
if (_state != STATE_PREFLIGHT && !_terminate && _enable_RC_fs &&
(mode == OBC_MANUAL || mode == OBC_FBW || !_rc_term_manual_only) &&
_rc_fail_time_seconds > 0 &&
(AP_HAL::millis() - last_valid_rc_ms) > (_rc_fail_time_seconds * 1000.0f)) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "RC failure terminate");
_terminate.set(1);
}
// tell the failsafe board if we are in manual control
// mode. This tells it to pass through controls from the
// receiver
if (_manual_pin != -1) {
hal.gpio->pinMode(_manual_pin, HAL_GPIO_OUTPUT);
hal.gpio->write(_manual_pin, mode==OBC_MANUAL);
}
uint32_t now = AP_HAL::millis();
bool gcs_link_ok = ((now - last_heartbeat_ms) < 10000);
bool gps_lock_ok = ((now - gps.last_fix_time_ms()) < 3000);
switch (_state) {
case STATE_PREFLIGHT:
// we startup in preflight mode. This mode ends when
// we first enter auto.
if (mode == OBC_AUTO) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Starting AFS_AUTO");
_state = STATE_AUTO;
}
break;
case STATE_AUTO:
// this is the normal mode.
if (!gcs_link_ok) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_ERROR, "State DATA_LINK_LOSS");
_state = STATE_DATA_LINK_LOSS;
if (_wp_comms_hold) {
_saved_wp = mission.get_current_nav_cmd().index;
mission.set_current_cmd(_wp_comms_hold);
}
// if two events happen within 30s we consider it to be part of the same event
if (now - _last_comms_loss_ms > 30*1000UL) {
_comms_loss_count++;
_last_comms_loss_ms = now;
}
break;
}
if (!gps_lock_ok) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "State GPS_LOSS");
_state = STATE_GPS_LOSS;
if (_wp_gps_loss) {
_saved_wp = mission.get_current_nav_cmd().index;
mission.set_current_cmd(_wp_gps_loss);
}
// if two events happen within 30s we consider it to be part of the same event
if (now - _last_gps_loss_ms > 30*1000UL) {
_gps_loss_count++;
_last_gps_loss_ms = now;
}
break;
}
break;
case STATE_DATA_LINK_LOSS:
if (!gps_lock_ok) {
// losing GPS lock when data link is lost
// leads to termination if AFS_DUAL_LOSS is 1
if(_enable_dual_loss) {
if (!_terminate) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Dual loss TERMINATE");
_terminate.set(1);
}
}
} else if (gcs_link_ok) {
_state = STATE_AUTO;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GCS OK");
// we only return to the mission if we have not exceeded AFS_MAX_COM_LOSS
if (_saved_wp != 0 &&
(_max_comms_loss <= 0 ||
_comms_loss_count <= _max_comms_loss)) {
mission.set_current_cmd(_saved_wp);
_saved_wp = 0;
}
}
break;
case STATE_GPS_LOSS:
if (!gcs_link_ok) {
// losing GCS link when GPS lock lost
// leads to termination if AFS_DUAL_LOSS is 1
if (!_terminate && _enable_dual_loss) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Dual loss TERMINATE");
_terminate.set(1);
}
} else if (gps_lock_ok) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS OK");
_state = STATE_AUTO;
// we only return to the mission if we have not exceeded AFS_MAX_GPS_LOSS
if (_saved_wp != 0 &&
(_max_gps_loss <= 0 || _gps_loss_count <= _max_gps_loss)) {
mission.set_current_cmd(_saved_wp);
_saved_wp = 0;
}
}
break;
}
// if we are not terminating or if there is a separate terminate
// pin configured then toggle the heartbeat pin at 10Hz
if (_heartbeat_pin != -1 && (_terminate_pin != -1 || !_terminate)) {
_heartbeat_pin_value = !_heartbeat_pin_value;
hal.gpio->pinMode(_heartbeat_pin, HAL_GPIO_OUTPUT);
hal.gpio->write(_heartbeat_pin, _heartbeat_pin_value);
}
// set the terminate pin
if (_terminate_pin != -1) {
hal.gpio->pinMode(_terminate_pin, HAL_GPIO_OUTPUT);
hal.gpio->write(_terminate_pin, _terminate?1:0);
}
}
// check for Failsafe conditions. This is called at 10Hz by the main
// ArduPlane code
void
APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms)
{
// we always check for fence breach
if (geofence_breached() || check_altlimit()) {
if (!_terminate) {
hal.console->println_P(PSTR("Fence TERMINATE"));
_terminate.set(1);
}
}
// tell the failsafe board if we are in manual control
// mode. This tells it to pass through controls from the
// receiver
if (_manual_pin != -1) {
if (_manual_pin != _last_manual_pin) {
hal.gpio->pinMode(_manual_pin, GPIO_OUTPUT);
_last_manual_pin = _manual_pin;
}
hal.gpio->write(_manual_pin, mode==OBC_MANUAL);
}
uint32_t now = hal.scheduler->millis();
bool gcs_link_ok = ((now - last_heartbeat_ms) < 10000);
bool gps_lock_ok = ((now - gps.last_fix_time_ms()) < 3000);
switch (_state) {
case STATE_PREFLIGHT:
// we startup in preflight mode. This mode ends when
// we first enter auto.
if (mode == OBC_AUTO) {
hal.console->println_P(PSTR("Starting OBC_AUTO"));
_state = STATE_AUTO;
}
break;
case STATE_AUTO:
// this is the normal mode.
if (!gcs_link_ok) {
hal.console->println_P(PSTR("State DATA_LINK_LOSS"));
_state = STATE_DATA_LINK_LOSS;
if (_wp_comms_hold) {
_saved_wp = mission.get_current_nav_cmd().index;
mission.set_current_cmd(_wp_comms_hold);
}
break;
}
if (!gps_lock_ok) {
hal.console->println_P(PSTR("State GPS_LOSS"));
_state = STATE_GPS_LOSS;
if (_wp_gps_loss) {
_saved_wp = mission.get_current_nav_cmd().index;
mission.set_current_cmd(_wp_gps_loss);
}
break;
}
break;
case STATE_DATA_LINK_LOSS:
if (!gps_lock_ok) {
// losing GPS lock when data link is lost
// leads to termination
hal.console->println_P(PSTR("Dual loss TERMINATE"));
_terminate.set(1);
} else if (gcs_link_ok) {
_state = STATE_AUTO;
hal.console->println_P(PSTR("GCS OK"));
if (_saved_wp != 0) {
mission.set_current_cmd(_saved_wp);
_saved_wp = 0;
}
}
break;
case STATE_GPS_LOSS:
if (!gcs_link_ok) {
// losing GCS link when GPS lock lost
// leads to termination
hal.console->println_P(PSTR("Dual loss TERMINATE"));
_terminate.set(1);
} else if (gps_lock_ok) {
hal.console->println_P(PSTR("GPS OK"));
_state = STATE_AUTO;
if (_saved_wp != 0) {
mission.set_current_cmd(_saved_wp);
_saved_wp = 0;
}
}
break;
}
// if we are not terminating or if there is a separate terminate
// pin configured then toggle the heartbeat pin at 10Hz
if (_heartbeat_pin != -1 && (_terminate_pin != -1 || !_terminate)) {
if (_heartbeat_pin != _last_heartbeat_pin) {
hal.gpio->pinMode(_heartbeat_pin, GPIO_OUTPUT);
_last_heartbeat_pin = _heartbeat_pin;
}
_heartbeat_pin_value = !_heartbeat_pin_value;
hal.gpio->write(_heartbeat_pin, _heartbeat_pin_value);
}
// set the terminate pin
if (_terminate_pin != -1) {
if (_terminate_pin != _last_terminate_pin) {
hal.gpio->pinMode(_terminate_pin, GPIO_OUTPUT);
_last_terminate_pin = _terminate_pin;
}
hal.gpio->write(_terminate_pin, _terminate?1:0);
}
}
// check for Failsafe conditions. This is called at 10Hz by the main
// ArduPlane code
void
AP_AdvancedFailsafe::check(uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms)
{
if (!_enable) {
return;
}
// only set the termination capability, clearing it can mess up copter and sub which can always be terminated
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION);
// we always check for fence breach
if(_enable_geofence_fs) {
if (geofence_breached || check_altlimit()) {
if (!_terminate) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terminating due to fence breach");
_terminate.set_and_notify(1);
}
}
}
enum control_mode mode = afs_mode();
// check if RC termination is enabled
// check for RC failure in manual mode or RC failure when AFS_RC_MANUAL is 0
if (_state != STATE_PREFLIGHT && !_terminate && _enable_RC_fs &&
(mode == AFS_MANUAL || mode == AFS_STABILIZED || !_rc_term_manual_only) &&
_rc_fail_time_seconds > 0 &&
(AP_HAL::millis() - last_valid_rc_ms) > (_rc_fail_time_seconds * 1000.0f)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terminating due to RC failure");
_terminate.set_and_notify(1);
}
// tell the failsafe board if we are in manual control
// mode. This tells it to pass through controls from the
// receiver
if (_manual_pin != -1) {
hal.gpio->pinMode(_manual_pin, HAL_GPIO_OUTPUT);
hal.gpio->write(_manual_pin, mode==AFS_MANUAL);
}
uint32_t now = AP_HAL::millis();
bool gcs_link_ok = ((now - last_heartbeat_ms) < 10000);
bool gps_lock_ok = ((now - gps.last_fix_time_ms()) < 3000);
switch (_state) {
case STATE_PREFLIGHT:
// we startup in preflight mode. This mode ends when
// we first enter auto.
if (mode == AFS_AUTO) {
gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO");
_state = STATE_AUTO;
}
break;
case STATE_AUTO:
// this is the normal mode.
if (!gcs_link_ok) {
gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: DATA_LINK_LOSS");
_state = STATE_DATA_LINK_LOSS;
if (_wp_comms_hold) {
_saved_wp = mission.get_current_nav_cmd().index;
mission.set_current_cmd(_wp_comms_hold);
}
// if two events happen within 30s we consider it to be part of the same event
if (now - _last_comms_loss_ms > 30*1000UL) {
_comms_loss_count++;
_last_comms_loss_ms = now;
}
break;
}
if (!gps_lock_ok) {
gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: GPS_LOSS");
_state = STATE_GPS_LOSS;
if (_wp_gps_loss) {
_saved_wp = mission.get_current_nav_cmd().index;
mission.set_current_cmd(_wp_gps_loss);
}
// if two events happen within 30s we consider it to be part of the same event
if (now - _last_gps_loss_ms > 30*1000UL) {
_gps_loss_count++;
_last_gps_loss_ms = now;
}
break;
}
break;
case STATE_DATA_LINK_LOSS:
if (!gps_lock_ok) {
// losing GPS lock when data link is lost
// leads to termination if AFS_DUAL_LOSS is 1
if(_enable_dual_loss) {
if (!_terminate) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terminating due to dual loss");
_terminate.set_and_notify(1);
}
}
} else if (gcs_link_ok) {
_state = STATE_AUTO;
gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO, GCS now OK");
// we only return to the mission if we have not exceeded AFS_MAX_COM_LOSS
if (_saved_wp != 0 &&
(_max_comms_loss <= 0 ||
_comms_loss_count <= _max_comms_loss)) {
mission.set_current_cmd(_saved_wp);
_saved_wp = 0;
}
}
break;
case STATE_GPS_LOSS:
if (!gcs_link_ok) {
// losing GCS link when GPS lock lost
// leads to termination if AFS_DUAL_LOSS is 1
if (!_terminate && _enable_dual_loss) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terminating due to dual loss");
_terminate.set_and_notify(1);
}
} else if (gps_lock_ok) {
gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO, GPS now OK");
_state = STATE_AUTO;
// we only return to the mission if we have not exceeded AFS_MAX_GPS_LOSS
if (_saved_wp != 0 &&
(_max_gps_loss <= 0 || _gps_loss_count <= _max_gps_loss)) {
mission.set_current_cmd(_saved_wp);
_saved_wp = 0;
}
}
break;
}
// if we are not terminating or if there is a separate terminate
// pin configured then toggle the heartbeat pin at 10Hz
if (_heartbeat_pin != -1 && (_terminate_pin != -1 || !_terminate)) {
_heartbeat_pin_value = !_heartbeat_pin_value;
hal.gpio->pinMode(_heartbeat_pin, HAL_GPIO_OUTPUT);
hal.gpio->write(_heartbeat_pin, _heartbeat_pin_value);
}
// set the terminate pin
if (_terminate_pin != -1) {
hal.gpio->pinMode(_terminate_pin, HAL_GPIO_OUTPUT);
hal.gpio->write(_terminate_pin, _terminate?1:0);
}
}
