From d82a6ece5a0c824476ffd8306f0dc9ed658acb53 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 15 Aug 2023 09:11:57 +0200 Subject: [PATCH] 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);