Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-29 20:18:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_SmallEKF: add function to report if the EKF is stable

Browse Source
This commit is contained in:
Arthur Benemann 2015-03-05 15:23:14 -08:00 committed by Randy Mackay
parent 7b28bf7d44
commit 1d9beed42f
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF/AP_SmallEKF.cpp
View File

@ -925,4 +925,11 @@ void SmallEKF::getQuat(Quaternion &quat) const
quat = state.quat; quat = state.quat;
} }
// get filter status - true is aligned
bool SmallEKF::getStatus() const
{
float run_time = hal.scheduler->millis() - StartTime_ms;
return YawAligned && (run_time > 10000);
}
#endif // HAL_CPU_CLASS #endif // HAL_CPU_CLASS

3
libraries/AP_NavEKF/AP_SmallEKF.h
View File

@ -92,6 +92,9 @@ public:
// get quaternion data // get quaternion data
void getQuat(Quaternion &quat) const; void getQuat(Quaternion &quat) const;
// get filter alignment status - true is aligned
bool getStatus(void) const;
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private: