diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 383fd1b1..6c22e91a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -176,6 +176,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, acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); @@ -194,8 +195,9 @@ void AcsController::performSafe() { safeCtrlFailureCounter = 0; break; case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): - safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value, + fusedRotRateData.rotRateOrthogonal.value, + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; @@ -206,6 +208,13 @@ void AcsController::performSafe() { safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; + case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): + safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value, + fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq, + errAng); + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + break; case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING): errAng = NAN; safeCtrlFailureFlag = false; diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index e37d8c3d..4f98543c 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,22 +9,22 @@ 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 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 fusedRateSplitValid, 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) { return acs::SafeModeStrategy::SAFECTRL_MEKF; } else if (gyrEnabled and satRotRateValid and sunDirValid) { return acs::SafeModeStrategy::SAFECTRL_GYR; - } else if (not gyrEnabled and sunDirValid) { + } else if (not gyrEnabled and sunDirValid and fusedRateSplitValid) { return acs::SafeModeStrategy::SAFECTRL_SUSMGM; } else if (gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; - } else if (not gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { + } else if (not gyrEnabled and dampingEnabled and satRotRateValid and fusedRateTotalValid and + not sunDirValid) { return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else if (not dampingEnabled and satRotRateValid and not sunDirValid) { return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index d5ca4dca..fbe401ef 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -14,8 +14,9 @@ class SafeCtrl { acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t gyrEnabled, - const uint8_t dampingEnabled); + const bool fusedRateSplitValid, + const bool fusedRateTotalValid, const uint8_t mekfEnabled, + const uint8_t gyrEnabled, const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, double *magMomB,