diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index e9e117af4e..39b8ab77d8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1170,14 +1170,6 @@ float NavEKF3::getPosDownDerivative(int8_t instance) const return 0.0f; } -// This returns the specific forces in the NED frame -void NavEKF3::getAccelNED(Vector3f &accelNED) const -{ - if (core) { - core[primary].getAccelNED(accelNED); - } -} - // return body axis gyro bias estimates in rad/sec void NavEKF3::getGyroBias(int8_t instance, Vector3f &gyroBias) const { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 556aa56f4c..bec3be967a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -93,9 +93,6 @@ public: // but will always be kinematically consistent with the z component of the EKF position state float getPosDownDerivative(int8_t instance) const; - // This returns the specific forces in the NED frame - void getAccelNED(Vector3f &accelNED) const; - // return body axis gyro bias estimates in rad/sec for the specified instance // An out of range instance (eg -1) returns data for the primary instance void getGyroBias(int8_t instance, Vector3f &gyroBias) const; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 4a250d4660..6e18779d42 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -216,12 +216,6 @@ float NavEKF3_core::getPosDownDerivative(void) const return vertCompFiltState.vel + velOffsetNED.z; } -// This returns the specific forces in the NED frame -void NavEKF3_core::getAccelNED(Vector3f &accelNED) const { - accelNED = velDotNED; - accelNED.z -= GRAVITY_MSS; -} - // Write the last estimated NE position of the body frame origin relative to the reference point (m). // Return true if the estimate is valid bool NavEKF3_core::getPosNE(Vector2f &posNE) const diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index b52b6df5e9..baab4aee9b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -125,9 +125,6 @@ public: // but will always be kinematically consistent with the z component of the EKF position state float getPosDownDerivative(void) const; - // This returns the specific forces in the NED frame - void getAccelNED(Vector3f &accelNED) const; - // return body axis gyro bias estimates in rad/sec void getGyroBias(Vector3f &gyroBias) const;