diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp index 8bd38ebf19..3746ede709 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp +++ b/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 diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h index cb06111813..39d436c6c7 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h +++ b/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;