diff --git a/CHANGELOG.md b/CHANGELOG.md index 21792d0e..b3ca4e51 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- Detumbling State Machine is now robust to commanded mode changes. + # [v7.6.0] 2024-01-30 - Bumped `eive-tmtc` to v5.13.0 diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 67fcf024..47f7f170 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -70,6 +70,9 @@ static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); static constexpr Event PTG_RATE_VIOLATION = MAKE_EVENT(10, severity::MEDIUM); //! [EXPORT] : [COMMENT] The system has recovered from a rate rotation violation. static constexpr Event RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); +//! [EXPORT] : [COMMENT] The detumble transition has failed. +//! //! P1: Last detumble state before failure. +static constexpr Event DETUMBLE_TRANSITION_FAILED = MAKE_EVENT(11, severity::HIGH); //! [EXPORT] : [COMMENT] Multiple RWs are invalid, uncommandable and therefore higher ACS modes //! cannot be maintained. static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 264e9f24..2036e6ab 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -568,9 +568,16 @@ void AcsController::handleDetumbling() { break; case DetumbleState::DETUMBLE_FROM_PTG: triggerEvent(acs::PTG_RATE_VIOLATION); + detumbleTransitionCountdow.resetTimer(); detumbleState = DetumbleState::PTG_TO_SAFE_TRANSITION; break; case DetumbleState::PTG_TO_SAFE_TRANSITION: + if (detumbleTransitionCountdow.hasTimedOut()) { + triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 2); + detumbleCounter = 0; + detumbleState = DetumbleState::NO_DETUMBLE; + break; + } if (mode == acs::AcsMode::SAFE) { detumbleState = DetumbleState::DETUMBLE_FROM_SAFE; } @@ -578,9 +585,14 @@ void AcsController::handleDetumbling() { case DetumbleState::DETUMBLE_FROM_SAFE: detumbleCounter = 0; // Triggers detumble mode transition in subsystem - triggerEvent(acs::SAFE_RATE_VIOLATION); - startTransition(mode, acs::SafeSubmode::DETUMBLE); - detumbleState = DetumbleState::IN_DETUMBLE; + if (mode == acs::AcsMode::SAFE) { + triggerEvent(acs::SAFE_RATE_VIOLATION); + startTransition(mode, acs::SafeSubmode::DETUMBLE); + detumbleState = DetumbleState::IN_DETUMBLE; + break; + } + triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 3); + detumbleState = DetumbleState::NO_DETUMBLE; break; case DetumbleState::IN_DETUMBLE: if (fusedRotRateData.rotRateTotal.isValid() and @@ -885,6 +897,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, } void AcsController::modeChanged(Mode_t mode, Submode_t submode) { + guidance.resetValues(); if (mode == acs::AcsMode::SAFE) { { PoolReadGuard pg(&rw1SpeedSet); @@ -903,7 +916,12 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) { rw4SpeedSet.setRwSpeed(0, 10); } } - guidance.resetValues(); + if (submode == acs::SafeSubmode::DETUMBLE) { + detumbleState = DetumbleState::IN_DETUMBLE; + } + if (detumbleState == DetumbleState::IN_DETUMBLE and submode != acs::SafeSubmode::DETUMBLE) { + detumbleState = DetumbleState::NO_DETUMBLE; + } return ExtendedControllerBase::modeChanged(mode, submode); } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index ed893479..e8102f35 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -283,6 +283,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); + + // Countdown after which the detumbling mode change should have been finished + static constexpr dur_millis_t MAX_DURATION = 60 * 1e3; + Countdown detumbleTransitionCountdow = Countdown(MAX_DURATION); }; #endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */