diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 9ac7b5e9..d7e8fe26 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -421,7 +421,11 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do errorAngle = QuaternionOperations::getAngle(errorQuat, true); // Calculate error satellite rotational rate - // First combine the target and reference satellite rotational rates + // Convert target rotational rate into body RF + double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0}; + QuaternionOperations::inverse(errorQuat, errorQuatInv); + QuaternionOperations::multiplyVector(errorQuat, targetSatRotRate, targetSatRotRateB); + // Combine the target and reference satellite rotational rates double combinedRefSatRotRate[3] = {0, 0, 0}; VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); // Then subtract the combined required satellite rotational rates from the actual rate