diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 61a4a95d93..631a63e7ff 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1211,6 +1211,15 @@ bool NavEKF3::getAirSpdVec(Vector3f &vel) const return false; } +// return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed +bool NavEKF3::getAirSpdHealthData(float &innovation, float &innovationVariance, uint32_t &age_ms) const +{ + if (core) { + return core[primary].getAirSpdHealthData(innovation, innovationVariance, age_ms); + } + return false; +} + // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s float NavEKF3::getPosDownDerivative() const { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 7b1487b3cc..2d14a44975 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -84,6 +84,10 @@ public: // returns false if estimate is unavailable bool getAirSpdVec(Vector3f &vel) const; + // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed + // returns false if the data is unavilable + bool getAirSpdHealthData(float &innovation, float &innovationVariance, uint32_t &age_ms) const; + // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF // but will always be kinematically consistent with the z component of the EKF position state diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 803cd9a935..aced7b8b02 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -191,6 +191,19 @@ bool NavEKF3_core::getAirSpdVec(Vector3f &vel) const return true; } +// return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed +// returns false if the data is unavailable +bool NavEKF3_core::getAirSpdHealthData(float &innovation, float &innovationVariance, uint32_t &age_ms) const +{ + if (tasDataDelayed.time_ms == 0) { + // no data has been processed since startup + return false; + } + innovation = (float)innovVtas; + innovationVariance = (float)varInnovVtas; + age_ms = imuSampleTime_ms - tasDataDelayed.time_ms; + return true; +} // Return the rate of change of vertical position in the down direction (dPosD/dt) of the body frame origin in m/s float NavEKF3_core::getPosDownDerivative(void) const diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 9f0687a42e..0eef8fd4f7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -150,6 +150,10 @@ public: // returns false if estimate is unavailable bool getAirSpdVec(Vector3f &vel) const; + // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed + // returns false if the data is unavailable + bool getAirSpdHealthData(float &innovation, float &innovationVariance, uint32_t &age_ms) const; + // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF // but will always be kinematically consistent with the z component of the EKF position state