From 609d429161129e18581a62575327efc92b8dd482 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Tue, 22 Nov 2022 21:10:05 +0100 Subject: [PATCH] added Nadir FLP version --- mission/controller/AcsController.cpp | 2 +- mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/Guidance.cpp | 94 +++++++++++++++++++++++++- mission/controller/acs/Guidance.h | 6 ++ 4 files changed, 101 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 6e5e4473..7ef85399 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -171,7 +171,7 @@ void AcsController::performPointingCtrl() { guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); break; case SUBMODE_PTG_NADIR: - guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + guidance.quatNadirPtgFLPVersion(&sensorValues, &outputValues, now, targetQuat, refSatRate); break; case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 953f50db..3566d3fb 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -852,6 +852,7 @@ class AcsParameters /*: public HasParametersIF*/ { double nadirRefDirection[3] = {-1, 0, 0}; //Camera double refQuatInertial[4] = {0, 0, 0, 1}; double refRotRateInertial[3] = {0, 0, 0}; + int8_t nadirTimeElapsedMax = 10; } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; struct StrParameters { diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index b2f47953..0d0022fc 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -125,10 +125,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues // Calculation of reference rotation rate in case of star tracker blinding //------------------------------------------------------------------------------------- if (acsParameters.targetModeControllerParameters.avoidBlindStr) { - double sunDirJ[3] = {0, 0, 0}; double sunDirB[3] = {0, 0, 0}; if (outputValues->sunDirModelValid) { + double sunDirJ[3] = {0, 0, 0}; sunDirJ[0] = outputValues->sunDirModel[0]; sunDirJ[1] = outputValues->sunDirModel[1]; sunDirJ[2] = outputValues->sunDirModel[2]; @@ -301,6 +301,98 @@ void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues * } +void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]) { + + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for Nadir pointing + //------------------------------------------------------------------------------------- + // Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; + double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, + sensorValues->gpsSet.altitude.value, posSatE); + double targetDirE[3] = {0, 0, 0}; + VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); + + // Transformation between ECEF and IJK frame + double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + + // Target Direction in the body frame + double targetDirJ[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); + + // negative x-Axis aligned with target (Camera position) + double xAxis[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + + // z-Axis parallel to long side of picture resolution + double zAxis[3] = {0, 0, 0}; + double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]}; + double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityJ, 3); + VectorOperations::cross(xAxis, velocityJ, zAxis); + VectorOperations::normalize(zAxis, zAxis, 3); + + // y-Axis completes RHS + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(zAxis, xAxis, yAxis); + + //Complete transformation matrix + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; + QuaternionOperations::fromDcm(dcmTgt,targetQuat); + + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + double timeElapsed = now.tv_sec + now.tv_usec * pow(10,-6) - (timeSavedQuaternionNadir.tv_sec + + timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6)); + if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { + double qDiff[4] = {0, 0, 0, 0}; + VectorOperations::subtract(targetQuat, savedQuaternionNadir, qDiff, 4); + VectorOperations::mulScalar(qDiff, 1/timeElapsed, qDiff, 4); + + double tgtQuatVec[3] = {targetQuat[0], targetQuat[1], targetQuat[2]}, + 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(targetQuat, qDiff, sum1); + VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); + VectorOperations::mulScalar(qDiffVec, targetQuat[3], sum3, 3); + VectorOperations::add(sum1, sum2, sum, 3); + VectorOperations::subtract(sum, sum3, sum, 3); + double omegaRefNew[3] = {0, 0, 0}; + VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); + + VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); + VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); + omegaRefSavedNadir[0] = omegaRefNew[0]; + omegaRefSavedNadir[1] = omegaRefNew[1]; + omegaRefSavedNadir[2] = omegaRefNew[2]; + } + else { + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + } + + timeSavedQuaternionNadir = now; + savedQuaternionNadir[0] = targetQuat[0]; + savedQuaternionNadir[1] = targetQuat[1]; + savedQuaternionNadir[2] = targetQuat[2]; + savedQuaternionNadir[3] = targetQuat[3]; +} + void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { for (int i = 0; i < 4; i++) { targetQuat[i] = acsParameters.inertialModeControllerParameters.refQuatInertial[i]; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 6b0759fb..80ef6cf4 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -34,6 +34,9 @@ public: void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); + void quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]); + // Function to get the target quaternion and refence rotation rate from parameters for inertial pointing void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); @@ -48,6 +51,9 @@ public: private: AcsParameters acsParameters; bool strBlindAvoidFlag = false; + timeval timeSavedQuaternionNadir; + double savedQuaternionNadir[4] = {0, 0, 0, 0}; + double omegaRefSavedNadir[3] = {0, 0, 0}; }; #endif /* ACS_GUIDANCE_H_ */