From adf4d8fd4788841eb8dd1fba62777166ff9540ff Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 1 Apr 2019 15:55:58 +0900
Subject: [PATCH] AP_InertialNav: use ekf::get_vert_pos_rate during high
 vibration

---
 libraries/AP_InertialNav/AP_InertialNav.h          |  6 +++---
 libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp | 10 +++++++++-
 libraries/AP_InertialNav/AP_InertialNav_NavEKF.h   |  2 +-
 3 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h
index f81cabf9e1..b81ba36c95 100644
--- a/libraries/AP_InertialNav/AP_InertialNav.h
+++ b/libraries/AP_InertialNav/AP_InertialNav.h
@@ -25,10 +25,10 @@ public:
     AP_InertialNav() {}
 
     /**
-     * update - updates velocity and position estimates using latest info from accelerometers
-     * augmented with gps and baro readings
+     * updates velocity and position estimates pulling data from EKF
+     * high_vibes should be set to true if high vibration have been detected
      */
-    virtual void update(void) = 0;
+    virtual void update(bool high_vibes = false) = 0;
 
     /**
      * get_filter_status : returns filter status as a series of flags
diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
index bc76532e56..beb6a36405 100644
--- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
+++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
@@ -1,4 +1,5 @@
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Baro/AP_Baro.h>
 #include "AP_InertialNav.h"
 
 #if AP_AHRS_NAVEKF_AVAILABLE
@@ -12,7 +13,7 @@
 /**
    update internal state
 */
-void AP_InertialNav_NavEKF::update()
+void AP_InertialNav_NavEKF::update(bool high_vibes)
 {
     // get the NE position relative to the local earth frame origin
     Vector2f posNE;
@@ -30,6 +31,13 @@ void AP_InertialNav_NavEKF::update()
     // get the velocity relative to the local earth frame
     Vector3f velNED;
     if (_ahrs_ekf.get_velocity_NED(velNED)) {
+        // during high vibration events use vertical position change
+        if (high_vibes) {
+            float rate_z;
+            if (_ahrs_ekf.get_vert_pos_rate(rate_z)) {
+                velNED.z = rate_z;
+            }
+        }
         _velocity_cm = velNED * 100; // convert to cm/s
         _velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU
     }
diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
index 8ef2ac604c..3be6c232b5 100644
--- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
+++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
@@ -19,7 +19,7 @@ public:
     /**
        update internal state
     */
-    void        update() override;
+    void        update(bool high_vibes = false) override;
 
     /**
      * get_filter_status - returns filter status as a series of flags