Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Add accessor function for airspeed health monitoring

Browse Source
This commit is contained in:
Paul Riseborough 2022-06-26 08:47:08 +10:00 committed by Randy Mackay
parent 991959ddf7
commit 037cf9d2e9
4 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -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
{

4
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -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

13
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -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

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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