From 4d22f7f8898ae85b35346fad33df4d69e7ed265c Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 31 Jan 2024 15:41:21 +0100
Subject: [PATCH] make detumble state machine robust agains mode changes

---
 mission/acs/defs.h                   |  3 +++
 mission/controller/AcsController.cpp | 26 ++++++++++++++++++++++----
 mission/controller/AcsController.h   |  4 ++++
 3 files changed, 29 insertions(+), 4 deletions(-)

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