From 47ae0f35f6dc21bbd0a5a1fe5005a795990ee938 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 15 Oct 2015 16:27:40 +1100 Subject: [PATCH] AP_NavEKF2: Make EKF2 PosDownDerivative interface follow coding conventions Updates arising from peer review. --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 5 +++-- libraries/AP_NavEKF2/AP_NavEKF2.h | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 4 ++-- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 9b0dea30b9..cd6420c8b7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -461,12 +461,13 @@ void NavEKF2::getVelNED(Vector3f &vel) const } // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s -void NavEKF2::getPosDownDerivative(float &ret) const +float NavEKF2::getPosDownDerivative(void) const { // return the value calculated from a complmentary filer applied to the EKF height and vertical acceleration if (core) { - core->getPosDownDerivative(ret); + return core->getPosDownDerivative(); } + return 0.0f; } // This returns the specific forces in the NED frame diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 8d78626d90..057d28e2f0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -68,7 +68,7 @@ public: // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF // but will always be kinematically consistent with the z component of the EKF position state - void getPosDownDerivative(float &ret) const; + float getPosDownDerivative(void) const; // This returns the specific forces in the NED frame void getAccelNED(Vector3f &accelNED) const; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 7e476d8b70..a9bf99360a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -150,10 +150,10 @@ void NavEKF2_core::getVelNED(Vector3f &vel) const } // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s -void NavEKF2_core::getPosDownDerivative(float &ret) const +float NavEKF2_core::getPosDownDerivative(void) const { // return the value calculated from a complmentary filer applied to the EKF height and vertical acceleration - ret = posDownDerivative; + return posDownDerivative; } // This returns the specific forces in the NED frame diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 856ce31853..5b5e2daf78 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -71,7 +71,7 @@ public: // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF // but will always be kinematically consistent with the z component of the EKF position state - void getPosDownDerivative(float &ret) const; + float getPosDownDerivative(void) const; // This returns the specific forces in the NED frame void getAccelNED(Vector3f &accelNED) const;