// read all airspeed sensors
void AP_Airspeed::read(void)
{
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
read(i);
}
#if 1
// debugging until we get MAVLink support for 2nd airspeed sensor
if (enabled(1)) {
gcs().send_named_float("AS2", get_airspeed(1));
}
#endif
// setup primary
if (healthy(primary_sensor.get())) {
primary = primary_sensor.get();
return;
}
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (healthy(i)) {
primary = i;
break;
}
}
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (fabsf(_alt_offset - _alt_offset_active) > 0.1f) {
// if there's more than 10cm difference then slowly slew to it via LPF.
// The EKF does not like step inputs so this keeps it happy
_alt_offset_active = (0.9f*_alt_offset_active) + (0.1f*_alt_offset);
} else {
_alt_offset_active = _alt_offset;
}
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->update();
}
}
// consider a sensor as healthy if it has had an update in the
// last 0.5 seconds
uint32_t now = AP_HAL::millis();
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && !is_zero(sensors[i].pressure);
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
if (is_zero(sensors[i].ground_pressure)) {
sensors[i].ground_pressure = sensors[i].pressure;
}
float altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
// sanity check altitude
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
if (sensors[i].alt_ok) {
sensors[i].altitude = altitude + _alt_offset_active;
}
}
}
// ensure the climb rate filter is updated
if (healthy()) {
_climb_rate_filter.update(get_altitude(), get_last_update());
}
// choose primary sensor
if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
_primary = _primary_baro;
} else {
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
}
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->update();
}
}
// consider a sensor as healthy if it has had an update in the
// last 0.5 seconds
uint32_t now = AP_HAL::millis();
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && !is_zero(sensors[i].pressure);
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
if (is_zero(sensors[i].ground_pressure)) {
sensors[i].ground_pressure = sensors[i].pressure;
}
float altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
// sanity check altitude
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
if (sensors[i].alt_ok) {
sensors[i].altitude = altitude + _alt_offset;
}
}
}
// ensure the climb rate filter is updated
if (healthy()) {
_climb_rate_filter.update(get_altitude(), get_last_update());
}
// choose primary sensor
if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
_primary = _primary_baro;
} else {
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
}
}
// Update the filter status
void NavEKF2_core::updateFilterStatus(void)
{
// init return value
filterStatus.value = 0;
bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
// set individual flags
filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid
filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode
filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode
filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching
}
bool AP_InertialSensor::get_accel_health(uint8_t instance) const
{
if (instance != 0) {
return false;
}
return healthy();
}
/*
return true if all enabled sensors are healthy
*/
bool AP_Airspeed::all_healthy(void) const
{
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (enabled(i) && !healthy(i)) {
return false;
}
}
return true;
}
/*
check if all barometers are healthy
*/
bool AP_Baro::all_healthy(void) const
{
for (uint8_t i=0; i<_num_sensors; i++) {
if (!healthy(i)) {
return false;
}
}
return _num_sensors > 0;
}
gps_fix_t Gps_mocap::fix(void) const
{
gps_fix_t fix = NO_GPS;
if (healthy())
{
fix = RTK;
}
return fix;
}
uint8_t
Compass::get_healthy_mask() const
{
uint8_t healthy_mask = 0;
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if(healthy(i)) {
healthy_mask |= 1 << i;
}
}
return healthy_mask;
}
bool
Compass::read(void)
{
for (uint8_t i=0; i< _backend_count; i++) {
// call read on each of the backend. This call updates field[i]
_backends[i]->read();
}
for (uint8_t i=0; i < COMPASS_MAX_INSTANCES; i++) {
_state[i].healthy = (AP_HAL::millis() - _state[i].last_update_ms < 500);
}
return healthy();
}
bool
Compass::start_calibration_all(bool retry, bool autosave, float delay)
{
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if (healthy(i) && use_for_yaw(i)) {
if (!start_calibration(i,retry,autosave,delay)) {
cancel_calibration_all();
return false;
}
}
}
return true;
}
/*
update the barometer calibration
this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro::update_calibration()
{
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
sensors[i].ground_pressure.set_and_notify(get_pressure(i));
}
float last_temperature = sensors[i].ground_temperature;
sensors[i].ground_temperature.set_and_notify(get_calibration_temperature(i));
if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) {
// reset _EAS2TAS to force it to recalculate. This happens
// when a digital airspeed sensor comes online
_EAS2TAS = 0;
}
}
}
void Database_Login::optimize ()
{
if (!healthy ()) {
// (Re)create damaged or non-existing database.
filter_url_unlink (database_sqlite_file (database ()));
create ();
}
// Vacuum it.
SqliteDatabase sql (database ());
// On Android, this pragma prevents the following error: VACUUM; Unable to open database file.
sql.add ("PRAGMA temp_store = MEMORY;");
sql.execute ();
sql.clear ();
sql.add ("VACUUM;");
sql.execute ();
}
bool
Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay)
{
if (healthy(i)) {
if (!is_calibrating() && delay > 0.5f) {
AP_Notify::events.initiated_compass_cal = 1;
}
if (i == get_primary()) {
_calibrator[i].set_tolerance(8);
} else {
_calibrator[i].set_tolerance(16);
}
_calibrator[i].start(retry, autosave, delay);
return true;
} else {
return false;
}
}
/*
update the barometer calibration
this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro::update_calibration()
{
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
sensors[i].ground_pressure.set(get_pressure(i));
}
// don't notify the GCS too rapidly or we flood the link
uint32_t now = AP_HAL::millis();
if (now - _last_notify_ms > 10000) {
sensors[i].ground_pressure.notify();
_last_notify_ms = now;
}
}
// always update the guessed ground temp
_guessed_ground_temperature = get_external_temperature();
// force EAS2TAS to recalculate
_EAS2TAS = 0;
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->update();
}
}
// consider a sensor as healthy if it has had an update in the
// last 0.5 seconds
uint32_t now = hal.scheduler->millis();
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && sensors[i].pressure != 0;
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
if (sensors[i].ground_pressure == 0) {
sensors[i].ground_pressure = sensors[i].pressure;
}
sensors[i].altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
// sanity check altitude
sensors[i].alt_ok = !(isnan(sensors[i].altitude) || isinf(sensors[i].altitude));
}
}
// choose primary sensor
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
// ensure the climb rate filter is updated
_climb_rate_filter.update(get_altitude(), get_last_update());
}
/*
update the barometer calibration
this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro::update_calibration()
{
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
sensors[i].ground_pressure.set(get_pressure(i));
}
float last_temperature = sensors[i].ground_temperature;
sensors[i].ground_temperature.set(get_calibration_temperature(i));
// don't notify the GCS too rapidly or we flood the link
uint32_t now = AP_HAL::millis();
if (now - _last_notify_ms > 10000) {
sensors[i].ground_pressure.notify();
sensors[i].ground_temperature.notify();
_last_notify_ms = now;
}
if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) {
// reset _EAS2TAS to force it to recalculate. This happens
// when a digital airspeed sensor comes online
_EAS2TAS = 0;
}
}
}
// calibrate the barometer. This must be called at least once before
// the altitude() or climb_rate() interfaces can be used
void AP_Baro::calibrate()
{
// reset the altitude offset when we calibrate. The altitude
// offset is supposed to be for within a flight
_alt_offset.set_and_save(0);
// start by assuming all sensors are calibrated (for healthy() test)
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].calibrated = true;
sensors[i].alt_ok = true;
}
// let the barometer settle for a full second after startup
// the MS5611 reads quite a long way off for the first second,
// leading to about 1m of error if we don't wait
for (uint8_t i = 0; i < 10; i++) {
uint32_t tstart = AP_HAL::millis();
do {
update();
if (AP_HAL::millis() - tstart > 500) {
AP_HAL::panic("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [2]\r\n");
}
hal.scheduler->delay(10);
} while (!healthy());
hal.scheduler->delay(100);
}
// now average over 5 values for the ground pressure and
// temperature settings
float sum_pressure[BARO_MAX_INSTANCES] = {0};
float sum_temperature[BARO_MAX_INSTANCES] = {0};
uint8_t count[BARO_MAX_INSTANCES] = {0};
const uint8_t num_samples = 5;
for (uint8_t c = 0; c < num_samples; c++) {
uint32_t tstart = AP_HAL::millis();
do {
update();
if (AP_HAL::millis() - tstart > 500) {
AP_HAL::panic("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [3]\r\n");
}
} while (!healthy());
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
sum_pressure[i] += sensors[i].pressure;
sum_temperature[i] += sensors[i].temperature;
count[i] += 1;
}
}
hal.scheduler->delay(100);
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (count[i] == 0) {
sensors[i].calibrated = false;
} else {
sensors[i].ground_pressure.set_and_save(sum_pressure[i] / count[i]);
sensors[i].ground_temperature.set_and_save(sum_temperature[i] / count[i]);
}
}
// panic if all sensors are not calibrated
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].calibrated) {
return;
}
}
AP_HAL::panic("AP_Baro: all sensors uncalibrated");
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (fabsf(_alt_offset - _alt_offset_active) > 0.01f) {
// If there's more than 1cm difference then slowly slew to it via LPF.
// The EKF does not like step inputs so this keeps it happy.
_alt_offset_active = (0.95f*_alt_offset_active) + (0.05f*_alt_offset);
} else {
_alt_offset_active = _alt_offset;
}
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->backend_update(i);
}
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
float ground_pressure = sensors[i].ground_pressure;
if (!is_positive(ground_pressure) || isnan(ground_pressure) || isinf(ground_pressure)) {
sensors[i].ground_pressure = sensors[i].pressure;
}
float altitude = sensors[i].altitude;
if (sensors[i].type == BARO_TYPE_AIR) {
float pressure = sensors[i].pressure + sensors[i].p_correction;
altitude = get_altitude_difference(sensors[i].ground_pressure, pressure);
} else if (sensors[i].type == BARO_TYPE_WATER) {
//101325Pa is sea level air pressure, 9800 Pascal/ m depth in water.
//No temperature or depth compensation for density of water.
altitude = (sensors[i].ground_pressure - sensors[i].pressure) / 9800.0f / _specific_gravity;
}
// sanity check altitude
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
if (sensors[i].alt_ok) {
sensors[i].altitude = altitude + _alt_offset_active;
}
}
if (_hil.have_alt) {
sensors[0].altitude = _hil.altitude;
}
if (_hil.have_last_update) {
sensors[0].last_update_ms = _hil.last_update_ms;
}
}
// ensure the climb rate filter is updated
if (healthy()) {
_climb_rate_filter.update(get_altitude(), get_last_update());
}
// choose primary sensor
if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
_primary = _primary_baro;
} else {
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
}
}
// return the estimated height of body frame origin above ground level
bool NavEKF3_core::getHAGL(float &HAGL) const
{
HAGL = terrainState - outputDataNew.position.z - posOffsetNED.z;
// If we know the terrain offset and altitude, then we have a valid height above ground estimate
return !hgtTimeout && gndOffsetValid && healthy();
}
/// return true if the compass should be used for yaw calculations
bool
Compass::use_for_yaw(void) const
{
uint8_t prim = get_primary();
return healthy(prim) && use_for_yaw(prim);
}
