From bd15d7b0e2ec3da56ccda529ff09a82b14168bc1 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 13:28:59 +0200 Subject: [PATCH] fixed calculation of rotational rate for ptgCtrl which also is used for idle now --- mission/controller/acs/Guidance.cpp | 29 +++++++++++++++++------------ mission/controller/acs/Guidance.h | 7 ++++--- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 329c6bdc..98dd56fa 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -266,7 +266,8 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- @@ -296,9 +297,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double //---------------------------------------------------------------------------- // Calculation of reference rotation rate //---------------------------------------------------------------------------- - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; + int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], @@ -453,20 +453,25 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua //------------------------------------------------------------------------------------- // Calculation of target rotation rate //------------------------------------------------------------------------------------- - double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - - (timeSavedQuaternion.tv_sec + - timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); + double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 - + (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6); + if (VectorOperations::norm(savedQuaternion, 4) == 0) { + std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); + } if (timeElapsed < timeElapsedMax) { + double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0}; + QuaternionOperations::inverse(quatInertialTarget, q); + QuaternionOperations::inverse(savedQuaternion, qS); double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); + VectorOperations::subtract(q, qS, qDiff, 4); VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); - double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, - qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double tgtQuatVec[3] = {q[0], q[1], q[2]}; + double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(quatInertialTarget, qDiff, sum1); + VectorOperations::cross(tgtQuatVec, qDiffVec, sum1); VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::mulScalar(qDiffVec, q[3], sum3, 3); VectorOperations::add(sum1, sum2, sum, 3); VectorOperations::subtract(sum, sum3, sum, 3); double omegaRefNew[3] = {0, 0, 0}; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 39e77456..7b81e411 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -15,7 +15,7 @@ class Guidance { void getTargetParamsSafe(double sunTargetSafe[3]); ReturnValue_t solarArrayDeploymentComplete(); - // Function to get the target quaternion and refence rotation rate from gps position and + // Function to get the target quaternion and reference rotation rate from gps position and // position of the ground station void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], double refDirB[3], double quatBI[4], double targetQuat[4], @@ -25,9 +25,10 @@ class Guidance { void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); - // Function to get the target quaternion and refence rotation rate for sun pointing after ground + // Function to get the target quaternion and reference rotation rate for sun pointing after ground // station - void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]); + void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing