/*
check for OBC failsafe check
*/
void Plane::obc_fs_check(void)
{
#if OBC_FAILSAFE == ENABLED
// perform OBC failsafe checks
obc.check(OBC_MODE(control_mode), failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
#endif
}
// 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
APM_OBC::check(APM_OBC::control_mode mode,
uint32_t last_heartbeat_ms,
uint32_t last_gps_fix_ms)
{
// we always check for fence breach
if (geofence_breached()) {
if (!_terminate) {
gcs_send_text_fmt(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) {
pinMode(_manual_pin, OUTPUT);
_last_manual_pin = _manual_pin;
}
digitalWrite(_manual_pin, mode==OBC_MANUAL);
}
uint32_t now = millis();
bool gcs_link_ok = ((now - last_heartbeat_ms) < 10000);
bool gps_lock_ok = ((now - last_gps_fix_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_send_text_fmt(PSTR("Starting OBC_AUTO"));
_state = STATE_AUTO;
}
break;
case STATE_AUTO:
// this is the normal mode.
if (!gcs_link_ok) {
gcs_send_text_fmt(PSTR("State DATA_LINK_LOSS"));
_state = STATE_DATA_LINK_LOSS;
if (_wp_comms_hold) {
if (_command_index != NULL) {
_saved_wp = _command_index->get();
}
change_command(_wp_comms_hold);
}
break;
}
if (!gps_lock_ok) {
gcs_send_text_fmt(PSTR("State GPS_LOSS"));
_state = STATE_GPS_LOSS;
if (_wp_gps_loss) {
if (_command_index != NULL) {
_saved_wp = _command_index->get();
}
change_command(_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
gcs_send_text_fmt(PSTR("Dual loss TERMINATE"));
_terminate.set(1);
} else if (gcs_link_ok) {
_state = STATE_AUTO;
gcs_send_text_fmt(PSTR("GCS OK"));
if (_saved_wp != 0) {
change_command(_saved_wp);
_saved_wp = 0;
}
}
break;
case STATE_GPS_LOSS:
if (!gcs_link_ok) {
// losing GCS link when GPS lock lost
// leads to termination
gcs_send_text_fmt(PSTR("Dual loss TERMINATE"));
_terminate.set(1);
} else if (gps_lock_ok) {
gcs_send_text_fmt(PSTR("GPS OK"));
_state = STATE_AUTO;
if (_saved_wp != 0) {
change_command(_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) {
pinMode(_heartbeat_pin, OUTPUT);
_last_heartbeat_pin = _heartbeat_pin;
}
_heartbeat_pin_value = !_heartbeat_pin_value;
digitalWrite(_heartbeat_pin, _heartbeat_pin_value);
}
// set the terminate pin
if (_terminate_pin != -1) {
if (_terminate_pin != _last_terminate_pin) {
pinMode(_terminate_pin, OUTPUT);
_last_terminate_pin = _terminate_pin;
}
digitalWrite(_terminate_pin, _terminate?HIGH:LOW);
}
}
/*
check for AFS failsafe check
*/
void Plane::afs_fs_check(void)
{
// perform AFS failsafe checks
afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
}
// update error mask of sensors and subsystems. The mask
// uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
// then it indicates that the sensor or subsystem is present but
// not functioning correctly.
void Plane::update_sensor_status_flags(void)
{
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
// first what sensors/controllers we have
if (g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
}
if (airspeed.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED
if (optflow.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
if (geofence_present()) {
control_sensors_present |= MAV_SYS_STATUS_GEOFENCE;
}
if (aparm.throttle_min < 0) {
control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
if (plane.DataFlash.logging_present()) { // primary logging only (usually File)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
}
// all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control, geofence and motor output which we will set individually
control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & ~MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & ~MAV_SYS_STATUS_SENSOR_YAW_POSITION & ~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_GEOFENCE & ~MAV_SYS_STATUS_LOGGING);
if (airspeed.enabled() && airspeed.use()) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
if (geofence_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
}
if (plane.DataFlash.logging_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
}
switch (control_mode) {
case MANUAL:
break;
case ACRO:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
break;
case STABILIZE:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case QSTABILIZE:
case QHOVER:
case QLAND:
case QLOITER:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
break;
case FLY_BY_WIRE_B:
case CRUISE:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
break;
case TRAINING:
if (!training_manual_roll || !training_manual_pitch) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
}
break;
case AUTO:
case RTL:
case LOITER:
case AVOID_ADSB:
case GUIDED:
case CIRCLE:
case QRTL:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; // altitude control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
break;
case INITIALISING:
break;
}
// set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
}
// default: all present sensors healthy except baro, 3D_MAG, GPS, DIFFERNTIAL_PRESSURE. GEOFENCE always defaults to healthy.
control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE |
MAV_SYS_STATUS_SENSOR_3D_MAG |
MAV_SYS_STATUS_SENSOR_GPS |
MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE);
control_sensors_health |= MAV_SYS_STATUS_GEOFENCE;
if (ahrs.initialised() && !ahrs.healthy()) {
// AHRS subsystem is unhealthy
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
}
if (ahrs.have_inertial_nav() && !ins.accel_calibrated_ok_all()) {
// trying to use EKF without properly calibrated accelerometers
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
}
if (barometer.all_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
}
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED
if (optflow.healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
}
if (!ins.get_accel_health_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
}
if (airspeed.healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
#if GEOFENCE_ENABLED
if (geofence_breached()) {
control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE;
}
#endif
if (plane.DataFlash.logging_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
}
if (millis() - failsafe.last_valid_rc_ms < 200) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
} else {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
#if AP_TERRAIN_AVAILABLE
switch (terrain.status()) {
case AP_Terrain::TerrainStatusDisabled:
break;
case AP_Terrain::TerrainStatusUnhealthy:
control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
break;
case AP_Terrain::TerrainStatusOK:
control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
control_sensors_health |= MAV_SYS_STATUS_TERRAIN;
break;
}
#endif
#if RANGEFINDER_ENABLED == ENABLED
if (rangefinder.num_sensors() > 0) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (g.rangefinder_landing) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
if (rangefinder.has_data()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
#endif
if (aparm.throttle_min < 0 && channel_throttle->get_servo_out() < 0) {
control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
if (AP_Notify::flags.initialising) {
// while initialising the gyros and accels are not enabled
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
#if FRSKY_TELEM_ENABLED == ENABLED
// give mask of error flags to Frsky_Telemetry
frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present);
#endif
}