From 55dec574c5bd23825fbbd1f12dbb6d01c62c86c5 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Mon, 14 Nov 2022 17:16:47 +0100 Subject: [PATCH] added inertial pointing --- mission/controller/AcsController.cpp | 19 ++++++++++++------- mission/controller/AcsController.h | 3 ++- mission/controller/acs/AcsParameters.h | 10 ++++++---- mission/controller/acs/Guidance.cpp | 15 ++++++++++++--- mission/controller/acs/Guidance.h | 5 ++++- mission/controller/acs/control/PtgCtrl.cpp | 3 +-- mission/controller/acs/control/PtgCtrl.h | 2 +- 7 files changed, 38 insertions(+), 19 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d2bd0510..6e5e4473 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -43,7 +43,7 @@ void AcsController::performControlOperation() { performDetumble(); break; - case SUBMODE_PTG_GS: + case SUBMODE_PTG_TARGET: performPointingCtrl(); break; @@ -113,7 +113,8 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid); } - + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); } void AcsController::performDetumble() { @@ -154,7 +155,8 @@ void AcsController::performPointingCtrl() { ACS::SensorValues sensorValues; ACS::OutputValues outputValues; - timeval now; // Übergabe ? + timeval now; + Clock::getClock_timeval(&now); sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); ReturnValue_t validMekf; @@ -162,14 +164,17 @@ void AcsController::performPointingCtrl() { double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; switch (submode) { - case SUBMODE_PTG_GS: + case SUBMODE_PTG_TARGET: guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); break; case SUBMODE_PTG_SUN: guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); break; case SUBMODE_PTG_NADIR: - guidance.targetQuatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + break; + case SUBMODE_PTG_INERTIAL: + guidance.inertialQuatPtg(targetQuat, refSatRate); break; } double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; @@ -188,8 +193,8 @@ void AcsController::performPointingCtrl() { if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? - double rwSpeed[4] = {&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value)}; + int32_t rwSpeed[4] = {(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), + (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 21a0c86d..4e9d435a 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -26,9 +26,10 @@ class AcsController : public ExtendedControllerBase { static const Submode_t SUBMODE_SAFE = 2; static const Submode_t SUBMODE_DETUMBLE = 3; - static const Submode_t SUBMODE_PTG_GS = 4; + static const Submode_t SUBMODE_PTG_TARGET = 4; static const Submode_t SUBMODE_PTG_NADIR = 5; static const Submode_t SUBMODE_PTG_SUN = 6; + static const Submode_t SUBMODE_PTG_INERTIAL = 7; protected: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index ec535b75..953f50db 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -45,10 +45,10 @@ class AcsParameters /*: public HasParametersIF*/ { } inertiaEIVE; struct MgmHandlingParameters { - float mgm0orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; - float mgm1orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; - float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; - float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; + float mgm0orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; // Documentation not consistent + float mgm1orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; // Documentation not consistent + float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; // Documentation not consistent + float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; // Documentation not consistent float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; float mgm0hardIronOffset[3] = {19.89364, -29.94111, -31.07508}; @@ -850,6 +850,8 @@ class AcsParameters /*: public HasParametersIF*/ { double omegaEarth = 0.000072921158553; double nadirRefDirection[3] = {-1, 0, 0}; //Camera + double refQuatInertial[4] = {0, 0, 0, 1}; + double refRotRateInertial[3] = {0, 0, 0}; } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; struct StrParameters { diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index bd7ccf6c..b2f47953 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -32,9 +32,9 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- - // Calculation of target quaternion to groundstation + // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth + // Transform longitude, latitude and altitude to cartesian coordiantes (earth // fixed/centered frame) double groundStationCart[3] = {0, 0, 0}; @@ -234,7 +234,7 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou refSatRate[2] = 0; } -void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, +void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing @@ -301,6 +301,15 @@ void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputVa } +void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { + for (int i = 0; i < 4; i++) { + targetQuat[i] = acsParameters.inertialModeControllerParameters.refQuatInertial[i]; + } + for (int i = 0; i < 3; i++) { + refSatRate[i] = acsParameters.inertialModeControllerParameters.refRotRateInertial[i]; + } +} + void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3]) { double quatRef[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index eb58daf8..6b0759fb 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -31,9 +31,12 @@ public: double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing - void targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); + // Function to get the target quaternion and refence rotation rate from parameters for inertial pointing + void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); + // @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 337a8d8f..fd9dd9eb 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -165,7 +165,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } -void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw, +void PtgCtrl::rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, double* torqueCommand) { for (uint8_t i = 0; i < 4; i++) { if (rwAvailable[i]) { @@ -196,4 +196,3 @@ void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw, omegaMemory[i] = omegaRw[i]; } } -} diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 2a3bde5c..8cacd556 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -57,7 +57,7 @@ class PtgCtrl { * omegaRw current wheel speed of reaction wheels * torqueCommand modified torque after antistiction */ - void rwAntistiction(const bool* rwAvailable, const double* omegaRw, + void rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, double* torqueCommand); private: