/* 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 }