AP_InertialNav: Add output method for rate of change of vertical position

libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp

@@ -24,9 +24,15 @@ void AP_InertialNav_NavEKF::update(float dt)
     _ahrs_ekf.get_NavEKF().getVelNED(_velocity_cm);
     _velocity_cm *= 100; // convert to cm/s
 
+    // A derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
+    // This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
+    _ahrs_ekf.get_NavEKF().getPosDownDerivative(_pos_z_rate);
+    _pos_z_rate *= 100; // convert to cm/s
+
     // InertialNav is NEU
     _relpos_cm.z = - _relpos_cm.z;
     _velocity_cm.z = -_velocity_cm.z;
+    _pos_z_rate = - _pos_z_rate;
 }
 
 /**
@@ -111,6 +117,14 @@ float AP_InertialNav_NavEKF::get_velocity_xy() const
     return pythagorous2(_velocity_cm.x, _velocity_cm.y);
 }
 
+/**
+ * get_pos_z_derivative - returns the derivative of the z position in cm/s
+*/
+float AP_InertialNav_NavEKF::get_pos_z_derivative() const
+{
+    return _pos_z_rate;
+}
+
 /**
  * get_altitude - get latest altitude estimate in cm
  * @return

libraries/AP_InertialNav/AP_InertialNav_NavEKF.h

@@ -75,6 +75,11 @@ public:
      */
     const Vector3f&    get_velocity() const;
 
+    /**
+     * get_pos_z_derivative - returns the derivative of the z position in cm/s
+    */
+    float    get_pos_z_derivative() const;
+
     /**
      * get_velocity_xy - returns the current horizontal velocity in cm/s
      *
@@ -114,6 +119,7 @@ public:
 private:
     Vector3f _relpos_cm;   // NEU
     Vector3f _velocity_cm; // NEU
+    float _pos_z_rate;
     struct Location _abspos;
     bool _haveabspos;
     AP_AHRS_NavEKF &_ahrs_ekf;