From ccc95d8726571442312b0c44d14350019d889329 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Fri, 16 Jul 2021 16:22:31 +1000
Subject: [PATCH] AP_NavEKF3: Add accessor function for vibration affected
 status

---
 libraries/AP_NavEKF3/AP_NavEKF3.cpp    | 12 ++++++++++++
 libraries/AP_NavEKF3/AP_NavEKF3.h      |  4 ++++
 libraries/AP_NavEKF3/AP_NavEKF3_core.h |  3 +++
 3 files changed, 19 insertions(+)

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;