From 608017ba4a50f91dd7af1e2dd9abb0568ccefadf Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Fri, 3 Apr 2015 13:22:36 -0700 Subject: [PATCH] AP_Gimbal: optimize function calls using const refrences --- libraries/AP_Gimbal/AP_Gimbal.cpp | 6 +++--- libraries/AP_Gimbal/AP_Gimbal.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Gimbal/AP_Gimbal.cpp b/libraries/AP_Gimbal/AP_Gimbal.cpp index ba694f1087..3a4ed82079 100644 --- a/libraries/AP_Gimbal/AP_Gimbal.cpp +++ b/libraries/AP_Gimbal/AP_Gimbal.cpp @@ -64,7 +64,7 @@ void AP_Gimbal::update_state() gimbalRateDemVec += getGimbalRateDemVecGyroBias(); } -Vector3f AP_Gimbal::getGimbalRateDemVecYaw(Quaternion quatEst) +Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst) { // Define rotation from vehicle to gimbal using a 312 rotation sequence Matrix3f Tvg; @@ -109,7 +109,7 @@ Vector3f AP_Gimbal::getGimbalRateDemVecYaw(Quaternion quatEst) return gimbalRateDemVecYaw; } -Vector3f AP_Gimbal::getGimbalRateDemVecTilt(Quaternion quatEst) +Vector3f AP_Gimbal::getGimbalRateDemVecTilt(const Quaternion &quatEst) { // Calculate the gimbal 321 Euler angle estimates relative to earth frame Vector3f eulerEst; @@ -142,7 +142,7 @@ Vector3f AP_Gimbal::getGimbalRateDemVecTilt(Quaternion quatEst) return gimbalRateDemVecTilt; } -Vector3f AP_Gimbal::getGimbalRateDemVecForward(Quaternion quatEst) +Vector3f AP_Gimbal::getGimbalRateDemVecForward(const Quaternion &quatEst) { // quaternion demanded at the previous time step static float lastDem; diff --git a/libraries/AP_Gimbal/AP_Gimbal.h b/libraries/AP_Gimbal/AP_Gimbal.h index ec9757cf41..ac85cef32d 100644 --- a/libraries/AP_Gimbal/AP_Gimbal.h +++ b/libraries/AP_Gimbal/AP_Gimbal.h @@ -83,9 +83,9 @@ private: uint8_t isCopterFliped(); // Control loop functions - Vector3f getGimbalRateDemVecYaw(Quaternion quatEst); - Vector3f getGimbalRateDemVecTilt(Quaternion quatEst); - Vector3f getGimbalRateDemVecForward(Quaternion quatEst); + Vector3f getGimbalRateDemVecYaw(const Quaternion &quatEst); + Vector3f getGimbalRateDemVecTilt(const Quaternion &quatEst); + Vector3f getGimbalRateDemVecForward(const Quaternion &quatEst); Vector3f getGimbalRateDemVecGyroBias(); };