From b879533d41b4bb3a663847129121c7fa7ab95aca Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 25 Jan 2023 17:02:46 +0100
Subject: [PATCH] extend syrlinks code a bit

---
 mission/devices/SyrlinksHandler.cpp           | 78 +++++++++++++++----
 mission/devices/SyrlinksHandler.h             |  3 +
 .../devicedefinitions/SyrlinksDefinitions.h   |  7 +-
 tmtc                                          |  2 +-
 4 files changed, 73 insertions(+), 17 deletions(-)

diff --git a/mission/devices/SyrlinksHandler.cpp b/mission/devices/SyrlinksHandler.cpp
index 4205f67f..f4ea46aa 100644
--- a/mission/devices/SyrlinksHandler.cpp
+++ b/mission/devices/SyrlinksHandler.cpp
@@ -674,6 +674,16 @@ ReturnValue_t SyrlinksHandler::handleAckReply(const uint8_t* packet) {
   return result;
 }
 
+ReturnValue_t SyrlinksHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
+  if (mode == HasModesIF::MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL) {
+    if (submode >= syrlinks::Submode::NUM_SUBMODES) {
+      return HasModesIF::INVALID_SUBMODE;
+    }
+    return returnvalue::OK;
+  }
+  return DeviceHandlerBase::isModeCombinationValid(mode, submode);
+}
+
 void SyrlinksHandler::prepareCommand(std::string command, DeviceCommandId_t commandId) {
   command.copy(reinterpret_cast<char*>(commandBuffer), command.size(), 0);
   rawPacketLen = command.size();
@@ -685,37 +695,79 @@ void SyrlinksHandler::setDebugMode(bool enable) { this->debugMode = enable; }
 
 void SyrlinksHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
   Mode_t tgtMode = getBaseMode(getMode());
+  auto commandDone = [&]() {
+    setMode(tgtMode);
+    internalState = InternalState::IDLE;
+  };
   if (tgtMode == HasModesIF::MODE_ON or tgtMode == DeviceHandlerIF::MODE_NORMAL) {
     switch (getSubmode()) {
-      case (syrlinks::Submode::TX_MODULATION): {
-        if (commandExecuted) {
-          setMode(tgtMode);
-          return;
+      case (syrlinks::Submode::TX_MODULATION_LOW_DATARATE): {
+        if (internalState == InternalState::IDLE) {
+          commandExecuted = false;
+          internalState = InternalState::SELECT_MODULATION_QPSK;
+        }
+        if (internalState == InternalState::SELECT_MODULATION_QPSK) {
+          if (commandExecuted) {
+            internalState = InternalState::SET_TX_MODULATION;
+            commandExecuted = false;
+          }
+        }
+        if (internalState == InternalState::SET_TX_MODULATION) {
+          if (commandExecuted) {
+            commandDone();
+            return;
+          }
+        }
+        break;
+      }
+      case (syrlinks::Submode::TX_MODULATION_HIGH_DATARATE): {
+        if (internalState == InternalState::IDLE) {
+          commandExecuted = false;
+          internalState = InternalState::SELECT_MODULATION_0BPSK;
+        }
+        if (internalState == InternalState::SELECT_MODULATION_0BPSK) {
+          if (commandExecuted) {
+            internalState = InternalState::SET_TX_MODULATION;
+            commandExecuted = false;
+          }
+        }
+        if (internalState == InternalState::SET_TX_MODULATION) {
+          if (commandExecuted) {
+            commandDone();
+            return;
+          }
         }
-        internalState = InternalState::SET_TX_MODULATION;
         break;
       }
       case (syrlinks::Submode::TX_STANDBY): {
-        if (commandExecuted) {
-          setMode(tgtMode);
-          return;
+        if (internalState == InternalState::IDLE) {
+          internalState = InternalState::SET_TX_STANDBY;
+          commandExecuted = false;
+        }
+        if (internalState == InternalState::SET_TX_STANDBY) {
+          if (commandExecuted) {
+            commandDone();
+            return;
+          }
         }
-        internalState = InternalState::SET_TX_STANDBY;
         break;
       }
       case (syrlinks::Submode::TX_CW): {
+        if (internalState == InternalState::IDLE) {
+          internalState = InternalState::SET_TX_STANDBY;
+          commandExecuted = false;
+        }
         if (commandExecuted) {
-          setMode(tgtMode);
+          commandDone();
           return;
         }
-        internalState = InternalState::SET_TX_CW;
         break;
       }
       default: {
-        setMode(tgtMode);
+        commandDone();
       }
     }
   } else {
-    setMode(tgtMode);
+    commandDone();
   }
 }
diff --git a/mission/devices/SyrlinksHandler.h b/mission/devices/SyrlinksHandler.h
index 8940da29..b6ca35e7 100644
--- a/mission/devices/SyrlinksHandler.h
+++ b/mission/devices/SyrlinksHandler.h
@@ -35,6 +35,7 @@ class SyrlinksHandler : public DeviceHandlerBase {
   void doStartUp() override;
   void doShutDown() override;
   void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override;
+  ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
   ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override;
   ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override;
   ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData,
@@ -112,6 +113,8 @@ class SyrlinksHandler : public DeviceHandlerBase {
   enum class InternalState {
     OFF,
     ENABLE_TEMPERATURE_PROTECTION,
+    SELECT_MODULATION_0BPSK,
+    SELECT_MODULATION_QPSK,
     SET_TX_MODULATION,
     SET_TX_CW,
     SET_TX_STANDBY,
diff --git a/mission/devices/devicedefinitions/SyrlinksDefinitions.h b/mission/devices/devicedefinitions/SyrlinksDefinitions.h
index 05ac7f0f..755dc9dc 100644
--- a/mission/devices/devicedefinitions/SyrlinksDefinitions.h
+++ b/mission/devices/devicedefinitions/SyrlinksDefinitions.h
@@ -9,9 +9,10 @@ namespace syrlinks {
 enum Submode {
   DEFAULT,
   TX_STANDBY,
-  TX_MODULATION,
-  // TODO: Is this needed?
-  TX_CW
+  TX_MODULATION_LOW_DATARATE,
+  TX_MODULATION_HIGH_DATARATE,
+  TX_CW,
+  NUM_SUBMODES
 };
 
 static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYRLINKS;
diff --git a/tmtc b/tmtc
index 49ccb4be..8a7880bc 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 49ccb4be8d42d6916be00ff9d8462a1f65481a6c
+Subproject commit 8a7880bc35d1069577b7e0427c1f45c54ac5241c