AP_AHRS: get airspeed estimate from non-primary instances too

with multiple airspeed sensors and airspeed affinity in EKF3,
it is possible to have switched over to a lane not using the
primary airspeed sensor, so AHRS should know which airspeed
sensor to use, i.e, the one being used by the primary lane.

libraries/AP_AHRS/AP_AHRS.h

@@ -292,6 +292,12 @@ public:
         return _airspeed != nullptr && _airspeed->use() && _airspeed->healthy();
     }
 
+    // return true if airspeed comes from a specific airspeed sensor, as
+    // opposed to an IMU estimate
+    bool airspeed_sensor_enabled(uint8_t airspeed_index) const {
+        return _airspeed != nullptr && _airspeed->use(airspeed_index) && _airspeed->healthy(airspeed_index);
+    }
+
     // return the parameter AHRS_WIND_MAX in metres per second
     uint8_t get_max_wind() const {
         return _wind_max;

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -1074,6 +1074,35 @@ bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const
     return true;
 }
 
+// return an airspeed estimate from a specific airspeed sensor instance if available
+bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const
+{
+    if (airspeed_sensor_enabled(airspeed_index)) {
+        airspeed_ret = _airspeed->get_airspeed(airspeed_index);
+    } else if (_flags.wind_estimation && have_gps()) {
+        // estimate it via GPS speed and wind
+        airspeed_ret = _last_airspeed;
+    } else {
+        // give the last estimate, but return false. This is used by
+        // dead-reckoning code
+        airspeed_ret = _last_airspeed;
+        return false;
+    }
+
+    if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
+        // constrain the airspeed by the ground speed
+        // and AHRS_WIND_MAX
+        const float gnd_speed = AP::gps().ground_speed();
+        float true_airspeed = airspeed_ret * get_EAS2TAS();
+        true_airspeed = constrain_float(true_airspeed,
+                                        gnd_speed - _wind_max,
+                                        gnd_speed + _wind_max);
+        airspeed_ret = true_airspeed / get_EAS2TAS();
+    }
+
+    return true;
+}
+
 bool AP_AHRS_DCM::set_home(const Location &loc)
 {
     // check location is valid

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -94,6 +94,10 @@ public:
     // if we have an estimate
     bool airspeed_estimate(float &airspeed_ret) const override;
 
+    // return an airspeed estimate if available. return true
+    // if we have an estimate from a specific sensor index
+    bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const;
+
     // return a synthetic airspeed estimate (one derived from sensors
     // other than an actual airspeed sensor), if available. return
     // true if we have a synthetic airspeed.  ret will not be modified

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -543,8 +543,8 @@ Vector3f AP_AHRS_NavEKF::wind_estimate(void) const
 // return an airspeed estimate if available. return true
 // if we have an estimate
 bool AP_AHRS_NavEKF::airspeed_estimate(float &airspeed_ret) const
-{
-    return AP_AHRS_DCM::airspeed_estimate(airspeed_ret);
+{    
+    return AP_AHRS_DCM::airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
 }
 
 // true if compass is being used
@@ -2160,6 +2160,22 @@ bool AP_AHRS_NavEKF::have_ekf_logging(void) const
     return false;
 }
 
+//get the index of the active airspeed sensor, wrt the primary core
+uint8_t AP_AHRS_NavEKF::get_active_airspeed_index() const
+{
+// we only have affinity for EKF3 as of now
+#if HAL_NAVEKF3_AVAILABLE
+    if (active_EKF_type() == EKFType::THREE) {
+        return EKF3.getActiveAirspeed(get_primary_core_index());   
+    }
+#endif
+    // for the rest, let the primary airspeed sensor be used
+    if (_airspeed != nullptr) {
+        return _airspeed->get_primary();
+    }
+    return 0;
+}
+
 // get the index of the current primary IMU
 uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const
 {

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -286,6 +286,13 @@ public:
     // is the EKF backend doing its own sensor logging?
     bool have_ekf_logging(void) const override;
 
+    // return the index of the airspeed we should use for airspeed measurements
+    // with multiple airspeed sensors and airspeed affinity in EKF3, it is possible to have switched
+    // over to a lane not using the primary airspeed sensor, so AHRS should know which airspeed sensor
+    // to use, i.e, the one being used by the primary lane. A lane switch could have happened due to an 
+    // airspeed sensor fault, which makes this even more necessary
+    uint8_t get_active_airspeed_index() const;
+
     // return the index of the primary core or -1 if no primary core selected
     int8_t get_primary_core_index() const override;