diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp index 7c91234dad..310c0d5bea 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp @@ -36,13 +36,6 @@ void AP_InertialNav_NavEKF::update(float dt) _velocity_cm = velNED * 100; // convert to cm/s _velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU } - - // Get 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 vertical position due to the various errors that are being corrected for. - if (_ahrs_ekf.get_vert_pos_rate(_pos_z_rate)) { - _pos_z_rate *= 100; // convert to cm/s - _pos_z_rate = - _pos_z_rate; // InertialNav is NEU - } } /** @@ -127,14 +120,6 @@ float AP_InertialNav_NavEKF::get_speed_xy() const return norm(_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 fd3604214b..e5f24d67a2 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h @@ -70,11 +70,6 @@ public: */ const Vector3f& get_velocity() const override; - /** - * get_pos_z_derivative - returns the derivative of the z position in cm/s - */ - float get_pos_z_derivative() const; - /** * get_speed_xy - returns the current horizontal speed in cm/s * @@ -100,7 +95,6 @@ public: private: Vector3f _relpos_cm; // NEU Vector3f _velocity_cm; // NEU - float _pos_z_rate; struct Location _abspos; bool _haveabspos; AP_AHRS_NavEKF &_ahrs_ekf;