Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 01:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: added healthy() API

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-02 16:16:16 +11:00
parent 644bf71f56
commit 9ef71a9dec
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -68,6 +68,22 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
dtIMUAvgInv = 1.0f/dtIMUAvg; dtIMUAvgInv = 1.0f/dtIMUAvg;
} }
bool NavEKF::healthy(void)
{
if (!statesInitialised) {
return false;
}
Quaternion q(states[0],states[1],states[2],states[3]);
if (q.is_nan()) {
return false;
}
if (isnan(states[4]) || isnan(states[5]) || isnan(states[6])) {
return false;
}
// all OK
return true;
}
void NavEKF::InitialiseFilter(void) void NavEKF::InitialiseFilter(void)
{ {
// Calculate initial filter quaternion states from ahrs solution // Calculate initial filter quaternion states from ahrs solution

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -76,6 +76,9 @@ public:
// Update Filter States - this should be called whenever new IMU data is available // Update Filter States - this should be called whenever new IMU data is available
void UpdateFilter(void); void UpdateFilter(void);
// return true if the filter is healthy
bool healthy(void);
// fill in latitude, longitude and height of the reference point // fill in latitude, longitude and height of the reference point
void getRefLLH(struct Location &loc) const; void getRefLLH(struct Location &loc) const;