diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index ca1fe6d06c..91c4249f2d 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/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; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 0d58c45578..4ebd50edf9 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 70825fc058..f0364ae9a7 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 85960954d0..5e8b83ea6c 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/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 { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 16b0f9a8bc..fcb5b90a0f 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/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;