diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 89f3d958..e4e9ccab 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -192,10 +192,12 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { - // First calculate error quaternion between current and target orientation - QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); - // Last calculate add rotation from reference quaternion - QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); + // First calculate error quaternion between current and target orientation without reference + // quaternion + double errorQuatWoRef[4] = {0, 0, 0, 0}; + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef); + // Then add rotation from reference quaternion + QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat); // Keep scalar part of quaternion positive if (errorQuat[3] < 0) { VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4);