AP_Gimbal: optimize function calls using const refrences

This commit is contained in:
Arthur Benemann 2015-04-03 13:22:36 -07:00 committed by Randy Mackay
parent 03c0e17a4d
commit 608017ba4a
2 changed files with 6 additions and 6 deletions

View File

@ -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;

View File

@ -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();
};