From 8d1cbd9f8b64f00c9e40571efffb5eda3753dc72 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Thu, 24 Nov 2022 13:40:55 +0100 Subject: [PATCH] changed calculation of quaternion for target and sun pointing --- mission/controller/AcsController.cpp | 4 +- mission/controller/acs/AcsParameters.h | 4 +- mission/controller/acs/Guidance.cpp | 238 ++++++++++++++++++++++--- mission/controller/acs/Guidance.h | 8 +- 4 files changed, 221 insertions(+), 33 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 7ef85399..211bea30 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -168,10 +168,10 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); break; case SUBMODE_PTG_SUN: - guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); + guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); break; case SUBMODE_PTG_NADIR: - guidance.quatNadirPtgFLPVersion(&sensorValues, &outputValues, now, targetQuat, refSatRate); + guidance.quatNadirPtg(&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 3566d3fb..24018ad9 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -850,8 +850,8 @@ class AcsParameters /*: public HasParametersIF*/ { double omegaEarth = 0.000072921158553; double nadirRefDirection[3] = {-1, 0, 0}; //Camera - double refQuatInertial[4] = {0, 0, 0, 1}; - double refRotRateInertial[3] = {0, 0, 0}; + double tgtQuatInertial[4] = {0, 0, 0, 1}; + double tgtRotRateInertial[3] = {0, 0, 0}; int8_t nadirTimeElapsedMax = 10; } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 0d0022fc..d1799d60 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -29,7 +29,7 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); } -void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, +void Guidance::targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude @@ -181,7 +181,124 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues } } -void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, +void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]) { + + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for target pointing + //------------------------------------------------------------------------------------- + // Transform longitude, latitude and altitude to cartesian coordiantes (earth + // fixed/centered frame) + double groundStationCart[3] = {0, 0, 0}; + + MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, + acsParameters.groundStationParameters.longitudeGs, + acsParameters.groundStationParameters.altitudeGs, + groundStationCart); + // 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::subtract(groundStationCart, posSatE, 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 and position vector in the inertial frame + double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); + MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); + + // negative x-Axis aligned with target (Camera/E-band transmitter position) + double xAxis[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + + // Transform velocity into inertial frame + 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); + + // orbital normal vector + double orbitalNormalJ[3] = {0, 0, 0}; + VectorOperations::cross(posSatJ, velocityJ, orbitalNormalJ); + VectorOperations::normalize(orbitalNormalJ, orbitalNormalJ, 3); + + // y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(orbitalNormalJ, xAxis, yAxis); + VectorOperations::normalize(yAxis, yAxis, 3); + + // z-Axis completes RHS + double zAxis[3] = {0, 0, 0}; + VectorOperations::cross(xAxis, yAxis, zAxis); + + //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]}}; + double quatInertialTarget[4] = {0, 0, 0, 0}; + QuaternionOperations::fromDcm(dcmTgt,quatInertialTarget); + + //------------------------------------------------------------------------------------- + // 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(quatInertialTarget, savedQuaternionNadir, 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 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::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); + VectorOperations::mulScalar(qDiffVec, quatInertialTarget[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] = quatInertialTarget[0]; + savedQuaternionNadir[1] = quatInertialTarget[1]; + savedQuaternionNadir[2] = quatInertialTarget[2]; + savedQuaternionNadir[3] = quatInertialTarget[3]; + + // Transform in system relative to satellite frame + double quatBJ[4] = {0, 0, 0, 0}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); +} + +void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun @@ -194,30 +311,34 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou quatBJ[3] = outputValues->quatMekfBJ[3]; QuaternionOperations::toDcm(quatBJ, dcmBJ); - double sunDirJ[3] = {0, 0, 0}, sunDir[3] = {0, 0, 0}; + double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0}; if (outputValues->sunDirModelValid) { sunDirJ[0] = outputValues->sunDirModel[0]; sunDirJ[1] = outputValues->sunDirModel[1]; sunDirJ[2] = outputValues->sunDirModel[2]; - MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDir, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); } else { - sunDir[0] = outputValues->sunDirEst[0]; - sunDir[1] = outputValues->sunDirEst[1]; - sunDir[2] = outputValues->sunDirEst[2]; + sunDirB[0] = outputValues->sunDirEst[0]; + sunDirB[1] = outputValues->sunDirEst[1]; + sunDirB[2] = outputValues->sunDirEst[2]; } + /* + // --------------------------------------------------------------------------- + // Old version of two vector quaternion (only one axis to align) + // --------------------------------------------------------------------------- double sunRef[3] = {0, 0, 0}; sunRef[0] = acsParameters.safeModeControllerParameters.sunTargetDir[0]; sunRef[1] = acsParameters.safeModeControllerParameters.sunTargetDir[1]; sunRef[2] = acsParameters.safeModeControllerParameters.sunTargetDir[2]; double sunCross[3] = {0, 0, 0}; - VectorOperations::cross(sunDir, sunRef, sunCross); - double normSunDir = VectorOperations::norm(sunDir, 3); + VectorOperations::cross(sunDirB, sunRef, sunCross); + double normSunDir = VectorOperations::norm(sunDirB, 3); double normSunRef = VectorOperations::norm(sunRef, 3); - double dotSun = VectorOperations::dot(sunDir, sunRef); + double dotSun = VectorOperations::dot(sunDirB, sunRef); targetQuat[0] = sunCross[0]; targetQuat[1] = sunCross[1]; @@ -225,17 +346,73 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou targetQuat[3] = sqrt(pow(normSunDir,2) * pow(normSunRef,2) + dotSun); VectorOperations::normalize(targetQuat, targetQuat, 4); + */ - //------------------------------------------------------------------------------------- + //---------------------------------------------------------------------------- + // New version + //---------------------------------------------------------------------------- + // 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); + + // positive z-Axis of EIVE in direction of sun + double zAxis[3] = {0 ,0 ,0}; + VectorOperations::normalize(sunDirB, zAxis, 3); + + // Position of the satellite in the earth/fixed frame via GPS and body + // velocity + 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 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); + double velocityB[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmBJ, velocityJ, velocityB, 3, 3, 1); + + // Normale to velocity and sunDir + double crossVelSun[3] = {0, 0, 0}; + VectorOperations::cross(velocityB, sunDirB, crossVelSun); + + // y- Axis as cross of normal velSun and zAxis + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(crossVelSun, sunDirB, yAxis); + VectorOperations::normalize(yAxis, yAxis, 3); + + // complete RHS for x-Axis + double xAxis[3] = {0, 0, 0}; + VectorOperations::cross(yAxis, zAxis, xAxis); + + // Transformation matrix to Sun, no further transforamtions, reference is already + // the EIVE body frame + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; + double quatSun[4] = {0, 0, 0, 0}; + QuaternionOperations::fromDcm(dcmTgt,quatSun); + + targetQuat[0] = quatSun[0]; + targetQuat[1] = quatSun[1]; + targetQuat[2] = quatSun[2]; + targetQuat[3] = quatSun[3]; + + //---------------------------------------------------------------------------- // Calculation of reference rotation rate - //------------------------------------------------------------------------------------- + //---------------------------------------------------------------------------- refSatRate[0] = 0; refSatRate[1] = 0; refSatRate[2] = 0; } -void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- @@ -301,7 +478,7 @@ void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues * } -void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, +void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- @@ -351,7 +528,8 @@ void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::Outp //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); + double quatInertialTarget[4] = {0, 0, 0, 0}; + QuaternionOperations::fromDcm(dcmTgt,quatInertialTarget); //------------------------------------------------------------------------------------- // Calculation of reference rotation rate @@ -360,15 +538,15 @@ void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::Outp 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::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); VectorOperations::mulScalar(qDiff, 1/timeElapsed, qDiff, 4); - double tgtQuatVec[3] = {targetQuat[0], targetQuat[1], targetQuat[2]}, + double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[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::cross(quatInertialTarget, qDiff, sum1); VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, targetQuat[3], sum3, 3); + VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); VectorOperations::add(sum1, sum2, sum, 3); VectorOperations::subtract(sum, sum3, sum, 3); double omegaRefNew[3] = {0, 0, 0}; @@ -387,18 +565,26 @@ void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::Outp } timeSavedQuaternionNadir = now; - savedQuaternionNadir[0] = targetQuat[0]; - savedQuaternionNadir[1] = targetQuat[1]; - savedQuaternionNadir[2] = targetQuat[2]; - savedQuaternionNadir[3] = targetQuat[3]; + savedQuaternionNadir[0] = quatInertialTarget[0]; + savedQuaternionNadir[1] = quatInertialTarget[1]; + savedQuaternionNadir[2] = quatInertialTarget[2]; + savedQuaternionNadir[3] = quatInertialTarget[3]; + + // Transform in system relative to satellite frame + double quatBJ[4] = {0, 0, 0, 0}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { for (int i = 0; i < 4; i++) { - targetQuat[i] = acsParameters.inertialModeControllerParameters.refQuatInertial[i]; + targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i]; } for (int i = 0; i < 3; i++) { - refSatRate[i] = acsParameters.inertialModeControllerParameters.refRotRateInertial[i]; + refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i]; } } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 80ef6cf4..ba43f395 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -23,18 +23,20 @@ public: void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); // Function to get the target quaternion and refence rotation rate from gps position and position of the ground station + void targetQuatPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]); void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate for sun pointing after ground station - void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, + void sunQuatPtg(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 gps position for Nadir pointing - void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + void quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); - void quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + void quatNadirPtg(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