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

26
mission/controller/AcsController.cpp
View File

@ -51,7 +51,7 @@ void AcsController::performControlOperation() {
case SUBMODE_DETUMBLE: case SUBMODE_DETUMBLE:
performDetumble(); performDetumble();
break; break;
case SUBMODE_PTG_SUN: case SUBMODE_PTG_SUN:
case SUBMODE_PTG_TARGET: case SUBMODE_PTG_TARGET:
case SUBMODE_PTG_NADIR: case SUBMODE_PTG_NADIR:
case SUBMODE_PTG_INERTIAL: case SUBMODE_PTG_INERTIAL:
@ -248,9 +248,10 @@ void AcsController::performPointingCtrl() {
&mekfData, &validMekf); &mekfData, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
switch (submode) { switch (submode) {
case SUBMODE_PTG_TARGET: case SUBMODE_PTG_TARGET:
guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate); guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat,
refSatRate);
break; break;
case SUBMODE_PTG_SUN: case SUBMODE_PTG_SUN:
guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
@ -261,7 +262,8 @@ void AcsController::performPointingCtrl() {
case SUBMODE_PTG_INERTIAL: case SUBMODE_PTG_INERTIAL:
guidance.inertialQuatPtg(targetQuat, refSatRate); guidance.inertialQuatPtg(targetQuat, refSatRate);
break; 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 deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? 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.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); (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 double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws( actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),

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

@ -26,22 +26,22 @@ class AcsParameters /*: public HasParametersIF*/ {
} onBoardParams; } onBoardParams;
struct InertiaEIVE { struct InertiaEIVE {
double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
{-0.0001821456, 0.1701302, 0.0004748963}, {-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 // Possible inertia matrices
double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
{-0.0001821456, 0.1701302, 0.0004748963}, {-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}, double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008},
{-0.0001798426, 0.162240, 0.000475596}, {-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}, double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207},
{-0.0001836122, 0.16619787, 0.0083537}, {-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}, double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767},
{-0.000178376, 0.166172, -0.007403}, {-0.000178376, 0.166172, -0.007403},
{-0.005009767, -0.007403, 0.07195314}}; {-0.005009767, -0.007403, 0.07195314}};
} inertiaEIVE; } inertiaEIVE;
struct MgmHandlingParameters { struct MgmHandlingParameters {
@ -786,9 +786,9 @@ class AcsParameters /*: public HasParametersIF*/ {
double rw2orientationMatrix[3][3]; double rw2orientationMatrix[3][3];
double rw3orientationMatrix[3][3]; double rw3orientationMatrix[3][3];
double inertiaWheel = 0.000028198; double inertiaWheel = 0.000028198;
double maxTrq = 0.0032; // 3.2 [mNm] double maxTrq = 0.0032; // 3.2 [mNm]
double stictionSpeed = 80; //RPM double stictionSpeed = 80; // RPM
double stictionReleaseSpeed = 120; //RPM double stictionReleaseSpeed = 120; // RPM
double stictionTorque = 0.0006; double stictionTorque = 0.0006;
} rwHandlingParameters; } rwHandlingParameters;
@ -796,31 +796,21 @@ class AcsParameters /*: public HasParametersIF*/ {
double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000}, double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000},
{0.0000, -0.9205, 0.0000, 0.9205}, {0.0000, -0.9205, 0.0000, 0.9205},
{0.3907, 0.3907, 0.3907, 0.3907}}; {0.3907, 0.3907, 0.3907, 0.3907}};
// double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, // double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597},
// {0.2136, -0.3317, 1.0123}, // {0.2136, -0.3317, 1.0123},
// {-0.8672, -0.1406, 0.1778}, // {-0.8672, -0.1406, 0.1778},
// {0.6426, 0.4794, 1.3603}}; // {0.6426, 0.4794, 1.3603}};
// where does the first pseudo inverse come frome - matlab gives result below // where does the first pseudo inverse come frome - matlab gives result below
double pseudoInverse[4][3] = {{0.5432, 0, 0.6398}, double pseudoInverse[4][3] = {
{0, -0.5432, 0.6398}, {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
{-0.5432, 0, 0.6398}, double without0[4][3] = {
{0, 0.5432, 0.6398}}; {0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
double without0[4][3] = {{0, 0, 0}, double without1[4][3] = {
{0.5432, -0.5432, 1.2797}, {0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
{-1.0864, 0, 0}, double without2[4][3] = {
{0.5432, 0.5432, 1.2797}}; {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
double without1[4][3] = {{0.5432, -0.5432, 1.2797}, double without3[4][3] = {
{0, 0, 0}, {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-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}; double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
} rwMatrices; } rwMatrices;
@ -828,9 +818,11 @@ class AcsParameters /*: public HasParametersIF*/ {
double k_rate_mekf = 0.00059437; double k_rate_mekf = 0.00059437;
double k_align_mekf = 0.000056875; double k_align_mekf = 0.000056875;
double k_rate_no_mekf = 0.00059437;; double k_rate_no_mekf = 0.00059437;
double k_align_no_mekf = 0.000056875;; ;
double sunMagAngleMin; // ??? double k_align_no_mekf = 0.000056875;
;
double sunMagAngleMin; // ???
double sunTargetDir[3] = {0, 0, 1}; double sunTargetDir[3] = {0, 0, 1};
double satRateRef[3] = {0, 0, 0}; double satRateRef[3] = {0, 0, 0};
@ -839,7 +831,7 @@ class AcsParameters /*: public HasParametersIF*/ {
// ToDo: mutiple structs for different pointing mode controllers? // ToDo: mutiple structs for different pointing mode controllers?
struct PointingModeControllerParameters { struct PointingModeControllerParameters {
double refDirection[3] = {-1, 0, 0}; //Antenna double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0}; double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1}; double quatRef[4] = {0, 0, 0, 1};
bool avoidBlindStr = true; bool avoidBlindStr = true;
@ -860,11 +852,12 @@ class AcsParameters /*: public HasParametersIF*/ {
double omegaEarth = 0.000072921158553; 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 tgtQuatInertial[4] = {0, 0, 0, 1};
double tgtRotRateInertial[3] = {0, 0, 0}; double tgtRotRateInertial[3] = {0, 0, 0};
int8_t nadirTimeElapsedMax = 10; int8_t nadirTimeElapsedMax = 10;
} pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; } pointingModeControllerParameters, inertialModeControllerParameters,
nadirModeControllerParameters, targetModeControllerParameters;
struct StrParameters { struct StrParameters {
double exclusionAngle = 20 * M_PI / 180; double exclusionAngle = 20 * M_PI / 180;
@ -872,7 +865,7 @@ class AcsParameters /*: public HasParametersIF*/ {
} strParameters; } strParameters;
struct GpsParameters { struct GpsParameters {
double timeDiffVelocityMax = 30; //[s] double timeDiffVelocityMax = 30; //[s]
} gpsParameters; } gpsParameters;
struct GroundStationParameters { struct GroundStationParameters {
@ -883,10 +876,11 @@ class AcsParameters /*: public HasParametersIF*/ {
struct SunModelParameters { struct SunModelParameters {
float domega = 36000.771; float domega = 36000.771;
float omega_0 = 280.46 * M_PI / 180.; //282.94 * M_PI / 180.; // RAAN plus argument of perigee float omega_0 = 280.46 * M_PI / 180.; // 282.94 * M_PI / 180.; // RAAN plus argument of
float m_0 = 357.5277; //357.5256; // coefficients for mean anomaly // perigee
float dm = 35999.049; // coefficients for mean anomaly float m_0 = 357.5277; // 357.5256; // coefficients for mean anomaly
float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis 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 e1 = 0.74508 * M_PI / 180.;
float p1 = 6892. / 3600. * M_PI / 180.; // some parameter float p1 = 6892. / 3600. * M_PI / 180.; // some parameter
@ -899,8 +893,8 @@ class AcsParameters /*: public HasParametersIF*/ {
double sensorNoiseMAG = 4 * M_PI / 180; double sensorNoiseMAG = 4 * M_PI / 180;
double sensorNoiseGYR = 0.1 * 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 double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
} kalmanFilterParameters; } kalmanFilterParameters;
struct MagnetorquesParameter { struct MagnetorquesParameter {
@ -914,7 +908,7 @@ class AcsParameters /*: public HasParametersIF*/ {
} magnetorquesParameter; } magnetorquesParameter;
struct DetumbleParameter { struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s uint8_t detumblecounter = 75; // 30 s
double omegaDetumbleStart = 2 * M_PI / 180; double omegaDetumbleStart = 2 * M_PI / 180;
double omegaDetumbleEnd = 0.4 * M_PI / 180; double omegaDetumbleEnd = 0.4 * M_PI / 180;
double gainD = pow(10.0, -3.3); double gainD = pow(10.0, -3.3);

4
mission/controller/acs/ActuatorCmd.cpp
View File

@ -33,8 +33,8 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
} }
if (maxValue > maxTrq) { if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue; double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
} }
} }

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

@ -6,38 +6,38 @@
#include "SensorProcessing.h" #include "SensorProcessing.h"
#include "SensorValues.h" #include "SensorValues.h"
class ActuatorCmd{ class ActuatorCmd {
public: public:
ActuatorCmd(AcsParameters *acsParameters_); //Input mode ? ActuatorCmd(AcsParameters *acsParameters_); // Input mode ?
virtual ~ActuatorCmd(); virtual ~ActuatorCmd();
/* /*
* @brief: scalingTorqueRws() scales the torque via maximum part in case this part is higher * @brief: scalingTorqueRws() scales the torque via maximum part in case this part is
* then the maximum torque * higher then the maximum torque
* @param: rwTrq given torque for reaction wheels * @param: rwTrq given torque for reaction wheels
* rwTrqScaled possible scaled torque * rwTrqScaled possible scaled torque
*/ */
void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled);
/* /*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction wheels, also * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction
* will calculate the needed revolutions per minute for the RWs, which will be given * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
* as Input to the RWs * as Input to the RWs
* @param: rwTrqIn given torque from pointing controller * @param: rwTrqIn given torque from pointing controller
* rwTrqNS Nullspace torque * 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, void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
double *rwCmdSpeed); const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
* *
* @param: dipolMoment given dipol moment in spacecraft frame * @param: dipolMoment given dipol moment in spacecraft frame
* dipolMomentUnits resulting dipol moment for every unit * dipolMomentUnits resulting dipol moment for every unit
*/ */
void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits); void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits);
protected: protected:
private: private:

424
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 // Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE); sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame // 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, void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
double targetQuat[4], double refSatRate[3]) { timeval now, double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
//-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame)
double groundStationCart[3] = {0, 0, 0};
//------------------------------------------------------------------------------------- MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
// Calculation of target quaternion for target pointing acsParameters.groundStationParameters.longitudeGs,
//------------------------------------------------------------------------------------- acsParameters.groundStationParameters.altitudeGs,
// Transform longitude, latitude and altitude to cartesian coordiantes (earth groundStationCart);
// fixed/centered frame) // Position of the satellite in the earth/fixed frame via GPS
double groundStationCart[3] = {0, 0, 0}; 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,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, // Transformation between ECEF and IJK frame
acsParameters.groundStationParameters.longitudeGs, double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
acsParameters.groundStationParameters.altitudeGs, double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
groundStationCart); double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// Position of the satellite in the earth/fixed frame via GPS MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
double posSatE[3] = {0, 0, 0}; MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
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);
// Transformation between ECEF and IJK frame double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
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<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // Target Direction and position vector in the inertial frame
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
// Target Direction and position vector in the inertial frame // negative x-Axis aligned with target (Camera/E-band transmitter position)
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; double xAxis[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// negative x-Axis aligned with target (Camera/E-band transmitter position) // Transform velocity into inertial frame
double xAxis[3] = {0, 0, 0}; double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1],
VectorOperations<double>::normalize(targetDirJ, xAxis, 3); outputValues->gpsVelocity[2]};
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3); 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);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
// Transform velocity into inertial frame // orbital normal vector
double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]}; double orbitalNormalJ[3] = {0, 0, 0};
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ);
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
// orbital normal vector // y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution
double orbitalNormalJ[3] = {0, 0, 0}; double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ); VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis);
VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3); VectorOperations<double>::normalize(yAxis, yAxis, 3);
// y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution // z-Axis completes RHS
double yAxis[3] = {0, 0, 0}; double zAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis); VectorOperations<double>::cross(xAxis, yAxis, zAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// z-Axis completes RHS // Complete transformation matrix
double zAxis[3] = {0, 0, 0}; double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
VectorOperations<double>::cross(xAxis, yAxis, zAxis); {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]}}; // Calculation of reference rotation rate
double quatInertialTarget[4] = {0, 0, 0, 0}; //-------------------------------------------------------------------------------------
QuaternionOperations::fromDcm(dcmTgt,quatInertialTarget); 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);
//------------------------------------------------------------------------------------- double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
// Calculation of reference rotation rate 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};
double timeElapsed = now.tv_sec + now.tv_usec * pow(10,-6) - (timeSavedQuaternionNadir.tv_sec + VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6)); VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
double qDiff[4] = {0, 0, 0, 0}; VectorOperations<double>::add(sum1, sum2, sum, 3);
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); VectorOperations<double>::subtract(sum, sum3, sum, 3);
VectorOperations<double>::mulScalar(qDiff, 1/timeElapsed, qDiff, 4); double omegaRefNew[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3);
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; omegaRefSavedNadir[0] = omegaRefNew[0];
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1); omegaRefSavedNadir[1] = omegaRefNew[1];
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); omegaRefSavedNadir[2] = omegaRefNew[2];
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); } else {
VectorOperations<double>::add(sum1, sum2, sum, 3); refSatRate[0] = 0;
VectorOperations<double>::subtract(sum, sum3, sum, 3); refSatRate[1] = 0;
double omegaRefNew[3] = {0, 0, 0}; refSatRate[2] = 0;
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3); }
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3); timeSavedQuaternionNadir = now;
VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); savedQuaternionNadir[0] = quatInertialTarget[0];
omegaRefSavedNadir[0] = omegaRefNew[0]; savedQuaternionNadir[1] = quatInertialTarget[1];
omegaRefSavedNadir[1] = omegaRefNew[1]; savedQuaternionNadir[2] = quatInertialTarget[2];
omegaRefSavedNadir[2] = omegaRefNew[2]; savedQuaternionNadir[3] = quatInertialTarget[3];
}
else {
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
timeSavedQuaternionNadir = now; // Transform in system relative to satellite frame
savedQuaternionNadir[0] = quatInertialTarget[0]; double quatBJ[4] = {0, 0, 0, 0};
savedQuaternionNadir[1] = quatInertialTarget[1]; quatBJ[0] = outputValues->quatMekfBJ[0];
savedQuaternionNadir[2] = quatInertialTarget[2]; quatBJ[1] = outputValues->quatMekfBJ[1];
savedQuaternionNadir[3] = quatInertialTarget[3]; quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3];
// Transform in system relative to satellite frame QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
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, void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
double targetQuat[4], double refSatRate[3]) { timeval now, double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun // Calculation of target quaternion to sun
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -354,17 +357,18 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// positive z-Axis of EIVE in direction of sun // 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); VectorOperations<double>::normalize(sunDirB, zAxis, 3);
// Position of the satellite in the earth/fixed frame via GPS and body // Position of the satellite in the earth/fixed frame via GPS and body
// velocity // velocity
double posSatE[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE); 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}; 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(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 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 // Transformation matrix to Sun, no further transforamtions, reference is already
// the EIVE body frame // 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}; double quatSun[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt,quatSun); QuaternionOperations::fromDcm(dcmTgt, quatSun);
targetQuat[0] = quatSun[0]; targetQuat[0] = quatSun[0];
targetQuat[1] = quatSun[1]; targetQuat[1] = quatSun[1];
@ -404,16 +410,18 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou
refSatRate[2] = 0; refSatRate[2] = 0;
} }
void Guidance::quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues,
double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing ACS::OutputValues *outputValues, timeval now,
double targetQuat[4],
double refSatRate[3]) { // old version of Nadir Pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing // Calculation of target quaternion for Nadir pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Position of the satellite in the earth/fixed frame via GPS // Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE); sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3); VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
@ -468,116 +476,118 @@ void Guidance::quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::Outp
refSatRate[0] = 0; refSatRate[0] = 0;
refSatRate[1] = 0; refSatRate[1] = 0;
refSatRate[2] = 0; refSatRate[2] = 0;
} }
void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
double targetQuat[4], double refSatRate[3]) { 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,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
//------------------------------------------------------------------------------------- // Transformation between ECEF and IJK frame
// Calculation of target quaternion for Nadir pointing 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}};
// Position of the satellite in the earth/fixed frame via GPS double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double posSatE[3] = {0, 0, 0}; MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
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);
// Transformation between ECEF and IJK frame double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
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<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // Target Direction in the body frame
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); double targetDirJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
// Target Direction in the body frame // negative x-Axis aligned with target (Camera position)
double targetDirJ[3] = {0, 0, 0}; double xAxis[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// negative x-Axis aligned with target (Camera position) // z-Axis parallel to long side of picture resolution
double xAxis[3] = {0, 0, 0}; double zAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3); double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1],
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3); 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);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// z-Axis parallel to long side of picture resolution // y-Axis completes RHS
double zAxis[3] = {0, 0, 0}; double yAxis[3] = {0, 0, 0};
double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]}; VectorOperations<double>::cross(zAxis, xAxis, yAxis);
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);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// y-Axis completes RHS // Complete transformation matrix
double yAxis[3] = {0, 0, 0}; double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
VectorOperations<double>::cross(zAxis, xAxis, yAxis); {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]}}; // Calculation of reference rotation rate
double quatInertialTarget[4] = {0, 0, 0, 0}; //-------------------------------------------------------------------------------------
QuaternionOperations::fromDcm(dcmTgt,quatInertialTarget); 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);
//------------------------------------------------------------------------------------- double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
// Calculation of reference rotation rate 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};
double timeElapsed = now.tv_sec + now.tv_usec * pow(10,-6) - (timeSavedQuaternionNadir.tv_sec + VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6)); VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
double qDiff[4] = {0, 0, 0, 0}; VectorOperations<double>::add(sum1, sum2, sum, 3);
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); VectorOperations<double>::subtract(sum, sum3, sum, 3);
VectorOperations<double>::mulScalar(qDiff, 1/timeElapsed, qDiff, 4); double omegaRefNew[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3);
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; omegaRefSavedNadir[0] = omegaRefNew[0];
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1); omegaRefSavedNadir[1] = omegaRefNew[1];
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); omegaRefSavedNadir[2] = omegaRefNew[2];
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); } else {
VectorOperations<double>::add(sum1, sum2, sum, 3); refSatRate[0] = 0;
VectorOperations<double>::subtract(sum, sum3, sum, 3); refSatRate[1] = 0;
double omegaRefNew[3] = {0, 0, 0}; refSatRate[2] = 0;
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3); }
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3); timeSavedQuaternionNadir = now;
VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); savedQuaternionNadir[0] = quatInertialTarget[0];
omegaRefSavedNadir[0] = omegaRefNew[0]; savedQuaternionNadir[1] = quatInertialTarget[1];
omegaRefSavedNadir[1] = omegaRefNew[1]; savedQuaternionNadir[2] = quatInertialTarget[2];
omegaRefSavedNadir[2] = omegaRefNew[2]; savedQuaternionNadir[3] = quatInertialTarget[3];
}
else {
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
timeSavedQuaternionNadir = now; // Transform in system relative to satellite frame
savedQuaternionNadir[0] = quatInertialTarget[0]; double quatBJ[4] = {0, 0, 0, 0};
savedQuaternionNadir[1] = quatInertialTarget[1]; quatBJ[0] = outputValues->quatMekfBJ[0];
savedQuaternionNadir[2] = quatInertialTarget[2]; quatBJ[1] = outputValues->quatMekfBJ[1];
savedQuaternionNadir[3] = quatInertialTarget[3]; quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3];
// Transform in system relative to satellite frame QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
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]) { void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
for (int i = 0; i < 4; i++) { 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++) { for (int i = 0; i < 3; i++) {
refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i]; refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i];
} }
} }

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

@ -21,27 +21,31 @@ class Guidance {
void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); 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
void targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, // of the ground station
acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], void targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
double refSatRate[3]); acsctrl::SusDataProcessed *susDataProcessed, timeval now,
void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, double targetQuat[4], double refSatRate[3]);
void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4],
double refSatRate[3]); double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate for sun pointing after ground station // Function to get the target quaternion and refence rotation rate for sun pointing after ground
void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, // station
double targetQuat[4], double refSatRate[3]); 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 // Function to get the target quaternion and refence rotation rate from gps position for Nadir
void quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, // pointing
double targetQuat[4], double refSatRate[3]); 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, void quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]); 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
void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); // pointing
void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and // @note: compares target Quaternion and reference quaternion, also actual satellite rate and
// desired // desired

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},
{0.0, 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 {0.0, 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}, 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}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 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]; double updatedG[14][13];

14
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 // Calculation of the satellite velocity in earth fixed frame
double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0};
// RADIANS OR DEGREE ? Function needs rad as input // RADIANS OR DEGREE ? Function needs rad as input
MathOperations<double>::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude, posSatE); MathOperations<double>::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude,
if (validSavedPosSatE && (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { posSatE);
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3); if (validSavedPosSatE &&
double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE; (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
VectorOperations<double>::mulScalar(deltaDistance, 1/timeDiffGpsMeas, gpsVelocityE, 3); VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1 / timeDiffGpsMeas, gpsVelocityE, 3);
} }
} }
{ {
@ -600,7 +602,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
validSavedPosSatE = true; validSavedPosSatE = true;
} }
else { else {
validGcLatitude = false; validGcLatitude = false;
} }
} }

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

@ -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 bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField,
const double *magField, const bool *magFieldValid, const bool *magFieldValid, double *magMom);
double *magMom);
private: private:
AcsParameters::DetumbleParameter* detumbleParameter; AcsParameters::DetumbleParameter *detumbleParameter;
AcsParameters::MagnetorquesParameter* magnetorquesParameter; AcsParameters::MagnetorquesParameter *magnetorquesParameter;
}; };
#endif /*ACS_CONTROL_DETUMBLE_H_*/ #endif /*ACS_CONTROL_DETUMBLE_H_*/

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

@ -16,9 +16,7 @@
#include "../util/MathOperations.h" #include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
loadAcsParameters(acsParameters_);
}
PtgCtrl::~PtgCtrl() {} PtgCtrl::~PtgCtrl() {}
@ -29,7 +27,8 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
rwMatrices = &(acsParameters_->rwMatrices); 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 // 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}; double pErrorSign[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (abs(pError[i]) > 1) { if (abs(pError[i]) > 1) {
pErrorSign[i] = sign(pError[i]); pErrorSign[i] = sign(pError[i]);
} } else {
else { pErrorSign[i] = pError[i];
pErrorSign[i] = pError[i]; }
} }
} // Torque for quaternion error
// Torque for quaternion error double torqueQuat[3] = {0, 0, 0};
double torqueQuat[3] = {0, 0, 0}; MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
// Torque for rate error // Torque for rate error
double torqueRate[3] = {0, 0, 0}; double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3); VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3); VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
// 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);
// 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, 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<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4); VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
} }
void PtgCtrl::rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, void PtgCtrl::rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw,
double* torqueCommand) { double *torqueCommand) {
for (uint8_t i = 0; i < 4; i++) { for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) { if (rwAvailable[i]) {
if (torqueMemory[i] != 0) { if (torqueMemory[i] != 0) {
if ((omegaRw[i] * torqueMemory[i]) if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) {
> rwHandlingParameters->stictionReleaseSpeed) { torqueMemory[i] = 0;
torqueMemory[i] = 0; } else {
} else { torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
torqueCommand[i] = torqueMemory[i] }
* rwHandlingParameters->stictionTorque; } else {
} if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) &&
} else { (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) if (omegaRw[i] < omegaMemory[i]) {
&& (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { torqueMemory[i] = -1;
if (omegaRw[i] < omegaMemory[i]) { } else {
torqueMemory[i] = -1; torqueMemory[i] = 1;
} else { }
torqueMemory[i] = 1;
}
torqueCommand[i] = torqueMemory[i] torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
* rwHandlingParameters->stictionTorque; }
} }
} } else {
} else { torqueMemory[i] = 0;
torqueMemory[i] = 0; }
} omegaMemory[i] = omegaRw[i];
omegaMemory[i] = omegaRw[i]; }
} }
}

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

@ -42,7 +42,7 @@ class PtgCtrl {
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/ */
void ptgLaw(const double mode, const double *qError, const double *deltaRate, 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, void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, 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 * omegaRw current wheel speed of reaction wheels
* torqueCommand modified torque after antistiction * torqueCommand modified torque after antistiction
*/ */
void rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
double* torqueCommand);
private: private:
AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;

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

@ -64,12 +64,13 @@ 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 = { Y, M, D, h, m,s}
// time in sec and microsec -> The Epoch (unixtime) // time in sec and microsec -> The Epoch (unixtime)
julianDate = 1721013.5 + 367*time[0]- floor(7/4*(time[0]+(time[1]+9)/12)) 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; floor(275 * time[1] / 9) + time[2] +
} (60 * time[3] + time[4] + (time(5) / 60)) / 1440;
}
static T1 convertUnixToJD2000(timeval time) { static T1 convertUnixToJD2000(timeval time) {
// time = {{s},{us}} // 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); 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,
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude, T2 *cartesianOutput) {
* longitude and altitude /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
* @param: lat geodetic latitude [rad] * longitude and altitude
* longi longitude [rad] * @param: lat geodetic latitude [rad]
* alt altitude [m] * longi longitude [rad]
* cartesianOutput Cartesian Coordinates in ECEF (3x1) * alt altitude [m]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff * cartesianOutput Cartesian Coordinates in ECEF (3x1)
* Landis Markley and John L. Crassidis*/ * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
double radiusPolar = 6356752.314; * Landis Markley and John L. Crassidis*/
double radiusEqua = 6378137; double radiusPolar = 6356752.314;
double radiusEqua = 6378137;
double eccentricity = sqrt(1 - pow(radiusPolar,2) / pow(radiusEqua,2)); double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity,2) * pow(sin(lat),2)); double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi); cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(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
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame * @param: time Current time
* @param: time Current time * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] * outputDotDcmEJ Derivative of transformation matrix [3][3]
* outputDotDcmEJ Derivative of transformation matrix [3][3] * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff * Landis Markley and John L. Crassidis*/
* Landis Markley and John L. Crassidis*/ double JD2000Floor = 0;
double JD2000Floor = 0; double JD2000 = convertUnixToJD2000(time);
double JD2000 = convertUnixToJD2000(time); // Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0) JD2000Floor = floor(JD2000);
JD2000Floor = floor(JD2000); if ((JD2000 - JD2000Floor) < 0.5) {
if ( ( JD2000 - JD2000Floor) < 0.5) { JD2000Floor -= 0.5;
JD2000Floor -= 0.5; } else {
} JD2000Floor += 0.5;
else { }
JD2000Floor += 0.5;
}
double JC2000 = JD2000Floor / 36525; double JC2000 = JD2000Floor / 36525;
double sec = (JD2000 - JD2000Floor) * 86400; double sec = (JD2000 - JD2000Floor) * 86400;
@ -152,150 +152,152 @@ class MathOperations {
outputDcmEJ[7] = 0; outputDcmEJ[7] = 0;
outputDcmEJ[8] = 1; outputDcmEJ[8] = 1;
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]}, double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
{outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]}, {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
{outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}}; {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = 0.000072921158553; 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>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 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
* 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 double JD2000UTC1 = convertUnixToJD2000(unixTime);
* 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) // Julian Date / century from TT
// International Atomic Time (TAI) 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 theta[0][0] = cos(gmst2000);
timeval terestrialTime = unixTime; theta[0][1] = sin(gmst2000);
terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37; theta[0][2] = 0;
double JD2000TT = convertUnixToJD2000(terestrialTime); theta[1][0] = -sin(gmst2000);
double JC2000TT = JD2000TT / 36525; 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 // Calculation of Transformation from earth Precession P
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double theta[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; double precession[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;
theta[0][0] = cos(gmst2000); double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3);
theta[0][1] = sin(gmst2000); double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3);
theta[0][2] = 0; double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3);
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;
//------------------------------------------------------------------------------------- zeta *= arcsecFactor;
// Calculation of Transformation from earth Precession P theta2 *= arcsecFactor;
//------------------------------------------------------------------------------------- ze *= arcsecFactor;
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); precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta);
double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT,2) - 0.041833 * pow(JC2000TT,3); precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta);
double ze = zeta + 0.79280 * pow(JC2000TT,2) + 0.000205 * pow(JC2000TT,3); 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; // Calculation of Transformation from earth Nutation N
ze *= arcsecFactor; //-------------------------------------------------------------------------------------
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); // delta eps approx
precession[1][0]=cos(ze)*sin(zeta)+sin(ze)*cos(theta2)*cos(zeta); double de = 9.203 * arcsecFactor * cos(Om);
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);
//------------------------------------------------------------------------------------- // % true obliquity of the ecliptic eps p.71 (simplified)
// Calculation of Transformation from earth Nutation N double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
//-------------------------------------------------------------------------------------
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);
// delta eps approx nutation[0][0] = cos(dp);
double de = 9.203 * arcsecFactor *cos(Om); 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<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);
nutation[0][0]=cos(dp); //-------------------------------------------------------------------------------------
nutation[1][0]=cos(e+de)*sin(dp); // Calculation of transformation matrix and Derivative of transformation matrix
nutation[2][0]=sin(e+de)*sin(dp); //-------------------------------------------------------------------------------------
nutation[0][1]=-cos(e)*sin(dp); double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
nutation[1][1]=cos(e)*cos(e+de)*cos(dp)+sin(e)*sin(e+de); MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
nutation[2][1]=cos(e)*sin(e+de)*cos(dp)-sin(e)*cos(e+de); MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
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);
//------------------------------------------------------------------------------------- MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
// Calculation of Derivative of rotation matrix from earth }
//------------------------------------------------------------------------------------- static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
double thetaDot[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; int i, j;
double dotMatrix[3][3] = {{0,1,0},{-1,0,0},{0,0,0}}; double determinant;
double omegaEarth = 0.000072921158553; double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); {matrix[3], matrix[4], matrix[5]},
MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); {matrix[6], matrix[7], matrix[8]}};
//------------------------------------------------------------------------------------- for (i = 0; i < 3; i++) {
// Calculation of transformation matrix and Derivative of transformation matrix 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]));
double nutationPrecession[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; }
MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3); // cout<<"\n\ndeterminant: "<<determinant;
MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3); // cout<<"\n\nInverse of matrix is: \n";
for (i = 0; i < 3; i++) {
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); 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])) /
static void inverseMatrixDimThree(const T1 *matrix, T1 * output){ determinant;
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: "<<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;
}
}
}
static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) { static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
float det = 0; float det = 0;