diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ef3c6cf4..9e400e75 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -51,7 +51,7 @@ void AcsController::performControlOperation() { case SUBMODE_DETUMBLE: performDetumble(); break; - case SUBMODE_PTG_SUN: + case SUBMODE_PTG_SUN: case SUBMODE_PTG_TARGET: case SUBMODE_PTG_NADIR: case SUBMODE_PTG_INERTIAL: @@ -248,9 +248,10 @@ void AcsController::performPointingCtrl() { &mekfData, &validMekf); double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - switch (submode) { + switch (submode) { case SUBMODE_PTG_TARGET: - guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate); + guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, + refSatRate); break; case SUBMODE_PTG_SUN: guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); @@ -261,7 +262,8 @@ void AcsController::performPointingCtrl() { case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); break; - } double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, + } + double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -277,16 +279,18 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { - bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? - int32_t rwSpeed[4] = {(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), - (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; - ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); + bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? + int32_t rwSpeed[4] = { + (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), + (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; + ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); } double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); + actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), + &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), + &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 7315ca89..88c4ddb0 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -26,22 +26,22 @@ class AcsParameters /*: public HasParametersIF*/ { } onBoardParams; struct InertiaEIVE { - double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, - {-0.0001821456, 0.1701302, 0.0004748963}, - {-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021 + double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, + {-0.0001821456, 0.1701302, 0.0004748963}, + {-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021 // Possible inertia matrices - double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, - {-0.0001821456, 0.1701302, 0.0004748963}, - {-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021 - double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008}, - {-0.0001798426, 0.162240, 0.000475596}, - {-0.005008, 0.000475596, 0.060136}}; //19.11.2021 - double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207}, - {-0.0001836122, 0.16619787, 0.0083537}, - {-0.00501207, 0.0083537, 0.07192588}}; //19.11.2021 - double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767}, - {-0.000178376, 0.166172, -0.007403}, - {-0.005009767, -0.007403, 0.07195314}}; + double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, + {-0.0001821456, 0.1701302, 0.0004748963}, + {-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021 + double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008}, + {-0.0001798426, 0.162240, 0.000475596}, + {-0.005008, 0.000475596, 0.060136}}; // 19.11.2021 + double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207}, + {-0.0001836122, 0.16619787, 0.0083537}, + {-0.00501207, 0.0083537, 0.07192588}}; // 19.11.2021 + double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767}, + {-0.000178376, 0.166172, -0.007403}, + {-0.005009767, -0.007403, 0.07195314}}; } inertiaEIVE; struct MgmHandlingParameters { @@ -786,9 +786,9 @@ class AcsParameters /*: public HasParametersIF*/ { double rw2orientationMatrix[3][3]; double rw3orientationMatrix[3][3]; double inertiaWheel = 0.000028198; - double maxTrq = 0.0032; // 3.2 [mNm] - double stictionSpeed = 80; //RPM - double stictionReleaseSpeed = 120; //RPM + double maxTrq = 0.0032; // 3.2 [mNm] + double stictionSpeed = 80; // RPM + double stictionReleaseSpeed = 120; // RPM double stictionTorque = 0.0006; } rwHandlingParameters; @@ -796,31 +796,21 @@ class AcsParameters /*: public HasParametersIF*/ { double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000}, {0.0000, -0.9205, 0.0000, 0.9205}, {0.3907, 0.3907, 0.3907, 0.3907}}; -// double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, -// {0.2136, -0.3317, 1.0123}, -// {-0.8672, -0.1406, 0.1778}, -// {0.6426, 0.4794, 1.3603}}; -// where does the first pseudo inverse come frome - matlab gives result below - double pseudoInverse[4][3] = {{0.5432, 0, 0.6398}, - {0, -0.5432, 0.6398}, - {-0.5432, 0, 0.6398}, - {0, 0.5432, 0.6398}}; - double without0[4][3] = {{0, 0, 0}, - {0.5432, -0.5432, 1.2797}, - {-1.0864, 0, 0}, - {0.5432, 0.5432, 1.2797}}; - double without1[4][3] = {{0.5432, -0.5432, 1.2797}, - {0, 0, 0}, - {-0.5432, -0.5432, 1.2797}, - {0, 1.0864, 0}}; - double without2[4][3] = {{1.0864, 0, 0}, - {-0.5432, -0.5432, 1.2797}, - {0, 0, 0}, - {-0.5432, 0.5432, 1.2797}}; - double without3[4][3] = {{0.5432, 0.5432, 1.2797}, - {0, -1.0864, 0}, - {-0.5432, 0.5432, 1.2797}, - {0, 0, 0}}; + // double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, + // {0.2136, -0.3317, 1.0123}, + // {-0.8672, -0.1406, 0.1778}, + // {0.6426, 0.4794, 1.3603}}; + // where does the first pseudo inverse come frome - matlab gives result below + double pseudoInverse[4][3] = { + {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}}; + double without0[4][3] = { + {0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}}; + double without1[4][3] = { + {0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}}; + double without2[4][3] = { + {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; + double without3[4][3] = { + {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000}; } rwMatrices; @@ -828,9 +818,11 @@ class AcsParameters /*: public HasParametersIF*/ { double k_rate_mekf = 0.00059437; double k_align_mekf = 0.000056875; - double k_rate_no_mekf = 0.00059437;; - double k_align_no_mekf = 0.000056875;; - double sunMagAngleMin; // ??? + double k_rate_no_mekf = 0.00059437; + ; + double k_align_no_mekf = 0.000056875; + ; + double sunMagAngleMin; // ??? double sunTargetDir[3] = {0, 0, 1}; double satRateRef[3] = {0, 0, 0}; @@ -839,7 +831,7 @@ class AcsParameters /*: public HasParametersIF*/ { // ToDo: mutiple structs for different pointing mode controllers? struct PointingModeControllerParameters { - double refDirection[3] = {-1, 0, 0}; //Antenna + double refDirection[3] = {-1, 0, 0}; // Antenna double refRotRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 1}; bool avoidBlindStr = true; @@ -860,11 +852,12 @@ class AcsParameters /*: public HasParametersIF*/ { double omegaEarth = 0.000072921158553; - double nadirRefDirection[3] = {-1, 0, 0}; //Camera + double nadirRefDirection[3] = {-1, 0, 0}; // Camera double tgtQuatInertial[4] = {0, 0, 0, 1}; double tgtRotRateInertial[3] = {0, 0, 0}; int8_t nadirTimeElapsedMax = 10; - } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; + } pointingModeControllerParameters, inertialModeControllerParameters, + nadirModeControllerParameters, targetModeControllerParameters; struct StrParameters { double exclusionAngle = 20 * M_PI / 180; @@ -872,7 +865,7 @@ class AcsParameters /*: public HasParametersIF*/ { } strParameters; struct GpsParameters { - double timeDiffVelocityMax = 30; //[s] + double timeDiffVelocityMax = 30; //[s] } gpsParameters; struct GroundStationParameters { @@ -883,10 +876,11 @@ class AcsParameters /*: public HasParametersIF*/ { struct SunModelParameters { float domega = 36000.771; - float omega_0 = 280.46 * M_PI / 180.; //282.94 * M_PI / 180.; // RAAN plus argument of perigee - float m_0 = 357.5277; //357.5256; // coefficients for mean anomaly - float dm = 35999.049; // coefficients for mean anomaly - float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis + float omega_0 = 280.46 * M_PI / 180.; // 282.94 * M_PI / 180.; // RAAN plus argument of + // perigee + float m_0 = 357.5277; // 357.5256; // coefficients for mean anomaly + float dm = 35999.049; // coefficients for mean anomaly + float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis float e1 = 0.74508 * M_PI / 180.; float p1 = 6892. / 3600. * M_PI / 180.; // some parameter @@ -899,8 +893,8 @@ class AcsParameters /*: public HasParametersIF*/ { double sensorNoiseMAG = 4 * M_PI / 180; double sensorNoiseGYR = 0.1 * M_PI / 180; - double sensorNoiseArwGYR = 3*0.0043*M_PI / sqrt(10) / 180; // Angular Random Walk - double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability + double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk + double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability } kalmanFilterParameters; struct MagnetorquesParameter { @@ -914,7 +908,7 @@ class AcsParameters /*: public HasParametersIF*/ { } magnetorquesParameter; struct DetumbleParameter { - uint8_t detumblecounter = 75; // 30 s + uint8_t detumblecounter = 75; // 30 s double omegaDetumbleStart = 2 * M_PI / 180; double omegaDetumbleEnd = 0.4 * M_PI / 180; double gainD = pow(10.0, -3.3); diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index f8c9b249..4b3bb28c 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -33,8 +33,8 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { } if (maxValue > maxTrq) { - double scalingFactor = maxTrq / maxValue; - VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); + double scalingFactor = maxTrq / maxValue; + VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index cd61e639..918bce9b 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -6,38 +6,38 @@ #include "SensorProcessing.h" #include "SensorValues.h" -class ActuatorCmd{ -public: - ActuatorCmd(AcsParameters *acsParameters_); //Input mode ? - virtual ~ActuatorCmd(); +class ActuatorCmd { + public: + ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? + virtual ~ActuatorCmd(); - /* - * @brief: scalingTorqueRws() scales the torque via maximum part in case this part is higher - * then the maximum torque - * @param: rwTrq given torque for reaction wheels - * rwTrqScaled possible scaled torque - */ - void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); + /* + * @brief: scalingTorqueRws() scales the torque via maximum part in case this part is + * higher then the maximum torque + * @param: rwTrq given torque for reaction wheels + * rwTrqScaled possible scaled torque + */ + void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); - /* - * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction wheels, also - * will calculate the needed revolutions per minute for the RWs, which will be given - * as Input to the RWs - * @param: rwTrqIn given torque from pointing controller - * rwTrqNS Nullspace torque - * rwCmdSpeed output revolutions per minute for every reaction wheel - */ - void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTorque, - double *rwCmdSpeed); + /* + * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction + * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given + * as Input to the RWs + * @param: rwTrqIn given torque from pointing controller + * rwTrqNS Nullspace torque + * rwCmdSpeed output revolutions per minute for every + * reaction wheel + */ + void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, + const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed); - /* - * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques - * - * @param: dipolMoment given dipol moment in spacecraft frame - * dipolMomentUnits resulting dipol moment for every unit - */ - void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits); + /* + * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques + * + * @param: dipolMoment given dipol moment in spacecraft frame + * dipolMomentUnits resulting dipol moment for every unit + */ + void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits); protected: private: diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index eeb161d0..199fce9c 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -47,9 +47,9 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData // 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, + 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); // Target direction in the ECEF frame @@ -174,125 +174,128 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData } } -void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]) { +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}; - //------------------------------------------------------------------------------------- - // 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); - 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); - // 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); - 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); - // 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); - // 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); - // 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); - // 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); - // 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); - // 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); - //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); - //------------------------------------------------------------------------------------- - // 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); - 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; + } - 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]; - 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); + // 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]) { +void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- @@ -354,17 +357,18 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); // positive z-Axis of EIVE in direction of sun - double zAxis[3] = {0 ,0 ,0}; + 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, + 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 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); @@ -387,9 +391,11 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou // 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 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); + QuaternionOperations::fromDcm(dcmTgt, quatSun); targetQuat[0] = quatSun[0]; targetQuat[1] = quatSun[1]; @@ -404,16 +410,18 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou refSatRate[2] = 0; } -void Guidance::quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing +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 //------------------------------------------------------------------------------------- // 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, + 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); @@ -468,116 +476,118 @@ void Guidance::quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::Outp 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::quatNadirPtg(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); - //------------------------------------------------------------------------------------- - // 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); - // 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); - 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); - // 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); - // 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); - // 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); - // 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]}}; + double quatInertialTarget[4] = {0, 0, 0, 0}; + QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - //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); - //------------------------------------------------------------------------------------- - // 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); - 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; + } - 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]; - 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); + // 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.tgtQuatInertial[i]; + targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i]; } for (int i = 0; i < 3; i++) { - refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i]; + refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i]; } } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 234627cf..1b6bfcb6 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -21,27 +21,31 @@ class Guidance { 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, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], - double refSatRate[3]); - void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + // Function to get the target quaternion and refence rotation rate from gps position and position + // of the ground station + void targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, timeval now, + double targetQuat[4], double refSatRate[3]); + void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, 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, 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, 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 quatNadirPtgOldVersion(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 quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + timeval now, double targetQuat[4], double refSatRate[3]); - void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]); + 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 - void inertialQuatPtg(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]); // @note: compares target Quaternion and reference quaternion, also actual satellite rate and // desired diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index f5cded71..ea2c1044 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -114,21 +114,15 @@ class Igrf13Model /*:public HasParametersIF*/ { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT - - double schmidtFactors[14][13] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}}; + + double schmidtFactors[14][13] = { + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}}; ; double updatedG[14][13]; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 73d5fa81..5ce2cb77 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -574,11 +574,13 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // Calculation of the satellite velocity in earth fixed frame double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}; // RADIANS OR DEGREE ? Function needs rad as input - MathOperations::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude, posSatE); - if (validSavedPosSatE && (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { - VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); - double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE; - VectorOperations::mulScalar(deltaDistance, 1/timeDiffGpsMeas, gpsVelocityE, 3); + MathOperations::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude, + posSatE); + if (validSavedPosSatE && + (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { + VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); + double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE; + VectorOperations::mulScalar(deltaDistance, 1 / timeDiffGpsMeas, gpsVelocityE, 3); } } { @@ -600,7 +602,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong validSavedPosSatE = true; } else { - validGcLatitude = false; + validGcLatitude = false; } } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index f47ce86c..16bf82bc 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -35,15 +35,14 @@ class Detumble { ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); - ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); + ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); - ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, - const double *magField, const bool *magFieldValid, - double *magMom); + ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField, + const bool *magFieldValid, double *magMom); -private: - AcsParameters::DetumbleParameter* detumbleParameter; - AcsParameters::MagnetorquesParameter* magnetorquesParameter; + private: + AcsParameters::DetumbleParameter *detumbleParameter; + AcsParameters::MagnetorquesParameter *magnetorquesParameter; }; #endif /*ACS_CONTROL_DETUMBLE_H_*/ diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 64666be1..9d4f8d6d 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -16,9 +16,7 @@ #include "../util/MathOperations.h" -PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){ - loadAcsParameters(acsParameters_); -} +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } PtgCtrl::~PtgCtrl() {} @@ -29,7 +27,8 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { rwMatrices = &(acsParameters_->rwMatrices); } -void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){ +void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate, + const double *rwPseudoInv, double *torqueRws) { //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix //------------------------------------------------------------------------------------------------ @@ -84,30 +83,28 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt double pErrorSign[3] = {0, 0, 0}; for (int i = 0; i < 3; i++) { - if (abs(pError[i]) > 1) { - pErrorSign[i] = sign(pError[i]); - } - else { - pErrorSign[i] = pError[i]; - } - } -// Torque for quaternion error - double torqueQuat[3] = {0, 0, 0}; - MatrixOperations::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); - VectorOperations::mulScalar(torqueQuat, -1, torqueQuat, 3); + if (abs(pError[i]) > 1) { + pErrorSign[i] = sign(pError[i]); + } else { + pErrorSign[i] = pError[i]; + } + } + // Torque for quaternion error + double torqueQuat[3] = {0, 0, 0}; + MatrixOperations::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); + VectorOperations::mulScalar(torqueQuat, -1, torqueQuat, 3); -// Torque for rate error - double torqueRate[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); - VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); - VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); - -// Final commanded Torque for every reaction wheel - double torque[3] = {0, 0, 0}; - VectorOperations::add(torqueRate, torqueQuat, torque, 3); - MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); - VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); + // Torque for rate error + double torqueRate[3] = {0, 0, 0}; + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); + VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); + VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); + // Final commanded Torque for every reaction wheel + double torque[3] = {0, 0, 0}; + VectorOperations::add(torqueRate, torqueQuat, torque, 3); + MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); + VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, @@ -162,34 +159,31 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } -void PtgCtrl::rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, - double* torqueCommand) { - for (uint8_t i = 0; i < 4; i++) { - if (rwAvailable[i]) { - if (torqueMemory[i] != 0) { - if ((omegaRw[i] * torqueMemory[i]) - > rwHandlingParameters->stictionReleaseSpeed) { - torqueMemory[i] = 0; - } else { - torqueCommand[i] = torqueMemory[i] - * rwHandlingParameters->stictionTorque; - } - } else { - if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) - && (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { - if (omegaRw[i] < omegaMemory[i]) { - torqueMemory[i] = -1; - } else { - torqueMemory[i] = 1; - } +void PtgCtrl::rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, + double *torqueCommand) { + for (uint8_t i = 0; i < 4; i++) { + if (rwAvailable[i]) { + if (torqueMemory[i] != 0) { + if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) { + torqueMemory[i] = 0; + } else { + torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; + } + } else { + if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) && + (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { + if (omegaRw[i] < omegaMemory[i]) { + torqueMemory[i] = -1; + } else { + torqueMemory[i] = 1; + } - torqueCommand[i] = torqueMemory[i] - * rwHandlingParameters->stictionTorque; - } - } - } else { - torqueMemory[i] = 0; - } - omegaMemory[i] = omegaRw[i]; - } - } + torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; + } + } + } else { + torqueMemory[i] = 0; + } + omegaMemory[i] = omegaRw[i]; + } +} diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 24ccafae..d176b0c2 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -42,7 +42,7 @@ class PtgCtrl { * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ void ptgLaw(const double mode, const double *qError, const double *deltaRate, - const double *rwPseudoInv, double *torqueRws); + const double *rwPseudoInv, double *torqueRws); void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, @@ -56,8 +56,7 @@ class PtgCtrl { * omegaRw current wheel speed of reaction wheels * torqueCommand modified torque after antistiction */ - void rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, - double* torqueCommand); + void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand); private: AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 90056927..56665c4d 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -64,12 +64,13 @@ class MathOperations { } } - static void convertDateToJD2000(const T1 time, T2 julianDate){ - // time = { Y, M, D, h, m,s} - // time in sec and microsec -> The Epoch (unixtime) - julianDate = 1721013.5 + 367*time[0]- floor(7/4*(time[0]+(time[1]+9)/12)) - +floor(275*time[1]/9)+time[2]+(60*time[3]+time[4]+(time(5)/60))/1440; - } + static void convertDateToJD2000(const T1 time, T2 julianDate) { + // time = { Y, M, D, h, m,s} + // time in sec and microsec -> The Epoch (unixtime) + julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) + + floor(275 * time[1] / 9) + time[2] + + (60 * time[3] + time[4] + (time(5) / 60)) / 1440; + } static T1 convertUnixToJD2000(timeval time) { // time = {{s},{us}} @@ -93,43 +94,42 @@ class MathOperations { outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2); } - static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){ - /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude, - * longitude and altitude - * @param: lat geodetic latitude [rad] - * longi longitude [rad] - * alt altitude [m] - * cartesianOutput Cartesian Coordinates in ECEF (3x1) - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff - * Landis Markley and John L. Crassidis*/ - double radiusPolar = 6356752.314; - double radiusEqua = 6378137; + static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, + T2 *cartesianOutput) { + /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude, + * longitude and altitude + * @param: lat geodetic latitude [rad] + * longi longitude [rad] + * alt altitude [m] + * cartesianOutput Cartesian Coordinates in ECEF (3x1) + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff + * Landis Markley and John L. Crassidis*/ + double radiusPolar = 6356752.314; + double radiusEqua = 6378137; - double eccentricity = sqrt(1 - pow(radiusPolar,2) / pow(radiusEqua,2)); - double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity,2) * pow(sin(lat),2)); + double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2)); + double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2)); - cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi); - cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); - cartesianOutput[2] = ((1 - pow(eccentricity,2)) * auxRadius + alt) * sin(lat); - - } - static void dcmEJ(timeval time, T1 * outputDcmEJ, T1 * outputDotDcmEJ){ - /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame - * @param: time Current time - * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] - * outputDotDcmEJ Derivative of transformation matrix [3][3] - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff - * Landis Markley and John L. Crassidis*/ - double JD2000Floor = 0; - double JD2000 = convertUnixToJD2000(time); - // Getting Julian Century from Day start : JD (Y,M,D,0,0,0) - JD2000Floor = floor(JD2000); - if ( ( JD2000 - JD2000Floor) < 0.5) { - JD2000Floor -= 0.5; - } - else { - JD2000Floor += 0.5; - } + cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi); + cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); + cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat); + } + static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { + /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame + * @param: time Current time + * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * outputDotDcmEJ Derivative of transformation matrix [3][3] + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff + * Landis Markley and John L. Crassidis*/ + double JD2000Floor = 0; + double JD2000 = convertUnixToJD2000(time); + // Getting Julian Century from Day start : JD (Y,M,D,0,0,0) + JD2000Floor = floor(JD2000); + if ((JD2000 - JD2000Floor) < 0.5) { + JD2000Floor -= 0.5; + } else { + JD2000Floor += 0.5; + } double JC2000 = JD2000Floor / 36525; double sec = (JD2000 - JD2000Floor) * 86400; @@ -152,150 +152,152 @@ class MathOperations { outputDcmEJ[7] = 0; outputDcmEJ[8] = 1; - // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION - double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]}, - {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]}, - {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}}; - double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; - double omegaEarth = 0.000072921158553; - double dotDcmEJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - MatrixOperations::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3); - MatrixOperations::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3); + // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION + double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]}, + {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]}, + {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}}; + double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; + double omegaEarth = 0.000072921158553; + double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3); + MatrixOperations::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3); } + /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame + * give also the back the derivative of this matrix + * @param: unixTime Current time in Unix format + * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * outputDotDcmEJ Derivative of transformation matrix [3][3] + * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und + Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff + * Oliver Zeile + * + https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/ + static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { + // TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 + //(initial Offset) International Atomic Time (TAI) - /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame - * give also the back the derivative of this matrix - * @param: unixTime Current time in Unix format - * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] - * outputDotDcmEJ Derivative of transformation matrix [3][3] - * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und - Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff - * Oliver Zeile - * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/ - static void ecfToEciWithNutPre(timeval unixTime, T1 * outputDcmEJ, T1 * outputDotDcmEJ ) { + double JD2000UTC1 = convertUnixToJD2000(unixTime); -// TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 (initial Offset) -// International Atomic Time (TAI) + // Julian Date / century from TT + timeval terestrialTime = unixTime; + terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37; + double JD2000TT = convertUnixToJD2000(terestrialTime); + double JC2000TT = JD2000TT / 36525; - double JD2000UTC1 = convertUnixToJD2000(unixTime); + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth rotation Theta + //------------------------------------------------------------------------------------- + double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + // Earth Rotation angle + double era = 0; + era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1); + // Greenwich Mean Sidereal Time + double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) - + 0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4); + double arcsecFactor = 1 * PI / (180 * 3600); + gmst2000 *= arcsecFactor; + gmst2000 += era; -// Julian Date / century from TT - timeval terestrialTime = unixTime; - terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37; - double JD2000TT = convertUnixToJD2000(terestrialTime); - double JC2000TT = JD2000TT / 36525; + theta[0][0] = cos(gmst2000); + theta[0][1] = sin(gmst2000); + theta[0][2] = 0; + theta[1][0] = -sin(gmst2000); + theta[1][1] = cos(gmst2000); + theta[1][2] = 0; + theta[2][0] = 0; + theta[2][1] = 0; + theta[2][2] = 1; -//------------------------------------------------------------------------------------- -// Calculation of Transformation from earth rotation Theta -//------------------------------------------------------------------------------------- - double theta[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; -// Earth Rotation angle - double era = 0; - era = 2* PI *(0.779057273264 + 1.00273781191135448 * JD2000UTC1); -// Greenwich Mean Sidereal Time - double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT,2) - - 0.00009344 * pow(JC2000TT,3) + 0.00001882 * pow(JC2000TT,4); - double arcsecFactor = 1 * PI / (180 * 3600); - gmst2000 *= arcsecFactor; - gmst2000 += era; + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth Precession P + //------------------------------------------------------------------------------------- + double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - theta[0][0] = cos(gmst2000); - theta[0][1] = sin(gmst2000); - theta[0][2] = 0; - theta[1][0] = -sin(gmst2000); - theta[1][1] = cos(gmst2000); - theta[1][2] = 0; - theta[2][0] = 0; - theta[2][1] = 0; - theta[2][2] = 1; + double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3); + double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3); + double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3); -//------------------------------------------------------------------------------------- -// Calculation of Transformation from earth Precession P -//------------------------------------------------------------------------------------- - double precession[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + zeta *= arcsecFactor; + theta2 *= arcsecFactor; + ze *= arcsecFactor; - double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT,2) + 0.017998 * pow(JC2000TT,3); - double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT,2) - 0.041833 * pow(JC2000TT,3); - double ze = zeta + 0.79280 * pow(JC2000TT,2) + 0.000205 * pow(JC2000TT,3); + precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta); + precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta); + precession[2][0] = sin(theta2) * cos(zeta); + precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta); + precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta); + precession[2][1] = -sin(theta2) * sin(zeta); + precession[0][2] = -cos(ze) * sin(theta2); + precession[1][2] = -sin(ze) * sin(theta2); + precession[2][2] = cos(theta2); - zeta *= arcsecFactor; - theta2 *= arcsecFactor; - ze *= arcsecFactor; + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth Nutation N + //------------------------------------------------------------------------------------- + double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + // lunar asc node + double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT + + 7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3); + Om *= arcsecFactor; + // delta psi approx + double dp = -17.2 * arcsecFactor * sin(Om); - precession[0][0]=-sin(ze)*sin(zeta)+cos(ze)*cos(theta2)*cos(zeta); - precession[1][0]=cos(ze)*sin(zeta)+sin(ze)*cos(theta2)*cos(zeta); - precession[2][0]=sin(theta2)*cos(zeta); - precession[0][1]=-sin(ze)*cos(zeta)-cos(ze)*cos(theta2)*sin(zeta); - precession[1][1]=cos(ze)*cos(zeta)-sin(ze)*cos(theta2)*sin(zeta); - precession[2][1]=-sin(theta2)*sin(zeta); - precession[0][2]=-cos(ze)*sin(theta2); - precession[1][2]=-sin(ze)*sin(theta2); - precession[2][2]=cos(theta2); + // delta eps approx + double de = 9.203 * arcsecFactor * cos(Om); -//------------------------------------------------------------------------------------- -// Calculation of Transformation from earth Nutation N -//------------------------------------------------------------------------------------- - double nutation[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; -// lunar asc node - double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT + - 7.455 * pow(JC2000TT,2) + 0.008 * pow(JC2000TT,3); - Om *= arcsecFactor; -// delta psi approx - double dp = -17.2 * arcsecFactor *sin(Om); + // % true obliquity of the ecliptic eps p.71 (simplified) + double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; -// delta eps approx - double de = 9.203 * arcsecFactor *cos(Om); + nutation[0][0] = cos(dp); + nutation[1][0] = cos(e + de) * sin(dp); + nutation[2][0] = sin(e + de) * sin(dp); + nutation[0][1] = -cos(e) * sin(dp); + nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de); + nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de); + nutation[0][2] = -sin(e) * sin(dp); + nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de); + nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de); -// % true obliquity of the ecliptic eps p.71 (simplified) - double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; + //------------------------------------------------------------------------------------- + // Calculation of Derivative of rotation matrix from earth + //------------------------------------------------------------------------------------- + double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; + double omegaEarth = 0.000072921158553; + MatrixOperations::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); + MatrixOperations::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); - nutation[0][0]=cos(dp); - nutation[1][0]=cos(e+de)*sin(dp); - nutation[2][0]=sin(e+de)*sin(dp); - nutation[0][1]=-cos(e)*sin(dp); - nutation[1][1]=cos(e)*cos(e+de)*cos(dp)+sin(e)*sin(e+de); - nutation[2][1]=cos(e)*sin(e+de)*cos(dp)-sin(e)*cos(e+de); - nutation[0][2]=-sin(e)*sin(dp); - nutation[1][2]=sin(e)*cos(e+de)*cos(dp)-cos(e)*sin(e+de); - nutation[2][2]=sin(e)*sin(e+de)*cos(dp)+cos(e)*cos(e+de); + //------------------------------------------------------------------------------------- + // Calculation of transformation matrix and Derivative of transformation matrix + //------------------------------------------------------------------------------------- + double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3); + MatrixOperations::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3); -//------------------------------------------------------------------------------------- -// Calculation of Derivative of rotation matrix from earth -//------------------------------------------------------------------------------------- - double thetaDot[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - double dotMatrix[3][3] = {{0,1,0},{-1,0,0},{0,0,0}}; - double omegaEarth = 0.000072921158553; - MatrixOperations::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); - MatrixOperations::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); + MatrixOperations::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); + } + static void inverseMatrixDimThree(const T1 *matrix, T1 *output) { + int i, j; + double determinant; + double mat[3][3] = {{matrix[0], matrix[1], matrix[2]}, + {matrix[3], matrix[4], matrix[5]}, + {matrix[6], matrix[7], matrix[8]}}; -//------------------------------------------------------------------------------------- -// Calculation of transformation matrix and Derivative of transformation matrix -//------------------------------------------------------------------------------------- - double nutationPrecession[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - MatrixOperations::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3); - MatrixOperations::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3); - - MatrixOperations::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); - - } - static void inverseMatrixDimThree(const T1 *matrix, T1 * output){ - int i,j; - double determinant; - double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},{matrix[3], matrix[4], matrix[5]}, - {matrix[6], matrix[7], matrix[8]}}; - - for(i = 0; i < 3; i++) { - determinant = determinant + (mat[0][i] * (mat[1][(i+1)%3] * mat[2][(i+2)%3] - mat[1][(i+2)%3] * mat[2][(i+1)%3])); - } -// cout<<"\n\ndeterminant: "<