From 57b41701ce380401dd3f45a0d9c46033b73a3288 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 4 Apr 2023 20:34:28 +0200
Subject: [PATCH] hopefully those were the last fixes

---
 bsp_q7s/core/ObjectFactory.cpp  |  2 +-
 mission/com/SyrlinksHandler.cpp | 27 +++++++++++++++++++--------
 2 files changed, 20 insertions(+), 9 deletions(-)

diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp
index 3f29de5e..3b71c6cd 100644
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@@ -62,9 +62,9 @@
 #include "mission/system/acs/acsModeTree.h"
 #include "mission/system/com/SyrlinksFdir.h"
 #include "mission/system/com/comModeTree.h"
-#include "mission/system/power/GomspacePowerFdir.h"
 #include "mission/system/fdir/RtdFdir.h"
 #include "mission/system/objects/TcsBoardAssembly.h"
+#include "mission/system/power/GomspacePowerFdir.h"
 #include "mission/system/tree/payloadModeTree.h"
 #include "mission/system/tree/tcsModeTree.h"
 #include "mission/tmtc/tmFilters.h"
diff --git a/mission/com/SyrlinksHandler.cpp b/mission/com/SyrlinksHandler.cpp
index 352781a6..1ee490a7 100644
--- a/mission/com/SyrlinksHandler.cpp
+++ b/mission/com/SyrlinksHandler.cpp
@@ -28,7 +28,8 @@ void SyrlinksHandler::doStartUp() {
     if (commandExecuted) {
       // Go to normal mode immediately and disable transmitter on startup.
       setMode(_MODE_TO_ON);
-      internalState = InternalState::TX_TRANSITION;
+      internalState = InternalState::IDLE;
+      transState = TransitionState::IDLE;
       commandExecuted = false;
     }
   }
@@ -99,7 +100,7 @@ ReturnValue_t SyrlinksHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
 }
 
 ReturnValue_t SyrlinksHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
-  if(transState == TransitionState::CMD_PENDING or transState == TransitionState::DONE) {
+  if (transState == TransitionState::CMD_PENDING or transState == TransitionState::DONE) {
     return NOTHING_TO_SEND;
   }
   switch (internalState) {
@@ -761,11 +762,13 @@ void SyrlinksHandler::setDebugMode(bool enable) { this->debugMode = enable; }
 
 void SyrlinksHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
   Mode_t tgtMode = getBaseMode(getMode());
-  if (transState == TransitionState::DONE) {
+  auto doneHandler = [&]() {
     internalState = InternalState::IDLE;
     transState = TransitionState::IDLE;
     DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
-    return;
+  };
+  if (transState == TransitionState::DONE) {
+    return doneHandler();
   }
   auto txStandbyHandler = [&]() {
     txDataset.setReportingEnabled(false);
@@ -777,13 +780,20 @@ void SyrlinksHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
     txDataset.setReportingEnabled(true);
     poolManager.changeCollectionInterval(txDataset.getSid(), 10.0);
     poolManager.changeCollectionInterval(temperatureSet.getSid(), 5.0);
-    internalState = InternalState::TX_TRANSITION;
     transState = tgtTransitionState;
+    internalState = InternalState::TX_TRANSITION;
   };
+
   if (tgtMode == HasModesIF::MODE_ON or tgtMode == DeviceHandlerIF::MODE_NORMAL) {
-    if(getSubmode() == subModeFrom) {
-      DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
+    // If submode has not changed, no special transition handling necessary.
+    if (getSubmode() == subModeFrom) {
+      return doneHandler();
     }
+    // Transition is on-going, wait for it to finish.
+    if (transState != TransitionState::IDLE) {
+      return;
+    }
+    // Transition start logic.
     switch (getSubmode()) {
       case (com::Submode::RX_AND_TX_DEFAULT_DATARATE): {
         auto currentDatarate = com::getCurrentDatarate();
@@ -817,6 +827,7 @@ void SyrlinksHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
     }
   } else if (tgtMode == HasModesIF::MODE_OFF) {
     txStandbyHandler();
+  } else {
+    return doneHandler();
   }
-  DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
 }