diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index b45a26df25..eb1e9dd18b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -647,6 +647,14 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @User: Advanced AP_GROUPINFO("ERR_THRESH", 61, NavEKF3, _err_thresh, 0.2), + // @Param: AFFINITY + // @DisplayName: EKF3 Sensor Affinity Options + // @Description: These options control the affinity between sensor instances and EKF cores + // @User: Advanced + // @Bitmask: 0:EnableGPSAffinity,1:EnableBaroAffinity,2:EnableCompassAffinity,3:EnableAirspeedAffinity + // @RebootRequired: True + AP_GROUPINFO("AFFINITY", 62, NavEKF3, _affinity, 0), + AP_GROUPEND }; @@ -1202,6 +1210,39 @@ uint8_t NavEKF3::getActiveMag(int8_t instance) const } } +// return the baro in use for the specified instance +uint8_t NavEKF3::getActiveBaro(int8_t instance) const +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + return core[instance].getActiveBaro(); + } else { + return 255; + } +} + +// return the GPS in use for the specified instance +uint8_t NavEKF3::getActiveGPS(int8_t instance) const +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + return core[instance].getActiveGPS(); + } else { + return 255; + } +} + +// return the airspeed sensor in use for the specified instance +uint8_t NavEKF3::getActiveAirspeed(int8_t instance) const +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + return core[instance].getActiveAirspeed(); + } else { + return 255; + } +} + // Return estimated magnetometer offsets // Return true if magnetometer offsets are valid bool NavEKF3::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index c64de2f7e9..0b53c283fb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -152,9 +152,12 @@ public: // An out of range instance (eg -1) returns data for the primary instance void getMagXYZ(int8_t instance, Vector3f &magXYZ) const; - // return the magnetometer in use for the specified instance + // return the sensor in use for the specified instance // An out of range instance (eg -1) returns data for the primary instance uint8_t getActiveMag(int8_t instance) const; + uint8_t getActiveBaro(int8_t instance) const; + uint8_t getActiveGPS(int8_t instance) const; + uint8_t getActiveAirspeed(int8_t instance) const; // Return estimated magnetometer offsets // Return true if magnetometer offsets are valid @@ -506,6 +509,7 @@ private: AP_Int16 _gsfResetDelay; // number of mSec from loss of navigation to requesting a reset using EKF-GSF yaw estimator data AP_Int8 _gsfResetMaxCount; // maximum number of times the EKF3 is allowed to reset it's yaw to the EKF-GSF estimate AP_Float _err_thresh; // lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError + AP_Int32 _affinity; // bitmask of sensor affinity options // Possible values for _flowUse #define FLOW_USE_NONE 0 @@ -616,6 +620,7 @@ private: void Log_Write_XKF3(uint8_t core, uint64_t time_us) const; void Log_Write_XKF4(uint8_t core, uint64_t time_us) const; void Log_Write_XKF5(uint64_t time_us) const; + void Log_Write_XKFS(uint8_t core, uint64_t time_us) const; void Log_Write_Quaternion(uint8_t core, uint64_t time_us) const; void Log_Write_Beacon(uint64_t time_us) const; void Log_Write_BodyOdom(uint64_t time_us) const; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 4128b96b45..be1af5922e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -51,7 +51,6 @@ void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const Vector3f wind; Vector3f magNED; Vector3f magXYZ; - uint8_t magIndex = getActiveMag(_core); getAccelBias(_core,accelBias); getWind(_core,wind); getMagNED(_core,magNED); @@ -70,12 +69,30 @@ void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const magD : (int16_t)(magNED.z), magX : (int16_t)(magXYZ.x), magY : (int16_t)(magXYZ.y), - magZ : (int16_t)(magXYZ.z), - index : (uint8_t)(magIndex) + magZ : (int16_t)(magXYZ.z) }; AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); } +void NavEKF3::Log_Write_XKFS(uint8_t _core, uint64_t time_us) const +{ + // Write sensor selection EKF packet + uint8_t magIndex = getActiveMag(_core); + uint8_t baroIndex = getActiveBaro(_core); + uint8_t GPSIndex = getActiveGPS(_core); + uint8_t airspeedIndex = getActiveAirspeed(_core); + const struct log_EKFS pkt { + LOG_PACKET_HEADER_INIT(LOG_XKFS_MSG), + time_us : time_us, + core : _core, + mag_index : (uint8_t)(magIndex), + baro_index : (uint8_t)(baroIndex), + gps_index : (uint8_t)(GPSIndex), + airspeed_index : (uint8_t)(airspeedIndex), + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const { // Write third EKF packet @@ -318,6 +335,7 @@ void NavEKF3::Log_Write() Log_Write_XKF2(i, time_us); Log_Write_XKF3(i, time_us); Log_Write_XKF4(i, time_us); + Log_Write_XKFS(i, time_us); Log_Write_Quaternion(i, time_us); Log_Write_GSF(i, time_us); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 8385210973..26e2865e61 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -285,7 +285,10 @@ void NavEKF3_core::readMagData() // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets - if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) { + if (magTimeout && (maxCount > 1) && + !onGround && + imuSampleTime_ms - ekfStartTime_ms > 30000 && + !(frontend->_affinity & EKF_AFFINITY_MAG)) { // search through the list of magnetometers for (uint8_t i=1; i frontend->sensorIntervalMin_ms) { - if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { + if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) { + if (gps.status(selected_gps) >= AP_GPS::GPS_OK_FIX_3D) { // report GPS fix status gpsCheckStatus.bad_fix = false; @@ -532,13 +535,13 @@ void NavEKF3_core::readGpsData() secondLastGpsTime_ms = lastTimeGpsReceived_ms; // get current fix time - lastTimeGpsReceived_ms = gps.last_message_time_ms(); + lastTimeGpsReceived_ms = gps.last_message_time_ms(selected_gps); // estimate when the GPS fix was valid, allowing for GPS processing and other delays // ideally we should be using a timing signal from the GPS receiver to set this time // Use the driver specified delay float gps_delay_sec = 0; - gps.get_lag(gps_delay_sec); + gps.get_lag(selected_gps, gps_delay_sec); gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f); // Correct for the average intersampling delay due to the filter updaterate @@ -548,17 +551,17 @@ void NavEKF3_core::readGpsData() gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.time_ms); // Get which GPS we are using for position information - gpsDataNew.sensor_idx = gps.primary_sensor(); + gpsDataNew.sensor_idx = selected_gps; // read the NED velocity from the GPS - gpsDataNew.vel = gps.velocity(); + gpsDataNew.vel = gps.velocity(selected_gps); // Use the speed and position accuracy from the GPS if available, otherwise set it to zero. // Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f); gpsSpdAccuracy *= (1.0f - alpha); float gpsSpdAccRaw; - if (!gps.speed_accuracy(gpsSpdAccRaw)) { + if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) { gpsSpdAccuracy = 0.0f; } else { gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw); @@ -567,7 +570,7 @@ void NavEKF3_core::readGpsData() } gpsPosAccuracy *= (1.0f - alpha); float gpsPosAccRaw; - if (!gps.horizontal_accuracy(gpsPosAccRaw)) { + if (!gps.horizontal_accuracy(selected_gps, gpsPosAccRaw)) { gpsPosAccuracy = 0.0f; } else { gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw); @@ -576,7 +579,7 @@ void NavEKF3_core::readGpsData() } gpsHgtAccuracy *= (1.0f - alpha); float gpsHgtAccRaw; - if (!gps.vertical_accuracy(gpsHgtAccRaw)) { + if (!gps.vertical_accuracy(selected_gps, gpsHgtAccRaw)) { gpsHgtAccuracy = 0.0f; } else { gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw); @@ -585,16 +588,16 @@ void NavEKF3_core::readGpsData() } // check if we have enough GPS satellites and increase the gps noise scaler if we don't - if (gps.num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) { + if (gps.num_sats(selected_gps) >= 6 && (PV_AidingMode == AID_ABSOLUTE)) { gpsNoiseScaler = 1.0f; - } else if (gps.num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) { + } else if (gps.num_sats(selected_gps) == 5 && (PV_AidingMode == AID_ABSOLUTE)) { gpsNoiseScaler = 1.4f; } else { // <= 4 satellites or in constant position mode gpsNoiseScaler = 2.0f; } // Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly - if (gps.have_vertical_velocity() && (frontend->_fusionModeGPS == 0) && !frontend->inhibitGpsVertVelUse) { + if (gps.have_vertical_velocity(selected_gps) && (frontend->_fusionModeGPS == 0) && !frontend->inhibitGpsVertVelUse) { useGpsVertVel = true; } else { useGpsVertVel = false; @@ -612,7 +615,7 @@ void NavEKF3_core::readGpsData() } // Read the GPS location in WGS-84 lat,long,height coordinates - const struct Location &gpsloc = gps.location(); + const struct Location &gpsloc = gps.location(selected_gps); // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed if (gpsGoodToAlign && !validOrigin) { @@ -701,10 +704,10 @@ void NavEKF3_core::readBaroData() // check to see if baro measurement has changed so we know if a new measurement has arrived // limit update rate to avoid overflowing the FIFO buffer const AP_Baro &baro = AP::baro(); - if (baro.get_last_update() - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) { + if (baro.get_last_update(selected_baro) - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) { frontend->logging.log_baro = true; - baroDataNew.hgt = baro.get_altitude(); + baroDataNew.hgt = baro.get_altitude(selected_baro); // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff // This prevents negative baro disturbances due to rotor wash ground interaction corrupting the EKF altitude during initial ascent @@ -713,7 +716,7 @@ void NavEKF3_core::readBaroData() } // time stamp used to check for new measurement - lastBaroReceived_ms = baro.get_last_update(); + lastBaroReceived_ms = baro.get_last_update(selected_baro); // estimate of time height measurement was taken, allowing for delays baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms; @@ -789,12 +792,12 @@ void NavEKF3_core::readAirSpdData() // if airspeed reading is valid and is set by the user to be used and has been updated then // we take a new reading, convert from EAS to TAS and set the flag letting other functions // know a new measurement is available - const AP_Airspeed *aspeed = _ahrs->get_airspeed(); - if (aspeed && - aspeed->use() && - (aspeed->last_update_ms() - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) { - tasDataNew.tas = aspeed->get_raw_airspeed() * AP::ahrs().get_EAS2TAS(); - timeTasReceived_ms = aspeed->last_update_ms(); + const auto *airspeed = AP::airspeed(); + if (airspeed && + airspeed->use(selected_airspeed) && + (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) { + tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * _ahrs->get_EAS2TAS(); + timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed); tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; // Correct for the average intersampling delay due to the filter update rate @@ -1028,6 +1031,104 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t storedExtNavVel.push(extNavVelNew); } +/* + update the GPS selection + */ +void NavEKF3_core::update_gps_selection(void) +{ + auto &gps = AP::gps(); + + // in normal operation use the primary GPS + selected_gps = gps.primary_sensor(); + preferred_gps = selected_gps; + + if (frontend->_affinity & EKF_AFFINITY_GPS) { + if (core_index < gps.num_sensors() ) { + // always prefer our core_index, unless we don't have that + // many GPS sensors available + preferred_gps = core_index; + } + if (gps.status(preferred_gps) >= AP_GPS::GPS_OK_FIX_3D) { + // select our preferred_gps if it has a 3D fix, otherwise + // use the primary GPS + selected_gps = preferred_gps; + } + } +} + +/* + update the mag selection + */ +void NavEKF3_core::update_mag_selection(void) +{ + const auto *compass = _ahrs->get_compass(); + if (compass == nullptr) { + return; + } + + if (frontend->_affinity & EKF_AFFINITY_MAG) { + if (core_index < compass->get_count() && + compass->healthy(core_index) && + compass->use_for_yaw(core_index)) { + // use core_index compass if it is healthy + magSelectIndex = core_index; + } + } +} + +/* + update the baro selection + */ +void NavEKF3_core::update_baro_selection(void) +{ + auto &baro = AP::baro(); + + // in normal operation use the primary baro + selected_baro = baro.get_primary(); + + if (frontend->_affinity & EKF_AFFINITY_BARO) { + if (core_index < baro.num_instances() && + baro.healthy(core_index)) { + // use core_index baro if it is healthy + selected_baro = core_index; + } + } +} + +/* + update the airspeed selection + */ +void NavEKF3_core::update_airspeed_selection(void) +{ + const auto *arsp = AP::airspeed(); + if (arsp == nullptr) { + return; + } + + // in normal operation use the primary airspeed sensor + selected_airspeed = arsp->get_primary(); + + if (frontend->_affinity & EKF_AFFINITY_ARSP) { + if (core_index < arsp->get_num_sensors() && + arsp->healthy(core_index) && + arsp->use(core_index)) { + // use core_index airspeed if it is healthy + selected_airspeed = core_index; + } + } +} + +/* + update sensor selections + */ +void NavEKF3_core::update_sensor_selection(void) +{ + update_gps_selection(); + update_mag_selection(); + update_baro_selection(); + update_airspeed_selection(); +} + /* update timing statistics structure */ diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 46ece610f6..e17ef8b223 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -266,9 +266,10 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const } else { // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate if(validOrigin) { - if ((AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { + auto &gps = AP::gps(); + if ((gps.status(selected_gps) >= AP_GPS::GPS_OK_FIX_2D)) { // If the origin has been set and we have GPS, then return the GPS position relative to the origin - const struct Location &gpsloc = AP::gps().location(); + const struct Location &gpsloc = gps.location(selected_gps); const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc); posNE.x = tempPosNE.x; posNE.y = tempPosNE.y; @@ -349,9 +350,9 @@ bool NavEKF3_core::getLLH(struct Location &loc) const } else { // we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS // in this mode we cannot use the EKF states to estimate position so will return the best available data - if ((gps.status() >= AP_GPS::GPS_OK_FIX_2D)) { + if ((gps.status(selected_gps) >= AP_GPS::GPS_OK_FIX_2D)) { // we have a GPS position fix to return - const struct Location &gpsloc = gps.location(); + const struct Location &gpsloc = gps.location(selected_gps); loc.lat = gpsloc.lat; loc.lng = gpsloc.lng; return true; @@ -370,8 +371,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const } else { // If no origin has been defined for the EKF, then we cannot use its position states so return a raw // GPS reading if available and return false - if ((gps.status() >= AP_GPS::GPS_OK_FIX_3D)) { - const struct Location &gpsloc = gps.location(); + if ((gps.status(selected_gps) >= AP_GPS::GPS_OK_FIX_3D)) { + const struct Location &gpsloc = gps.location(selected_gps); loc = gpsloc; loc.relative_alt = 0; loc.terrain_alt = 0; @@ -458,6 +459,24 @@ uint8_t NavEKF3_core::getActiveMag() const return (uint8_t)magSelectIndex; } +// return the index for the active barometer +uint8_t NavEKF3_core::getActiveBaro() const +{ + return (uint8_t)selected_baro; +} + +// return the index for the active GPS +uint8_t NavEKF3_core::getActiveGPS() const +{ + return (uint8_t)selected_gps; +} + +// return the index for the active airspeed +uint8_t NavEKF3_core::getActiveAirspeed() const +{ + return (uint8_t)selected_airspeed; +} + // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements void NavEKF3_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 6e42d0a0f7..7da6570e30 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -54,7 +54,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void) // This check can only be used when the vehicle is stationary const AP_GPS &gps = AP::gps(); - const struct Location &gpsloc = gps.location(); // Current location + const struct Location &gpsloc = gps.location(preferred_gps); // Current location const float posFiltTimeConst = 10.0f; // time constant used to decay position drift // calculate time lapsed since last update and limit to prevent numerical errors float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst); @@ -82,18 +82,18 @@ void NavEKF3_core::calcGpsGoodToAlign(void) // Check that the vertical GPS vertical velocity is reasonable after noise filtering bool gpsVertVelFail; - if (gps.have_vertical_velocity() && onGround) { + if (gps.have_vertical_velocity(preferred_gps) && onGround) { // check that the average vertical GPS velocity is close to zero gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); - } else if ((frontend->_fusionModeGPS == 0) && !gps.have_vertical_velocity()) { + } else if ((frontend->_fusionModeGPS == 0) && !gps.have_vertical_velocity(preferred_gps)) { // If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail gpsVertVelFail = true; // if we have a 3D fix with no vertical velocity and // EK3_GPS_TYPE=0 then change it to 1. It means the GPS is not // capable of giving a vertical velocity - if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { + if (gps.status(preferred_gps) >= AP_GPS::GPS_OK_FIX_3D) { frontend->_fusionModeGPS.set(1); gcs().send_text(MAV_SEVERITY_WARNING, "EK3: Changed EK3_GPS_TYPE to 1"); } @@ -135,7 +135,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void) // fail if horiziontal position accuracy not sufficient float hAcc = 0.0f; bool hAccFail; - if (gps.horizontal_accuracy(hAcc)) { + if (gps.horizontal_accuracy(preferred_gps, hAcc)) { hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR); } else { hAccFail = false; @@ -155,7 +155,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void) // Check for vertical GPS accuracy float vAcc = 0.0f; bool vAccFail = false; - if (gps.vertical_accuracy(vAcc)) { + if (gps.vertical_accuracy(preferred_gps, vAcc)) { vAccFail = (vAcc > 7.5f * checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR); } // Report check result as a text string and bitmask @@ -182,24 +182,24 @@ void NavEKF3_core::calcGpsGoodToAlign(void) } // fail if satellite geometry is poor - bool hdopFail = (gps.get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP); + bool hdopFail = (gps.get_hdop(preferred_gps) > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP); // Report check result as a text string and bitmask if (hdopFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), - "GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop())); + "GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop(preferred_gps))); gpsCheckStatus.bad_hdop = true; } else { gpsCheckStatus.bad_hdop = false; } // fail if not enough sats - bool numSatsFail = (gps.num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS); + bool numSatsFail = (gps.num_sats(preferred_gps) < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS); // Report check result as a text string and bitmask if (numSatsFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), - "GPS numsats %u (needs 6)", gps.num_sats()); + "GPS numsats %u (needs 6)", gps.num_sats(preferred_gps)); gpsCheckStatus.bad_sats = true; } else { gpsCheckStatus.bad_sats = false; @@ -265,7 +265,7 @@ void NavEKF3_core::calcGpsGoodForFlight(void) // get the receivers reported speed accuracy float gpsSpdAccRaw; - if (!AP::gps().speed_accuracy(gpsSpdAccRaw)) { + if (!AP::gps().speed_accuracy(preferred_gps, gpsSpdAccRaw)) { gpsSpdAccRaw = 0.0f; } @@ -325,9 +325,9 @@ void NavEKF3_core::detectFlight() bool largeHgtChange = false; // trigger at 8 m/s airspeed - if (_ahrs->airspeed_sensor_enabled()) { - const AP_Airspeed *airspeed = _ahrs->get_airspeed(); - if (airspeed->get_airspeed() * AP::ahrs().get_EAS2TAS() > 10.0f) { + const auto *arsp = AP::airspeed(); + if (arsp && arsp->healthy(selected_airspeed) && arsp->use(selected_airspeed)) { + if (arsp->get_airspeed(selected_airspeed) * _ahrs->get_EAS2TAS() > 10.0f) { highAirSpd = true; } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index f0c0291b1c..75003f5364 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -70,7 +70,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) if (frontend->_fusionModeGPS != 3) { // Wait for the configuration of all GPS units to be confirmed. Until this has occurred the GPS driver cannot provide a correct time delay float gps_delay_sec = 0; - if (!AP::gps().get_lag(gps_delay_sec)) { + if (!AP::gps().get_lag(selected_gps, gps_delay_sec)) { if (AP_HAL::millis() - lastInitFailReport_ms > 10000) { lastInitFailReport_ms = AP_HAL::millis(); // provide an escalating series of messages @@ -483,8 +483,11 @@ after the tilt has stabilised. */ bool NavEKF3_core::InitialiseFilterBootstrap(void) { + // update sensor selection (for affinity) + update_sensor_selection(); + // If we are a plane and don't have GPS lock then don't initialise - if (assume_zero_sideslip() && AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) { + if (assume_zero_sideslip() && AP::gps().status(preferred_gps) < AP_GPS::GPS_OK_FIX_3D) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 init failure: No GPS lock"); @@ -646,6 +649,9 @@ void NavEKF3_core::UpdateFilter(bool predict) fill_scratch_variables(); + // update sensor selection (for affinity) + update_sensor_selection(); + // TODO - in-flight restart method // Check arm status and perform required checks and mode changes diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index cfde7a7aff..c6b35a5c8c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -171,8 +171,11 @@ public: // return body magnetic field estimates in measurement units / 1000 void getMagXYZ(Vector3f &magXYZ) const; - // return the index for the active magnetometer + // return the index for the active sensors uint8_t getActiveMag() const; + uint8_t getActiveBaro() const; + uint8_t getActiveGPS() const; + uint8_t getActiveAirspeed() const; // Return estimated magnetometer offsets // Return true if magnetometer offsets are valid @@ -1482,4 +1485,28 @@ private: uint8_t EKFGSF_yaw_reset_count; // number of emergency yaw resets performed bool EKFGSF_run_filterbank; // true when the filter bank is active uint8_t EKFGSF_yaw_valid_count; // number of updates since the last invalid yaw estimate + + // bits in EK3_AFFINITY + enum ekf_affinity { + EKF_AFFINITY_GPS = (1U<<0), + EKF_AFFINITY_BARO = (1U<<1), + EKF_AFFINITY_MAG = (1U<<2), + EKF_AFFINITY_ARSP = (1U<<3), + }; + + // update selected_sensors for this core + void update_sensor_selection(void); + void update_gps_selection(void); + void update_mag_selection(void); + void update_baro_selection(void); + void update_airspeed_selection(void); + + // selected and preferred sensor instances. We separate selected + // from preferred so that calcGpsGoodToAlign() can ensure the + // preferred sensor is ready. Note that magSelectIndex is used for + // compass selection + uint8_t selected_gps; + uint8_t preferred_gps; + uint8_t selected_baro; + uint8_t selected_airspeed; };