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