Merge pull request 'Improve Detumble State Machine' (#854) from detumble-state-machine-vs-mode-change into main

Reviewed-on: #854
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

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

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);

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,10 +585,15 @@ void AcsController::handleDetumbling() {
     case DetumbleState::DETUMBLE_FROM_SAFE:
       detumbleCounter = 0;
       // Triggers detumble mode transition in subsystem
+      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
           VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
@@ -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);
 }
 

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_ */