AP_InertialNav: use ekf::get_vert_pos_rate during high vibration

libraries/AP_InertialNav/AP_InertialNav.h

@@ -25,10 +25,10 @@ public:
     AP_InertialNav() {}
 
     /**
-     * update - updates velocity and position estimates using latest info from accelerometers
+     * updates velocity and position estimates pulling data from EKF
-     * augmented with gps and baro readings
+     * 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

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
     }

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