// 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); } }