AP_NavEKF3: implement sensor affinity using EK3_AFFINITY parameter

this allows the EKF core index to be used to select a GPS/baro/mag
instance. This is an alternative to GPS blending that allows EKF lane
switching to be used to select the right combination of GPS and IMU
add logging to XKFS message

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

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;

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),
+        magZ    : (int16_t)(magXYZ.z)
-        index   : (uint8_t)(magIndex)
     };
     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);
     }

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<maxCount; i++) {
@@ -523,8 +526,8 @@ void NavEKF3_core::readGpsData()
     // limit update rate to avoid overflowing the FIFO buffer
     const AP_GPS &gps = AP::gps();
 
-    if (gps.last_message_time_ms() - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) {
+        if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) {
-        if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+            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();
+    const auto *airspeed = AP::airspeed();
-    if (aspeed &&
+    if (airspeed &&
-            aspeed->use() &&
+        airspeed->use(selected_airspeed) &&
-            (aspeed->last_update_ms() - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
+        (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
-        tasDataNew.tas = aspeed->get_raw_airspeed() * AP::ahrs().get_EAS2TAS();
+        tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * _ahrs->get_EAS2TAS();
-        timeTasReceived_ms = aspeed->last_update_ms();
+        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
  */

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)) {
+        if ((gps.status(selected_gps) >= AP_GPS::GPS_OK_FIX_3D)) {
-            const struct Location &gpsloc = gps.location();
+            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
 {

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 auto *arsp = AP::airspeed();
-            const AP_Airspeed *airspeed = _ahrs->get_airspeed();
+        if (arsp && arsp->healthy(selected_airspeed) && arsp->use(selected_airspeed)) {
-            if (airspeed->get_airspeed() * AP::ahrs().get_EAS2TAS() > 10.0f) {
+            if (arsp->get_airspeed(selected_airspeed) * _ahrs->get_EAS2TAS() > 10.0f) {
                 highAirSpd = true;
             }
         }

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

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