diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 2a1f8ecf5d009bec19e137033d0cc2dc2ce0bfd9..39e7309566268af7bebe6ac0da04c22a3cc0bc1e 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -324,6 +324,9 @@ public: // return the active accelerometer instance uint8_t get_active_accel_instance(void) const { return _active_accel_instance; } + // is the AHRS subsystem healthy? + virtual bool healthy(void) = 0; + protected: AHRS_VehicleClass _vehicle_class; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index aba87bd3236e34bb73440f8fc7f24e1cb980330b..10a5738cd761a23120cbcc3ecf299baf1143b898 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -241,6 +241,7 @@ AP_AHRS_DCM::normalize(void) !renorm(t2, _dcm_matrix.c)) { // Our solution is blowing up and we will force back // to last euler angles + _last_failure_ms = hal.scheduler->millis(); reset(true); } } @@ -598,6 +599,7 @@ AP_AHRS_DCM::drift_correction(float deltat) GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information + _last_failure_ms = hal.scheduler->millis(); return; } using_gps_corrections = true; @@ -648,6 +650,7 @@ AP_AHRS_DCM::drift_correction(float deltat) if (besti == -1) { // no healthy accelerometers! + _last_failure_ms = hal.scheduler->millis(); return; } @@ -697,6 +700,7 @@ AP_AHRS_DCM::drift_correction(float deltat) if (error[besti].is_nan() || error[besti].is_inf()) { // don't allow bad values check_matrix(); + _last_failure_ms = hal.scheduler->millis(); return; } @@ -918,3 +922,11 @@ void AP_AHRS_DCM::set_home(const Location &loc) _home.options = 0; } +/* + check if the AHRS subsystem is healthy +*/ +bool AP_AHRS_DCM::healthy(void) +{ + // consider ourselves healthy if there have been no failures for 5 seconds + return (_last_failure_ms == 0 || hal.scheduler->millis() - _last_failure_ms > 5000); +} diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index e8709f7997499a06c86e5237ba7044669190e06b..4179c7cf3b77dcc9827731a8f673b388409b0fed 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -81,6 +81,9 @@ public: void set_home(const Location &loc); void estimate_wind(void); + // is the AHRS subsystem healthy? + bool healthy(void); + private: float _ki; float _ki_yaw; @@ -165,6 +168,9 @@ private: // estimated wind in m/s Vector3f _wind; + + // last time AHRS failed in milliseconds + uint32_t _last_failure_ms; }; #endif // __AP_AHRS_DCM_H__ diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 3bd7d5793b896d5c61659cc22b6b996e40e0bb76..ffb263e8e3cef8cb92712240c89143240ec9185f 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -228,5 +228,16 @@ bool AP_AHRS_NavEKF::using_EKF(void) const return ekf_started && _ekf_use && EKF.healthy(); } +/* + check if the AHRS subsystem is healthy +*/ +bool AP_AHRS_NavEKF::healthy(void) +{ + if (_ekf_use) { + return ekf_started && EKF.healthy(); + } + return AP_AHRS_DCM::healthy(); +} + #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 20775fdc813985a67f541dce3684a2849028b72c..52426fba6146af3f33c7281de19ab7c4d7172562 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -92,6 +92,9 @@ public: void set_ekf_use(bool setting) { _ekf_use.set(setting); } + // is the AHRS subsystem healthy? + bool healthy(void); + private: bool using_EKF(void) const;