From 8be94cf2dcbca978d6fad8bc793ba12df6d6a211 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Feb 2024 12:38:16 +0100 Subject: [PATCH] overloading is fun (but should be done right) --- mission/controller/acs/Guidance.cpp | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 12957ab2..3cadb861 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -408,9 +408,7 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do 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 - double invTargetQuat[4] = {0, 0, 0, 0}; - QuaternionOperations::inverse(targetQuat, invTargetQuat); - QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat); + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Last calculate add rotation from reference quaternion QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); // Keep scalar part of quaternion positive @@ -435,17 +433,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { - // First calculate error quaternion between current and target orientation - QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); - // Keep scalar part of quaternion positive - if (errorQuat[3] < 0) { - VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); - } - // Calculate error angle - errorAngle = QuaternionOperations::getAngle(errorQuat, true); - - // Calculate error satellite rotational rate - VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); + double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0}; + comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate, + errorQuat, errorSatRotRate, errorAngle); } void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,