From d82a6ece5a0c824476ffd8306f0dc9ed658acb53 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 15 Aug 2023 09:11:57 +0200 Subject: [PATCH 1/3] use safeCtrl even if SUS and MGM vectors are too close --- mission/controller/AcsController.cpp | 6 ++--- mission/controller/acs/control/SafeCtrl.cpp | 29 ++++++++++++++------- mission/controller/acs/control/SafeCtrl.h | 8 +++--- 3 files changed, 26 insertions(+), 17 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index febe385a..1350bcf0 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -204,8 +204,7 @@ void AcsController::performSafe() { acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), - fusedRotRateData.rotRateOrthogonal.isValid(), fusedRotRateData.rotRateTotal.isValid(), - acsParameters.safeModeControllerParameters.useMekf, + fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { @@ -223,7 +222,8 @@ void AcsController::performSafe() { safeCtrlFailureCounter = 0; break; case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): - safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value, + safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value, + fusedRotRateData.rotRateParallel.value, fusedRotRateData.rotRateOrthogonal.value, susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 8ad8b578..de0cd197 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,10 +9,12 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy( - const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, - const bool sunDirValid, const bool fusedRateSplitValid, const bool fusedRateTotalValid, - const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) { +acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const bool fusedRateTotalValid, + const uint8_t mekfEnabled, + const uint8_t gyrEnabled, + const uint8_t dampingEnabled) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { @@ -20,7 +22,7 @@ acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy( } else if (sunDirValid) { if (gyrEnabled and satRotRateValid) { return acs::SafeModeStrategy::SAFECTRL_GYR; - } else if (not gyrEnabled and fusedRateSplitValid) { + } else if (not gyrEnabled and fusedRateTotalValid) { return acs::SafeModeStrategy::SAFECTRL_SUSMGM; } else { return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; @@ -95,9 +97,10 @@ void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const calculateMagneticMoment(magMomB); } -void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallelB, - const double *rotRateOrthogonalB, const double *sunDirB, - const double *sunDirRefB, double *magMomB, double &errorAngle) { +void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateTotalB, + const double *rotRateParallelB, const double *rotRateOrthogonalB, + const double *sunDirB, const double *sunDirRefB, double *magMomB, + double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -105,8 +108,14 @@ void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallel double dotSun = VectorOperations::dot(sunDirRefB, sunDirB); errorAngle = acos(dotSun); - std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB)); - std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB)); + if (VectorOperations::norm(rotRateParallelB, 3) != 0 and + VectorOperations::norm(rotRateOrthogonalB, 3) != 0) { + std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB)); + std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB)); + } else { + splitRotationalRate(rotRateTotalB, sunDirB); + } + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm, acsParameters->safeModeControllerParameters.k_orthoSusMgm); calculateAngleErrorTorque(sunDirB, sunDirRefB, diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 55bcbf08..d35d5d04 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -14,7 +14,6 @@ class SafeCtrl { acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, - const bool fusedRateSplitValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled); @@ -25,9 +24,10 @@ class SafeCtrl { void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB, const double *sunDirRefB, double *magMomB, double &errorAngle); - void safeSusMgm(const double *magFieldB, const double *rotRateParallelB, - const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB, - double *magMomB, double &errorAngle); + void safeSusMgm(const double *magFieldB, const double *rotRateTotalB, + const double *rotRateParallelB, const double *rotRateOrthogonalB, + const double *sunDirB, const double *sunDirRefB, double *magMomB, + double &errorAngle); void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, double *magMomB, double &errorAngle); -- 2.43.0 From 0a42093de34de91550cec89a0c8f1abe4014f09a Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 15 Aug 2023 09:20:00 +0200 Subject: [PATCH 2/3] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 41d1b480..9d287dd5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,8 @@ will consitute of a breaking change warranting a new major release: return the actual GPS data will be ignored once SPG4 is running. However, by setting the according parameter, the ACS Controller can be directed to ignore the SGP4 solution. - Skyview dataset for more GPS TM has been added +- The MGM and SUS vectors being too close together does not prevent the usage of the safe + mode controller anymore. ## Fixed -- 2.43.0 From 181d3557417c9a483d4706f213be21df5bc780d5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Aug 2023 13:17:08 +0200 Subject: [PATCH 3/3] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 4b054b76..40365711 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 4b054b7628e43975e2401c7db10c358824f7a2d3 +Subproject commit 403657110b1555b0e15702cb96d7fe43e815a036 -- 2.43.0