diff --git a/CHANGELOG.md b/CHANGELOG.md index c44238ee..0ec0f64f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,7 @@ will consitute of a breaking change warranting a new major release: ## Fixed - Fixed bugs in `Guidance::comparePtg` and corrected overloading +- Detumbling State Machine is now robust to commanded mode changes. # [v7.6.0] 2024-01-30 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 f5f246c2..8d35a8fd 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -567,9 +567,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; } @@ -577,9 +584,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 @@ -884,6 +896,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); @@ -902,7 +915,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_ */