diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index d82309aa6b..641ba8bfde 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1990,3 +1990,15 @@ bool NavEKF3::yawAlignmentComplete(void) const } return core[primary].have_aligned_yaw(); } + +// returns true when the state estimates for the selected core are significantly degraded by vibration +bool NavEKF3::isVibrationAffected(int8_t instance) const +{ + if (instance < 0 || instance >= num_cores) { + instance = primary; + } + if (core) { + return core[instance].isVibrationAffected(); + } + return false; +} diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 882a276e00..c35e9b7daa 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -351,6 +351,10 @@ public: // returns true when the yaw angle has been aligned bool yawAlignmentComplete(void) const; + // returns true when the state estimates for the selected core are significantly degraded by vibration + // if instance < 0, the primary instance will be used + bool isVibrationAffected(int8_t instance) const; + private: uint8_t num_cores; // number of allocated cores uint8_t primary; // current primary core diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 1337083683..4b08be8ea7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -403,6 +403,9 @@ public: void Log_Write(uint64_t time_us); + // returns true when the state estimates are significantly degraded by vibration + bool isVibrationAffected() const { return badIMUdata; } + private: EKFGSF_yaw *yawEstimator; AP_DAL &dal;