From 1a05c27bbb89937867cba8020606df4a2a28bdc7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell <tridge@samba.org> Date: Thu, 15 May 2014 17:09:18 +1000 Subject: [PATCH] AP_AHRS: added healthy() function this will be used to report when the AHRS subsystem becomes unhealthy --- libraries/AP_AHRS/AP_AHRS.h | 3 +++ libraries/AP_AHRS/AP_AHRS_DCM.cpp | 12 ++++++++++++ libraries/AP_AHRS/AP_AHRS_DCM.h | 6 ++++++ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 11 +++++++++++ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 3 +++ 5 files changed, 35 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 2a1f8ecf5..39e730956 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 aba87bd32..10a5738cd 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 e8709f799..4179c7cf3 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 3bd7d5793..ffb263e8e 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 20775fdc8..52426fba6 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; -- GitLab