AP_AHRS: fixed switching airspeed sensor based on EKF3 affinity (for 4.1)

we need to use the EKF selected airspeed sensor when the EKF is setup
to run a different sensor on each lane

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -622,7 +622,8 @@ bool AP_AHRS_NavEKF::airspeed_estimate(float &airspeed_ret) const
 {
     bool ret = false;
     if (airspeed_sensor_enabled()) {
-        airspeed_ret = AP::airspeed()->get_airspeed();
+        uint8_t idx = get_active_airspeed_index();
+        airspeed_ret = AP::airspeed()->get_airspeed(idx);
 
         if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
             // constrain the airspeed by the ground speed
@@ -2639,18 +2640,23 @@ bool AP_AHRS_NavEKF::getGpsGlitchStatus() const
 //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
+    const auto *airspeed = AP::airspeed();
+    if (airspeed == nullptr) {
+        return 0;
+    }
+
+    // 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());   
+        uint8_t ret = EKF3.getActiveAirspeed(get_primary_core_index());
+        if (ret != 255 && airspeed->healthy(ret)) {
+            return ret;
+        }
     }
 #endif
+
     // for the rest, let the primary airspeed sensor be used
-    const AP_Airspeed * _airspeed = AP::airspeed();
-    if (_airspeed != nullptr) {
-        return _airspeed->get_primary();
-    }
-    return 0;
+    return airspeed->get_primary();
 }
 
 // get the index of the current primary IMU