diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index f7ac1e2bbb..bce6e9b5f4 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -265,6 +265,12 @@ public: return true; } + // return estimate of true airspeed vector in body frame in m/s + // returns false if estimate is unavailable + virtual bool airspeed_vector_true(Vector3f &vec) const WARN_IF_UNUSED { + return false; + } + // 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 9921e42a04..ef0eab1a5c 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -561,6 +561,31 @@ bool AP_AHRS_NavEKF::airspeed_estimate(float &airspeed_ret) const return AP_AHRS_DCM::airspeed_estimate(get_active_airspeed_index(), airspeed_ret); } +// return estimate of true airspeed vector in body frame in m/s +// returns false if estimate is unavailable +bool AP_AHRS_NavEKF::airspeed_vector_true(Vector3f &vec) const +{ + switch (active_EKF_type()) { + case EKFType::NONE: + break; +#if HAL_NAVEKF2_AVAILABLE + case EKFType::TWO: + return EKF2.getAirSpdVec(-1, vec); +#endif + +#if HAL_NAVEKF3_AVAILABLE + case EKFType::THREE: + return EKF3.getAirSpdVec(-1, vec); +#endif + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKFType::SITL: + break; +#endif + } + return false; +} + // true if compass is being used bool AP_AHRS_NavEKF::use_compass(void) { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 96ebfa7ef0..9678811d17 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -96,6 +96,10 @@ public: // if we have an estimate bool airspeed_estimate(float &airspeed_ret) const override; + // return estimate of true airspeed vector in body frame in m/s + // returns false if estimate is unavailable + bool airspeed_vector_true(Vector3f &vec) const override; + // true if compass is being used bool use_compass() override;