diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 3929d842..d8e97728 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -17,17 +17,24 @@ acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy( 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 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 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; + } else if (sunDirValid) { + if (gyrEnabled and satRotRateValid) { + return acs::SafeModeStrategy::SAFECTRL_GYR; + } else if (not gyrEnabled and fusedRateSplitValid) { + return acs::SafeModeStrategy::SAFECTRL_SUSMGM; + } else { + return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } + } else if (not sunDirValid) { + if (gyrEnabled and dampingEnabled and satRotRateValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + } else if (not gyrEnabled and dampingEnabled and satRotRateValid and fusedRateTotalValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; + } else if (not dampingEnabled and satRotRateValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; + } else { + return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } } else { return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; }