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

AP_AHRS: check for NavEKF health

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

16
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -64,7 +64,7 @@ void AP_AHRS_NavEKF::update(void)
}
if (ekf_started) {
EKF.UpdateFilter();
if (_ekf_use) {
if (using_EKF()) {
EKF.getRotationBodyToNED(_dcm_matrix);
Vector3f eulers;
EKF.getEulerAngles(eulers);
@ -100,10 +100,8 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con
// dead-reckoning support
bool AP_AHRS_NavEKF::get_position(struct Location &loc)
{
if (ekf_started && _ekf_use) {
if (EKF.getLLH(loc)) {
return true;
}
if (using_EKF() && EKF.getLLH(loc)) {
return true;
}
return AP_AHRS_DCM::get_position(loc);
}
@ -122,7 +120,7 @@ float AP_AHRS_NavEKF::get_error_yaw(void)
// return a wind estimation vector, in m/s
Vector3f AP_AHRS_NavEKF::wind_estimate(void)
{
if (!ekf_started || !_ekf_use) {
if (!using_EKF()) {
return AP_AHRS_DCM::wind_estimate();
}
Vector3f wind;
@ -147,7 +145,7 @@ bool AP_AHRS_NavEKF::use_compass(void)
// return secondary attitude solution if available, as eulers in radians
bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers)
{
if (ekf_started && _ekf_use) {
if (using_EKF()) {
// return DCM attitude
eulers = _dcm_attitude;
return true;
@ -164,7 +162,7 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers)
// return secondary position solution if available
bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc)
{
if (ekf_started && _ekf_use) {
if (using_EKF()) {
// return DCM position
AP_AHRS_DCM::get_position(loc);
return true;
@ -181,7 +179,7 @@ bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc)
// EKF has a better ground speed vector estimate
Vector2f AP_AHRS_NavEKF::groundspeed_vector(void)
{
if (!ekf_started || !_ekf_use) {
if (!using_EKF()) {
return AP_AHRS_DCM::groundspeed_vector();
}
Vector3f vec;

2
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -83,6 +83,8 @@ public:
Vector2f groundspeed_vector(void);
private:
bool using_EKF(void) { return ekf_started && _ekf_use && EKF.healthy(); }
NavEKF EKF;
AP_Baro &_baro;
bool ekf_started;