eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Final Version of the ACS Controller #367

Merged
muellerr merged 78 commits from eggert/acs into develop 2023-02-08 13:50:11 +01:00
Conversation 0 Commits 78 Files Changed 27 +577 -575
12 changed files with 577 additions and 575 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit b49d37e15a - Show all commits

16
mission/controller/AcsController.cpp
View File

@ -250,7 +250,8 @@ void AcsController::performPointingCtrl() {
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
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}};
@ -278,15 +280,17 @@ void AcsController::performPointingCtrl() {
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),
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),

72
mission/controller/acs/AcsParameters.h
View File

@ -28,17 +28,17 @@ class AcsParameters /*: public HasParametersIF*/ {
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
{-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
{-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
{-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
{-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}};
@ -787,8 +787,8 @@ class AcsParameters /*: public HasParametersIF*/ {
double rw3orientationMatrix[3][3];
double inertiaWheel = 0.000028198;
double maxTrq = 0.0032; // 3.2 [mNm]
double stictionSpeed = 80; //RPM
double stictionReleaseSpeed = 120; //RPM
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,8 +818,10 @@ 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 k_rate_no_mekf = 0.00059437;
;
double k_align_no_mekf = 0.000056875;
;
double sunMagAngleMin; // ???
double sunTargetDir[3] = {0, 0, 1};
@ -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;
@ -883,8 +876,9 @@ 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 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.;
@ -899,7 +893,7 @@ 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 sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk
double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
} kalmanFilterParameters;

22
mission/controller/acs/ActuatorCmd.h
View File

@ -6,30 +6,30 @@
#include "SensorProcessing.h"
#include "SensorValues.h"
class ActuatorCmd{
public:
ActuatorCmd(AcsParameters *acsParameters_); //Input mode ?
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
* @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
* @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
* 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);
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

106
mission/controller/acs/Guidance.cpp
View File

@ -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<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame
@ -174,9 +174,8 @@ 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
//-------------------------------------------------------------------------------------
@ -190,9 +189,9 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
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<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
@ -218,7 +217,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// Transform velocity into inertial frame
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<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
@ -238,20 +238,24 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::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]}};
// 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);
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));
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<double>::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1/timeElapsed, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
@ -269,8 +273,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
omegaRefSavedNadir[0] = omegaRefNew[0];
omegaRefSavedNadir[1] = omegaRefNew[1];
omegaRefSavedNadir[2] = omegaRefNew[2];
}
else {
} else {
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
@ -291,8 +294,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
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<double>::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<double>::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<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::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<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::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<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
@ -468,20 +476,18 @@ 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<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
@ -507,7 +513,8 @@ void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *
// 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 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<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
@ -519,20 +526,24 @@ void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::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]}};
// 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);
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));
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<double>::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1/timeElapsed, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
@ -550,8 +561,7 @@ void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *
omegaRefSavedNadir[0] = omegaRefNew[0];
omegaRefSavedNadir[1] = omegaRefNew[1];
omegaRefSavedNadir[2] = omegaRefNew[2];
}
else {
} else {
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;

26
mission/controller/acs/Guidance.h
View File

@ -21,26 +21,30 @@ 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
// 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]);
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,
// 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,
// 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
// 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

22
mission/controller/acs/Igrf13Model.h
View File

@ -115,20 +115,14 @@ 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}}; // [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];

8
mission/controller/acs/SensorProcessing.cpp
View File

@ -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<double>::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude, posSatE);
if (validSavedPosSatE && (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
MathOperations<double>::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude,
posSatE);
if (validSavedPosSatE &&
(gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1/timeDiffGpsMeas, gpsVelocityE, 3);
VectorOperations<double>::mulScalar(deltaDistance, 1 / timeDiffGpsMeas, gpsVelocityE, 3);
}
}
{

11
mission/controller/acs/control/Detumble.h
View File

@ -37,13 +37,12 @@ class Detumble {
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_*/

36
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -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
//------------------------------------------------------------------------------------------------
@ -86,28 +85,26 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt
for (int i = 0; i < 3; i++) {
if (abs(pError[i]) > 1) {
pErrorSign[i] = sign(pError[i]);
}
else {
} else {
pErrorSign[i] = pError[i];
}
}
// Torque for quaternion error
// Torque for quaternion error
double torqueQuat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
// Torque for rate error
// Torque for rate error
double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
// Final commanded Torque for every reaction wheel
// Final commanded Torque for every reaction wheel
double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
}
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
@ -162,29 +159,26 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
}
void PtgCtrl::rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw,
double* torqueCommand) {
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) {
if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) {
torqueMemory[i] = 0;
} else {
torqueCommand[i] = torqueMemory[i]
* rwHandlingParameters->stictionTorque;
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
}
} else {
if ((omegaRw[i] < rwHandlingParameters->stictionSpeed)
&& (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
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;
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
}
}
} else {
@ -192,4 +186,4 @@ void PtgCtrl::rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw,
}
omegaMemory[i] = omegaRw[i];
}
}
}

3
mission/controller/acs/control/PtgCtrl.h
View File

@ -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;

172
mission/controller/acs/util/MathOperations.h
View File

@ -64,11 +64,12 @@ class MathOperations {
}
}
static void convertDateToJD2000(const T1 time, T2 julianDate){
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;
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) {
@ -93,7 +94,8 @@ 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){
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]
@ -105,15 +107,14 @@ class MathOperations {
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);
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
}
static void dcmEJ(timeval time, T1 * outputDcmEJ, T1 * outputDotDcmEJ){
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]
@ -124,10 +125,9 @@ class MathOperations {
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) {
if ((JD2000 - JD2000Floor) < 0.5) {
JD2000Floor -= 0.5;
}
else {
} else {
JD2000Floor += 0.5;
}
@ -158,12 +158,11 @@ class MathOperations {
{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}};
double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
MatrixOperations<double>::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
@ -172,30 +171,30 @@ class MathOperations {
* @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)
*
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)
double JD2000UTC1 = convertUnixToJD2000(unixTime);
// Julian Date / century from TT
// 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;
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth rotation Theta
//-------------------------------------------------------------------------------------
double theta[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
// Earth Rotation angle
//-------------------------------------------------------------------------------------
// 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);
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;
@ -210,89 +209,92 @@ class MathOperations {
theta[2][1] = 0;
theta[2][2] = 1;
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth Precession P
//-------------------------------------------------------------------------------------
double precession[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth Precession P
//-------------------------------------------------------------------------------------
double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
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);
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);
zeta *= arcsecFactor;
theta2 *= arcsecFactor;
ze *= arcsecFactor;
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);
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);
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth Nutation N
//-------------------------------------------------------------------------------------
double nutation[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
// lunar asc node
//-------------------------------------------------------------------------------------
// 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);
7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3);
Om *= arcsecFactor;
// delta psi approx
double dp = -17.2 * arcsecFactor *sin(Om);
// delta psi approx
double dp = -17.2 * arcsecFactor * sin(Om);
// delta eps approx
double de = 9.203 * arcsecFactor *cos(Om);
// delta eps approx
double de = 9.203 * arcsecFactor * cos(Om);
// % true obliquity of the ecliptic eps p.71 (simplified)
// % true obliquity of the ecliptic eps p.71 (simplified)
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
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);
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 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}};
//-------------------------------------------------------------------------------------
// 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<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);
//-------------------------------------------------------------------------------------
// Calculation of transformation matrix and Derivative of transformation matrix
//-------------------------------------------------------------------------------------
double nutationPrecession[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
//-------------------------------------------------------------------------------------
// Calculation of transformation matrix and Derivative of transformation matrix
//-------------------------------------------------------------------------------------
double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
}
static void inverseMatrixDimThree(const T1 *matrix, T1 * output){
int i,j;
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]},
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]));
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: "<<determinant;
// cout<<"\n\nInverse of matrix is: \n";
for(i = 0; i < 3; i++){
for(j = 0; j < 3; j++){
output[i*3+j] = ((mat[(j+1)%3][(i+1)%3] * mat[(j+2)%3][(i+2)%3]) - (mat[(j+1)%3][(i+2)%3] * mat[(j+2)%3][(i+1)%3]))/ determinant;
// cout<<"\n\ndeterminant: "<<determinant;
// cout<<"\n\nInverse of matrix is: \n";
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
(mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
determinant;
}
}
}