From 73ed59928e97efb17133de7a4246d51a638b7a9f Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 21 Nov 2023 17:37:26 +0100
Subject: [PATCH] check TM before normal periodic OP

---
 fsfw                               |  2 +-
 linux/payload/FreshSupvHandler.cpp | 16 ++++++++++++----
 linux/payload/FreshSupvHandler.h   |  1 +
 3 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/fsfw b/fsfw
index 41d67bff..0021aa29 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 41d67bff639192afa2e18a23db6801c75b4fea88
+Subproject commit 0021aa29f54838a21658dbbaca98fe0e54318d8a
diff --git a/linux/payload/FreshSupvHandler.cpp b/linux/payload/FreshSupvHandler.cpp
index 640c5535..b97d1e8a 100644
--- a/linux/payload/FreshSupvHandler.cpp
+++ b/linux/payload/FreshSupvHandler.cpp
@@ -74,7 +74,8 @@ void FreshSupvHandler::performDeviceOperation(uint8_t opCode) {
       }
     } else {
       // I think the SUPV is not able to process multiple commands consecutively, so only send
-      // normal command if no other command is pending.
+      // normal command if no other command is pending. We handle the action queue first, which
+      // should ensure that these commands take precendence.
       if (mode == MODE_NORMAL and not isCommandPending()) {
         auto cmdIter = activeActionCmds.find(
             buildActiveCmdKey(Apid::HK, static_cast<uint8_t>(tc::HkId::GET_REPORT)));
@@ -101,6 +102,15 @@ void FreshSupvHandler::performDeviceOperation(uint8_t opCode) {
   }
 }
 
+ReturnValue_t FreshSupvHandler::performDeviceOperationPreQueueHandling(uint8_t opCode) {
+  if (opCode != OpCode::DEFAULT_OPERATION) {
+    return returnvalue::OK;
+  }
+  // We parse for TM packets shortly before handling the queue, this might complete some packets,
+  // which then allows the handling of new action commands.
+  return parseTmPackets();
+}
+
 ReturnValue_t FreshSupvHandler::handleCommandMessage(CommandMessage* message) {
   // No custom messages.
   return returnvalue::FAILED;
@@ -254,6 +264,7 @@ ReturnValue_t FreshSupvHandler::executeAction(ActionId_t actionId, MessageQueueI
     return HasActionsIF::IS_BUSY;
   }
   spParams.buf = commandBuffer.data();
+  this->commandedByCached = commandedBy;
   switch (actionId) {
     case GET_HK_REPORT: {
       sendEmptyCmd(supv::GET_HK_REPORT, Apid::HK, static_cast<uint8_t>(tc::HkId::GET_REPORT), true);
@@ -395,9 +406,6 @@ ReturnValue_t FreshSupvHandler::executeAction(ActionId_t actionId, MessageQueueI
       result = DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
       break;
   }
-  if (result == returnvalue::OK) {
-    this->commandedByCached = commandedBy;
-  }
   return result;
 }
 
diff --git a/linux/payload/FreshSupvHandler.h b/linux/payload/FreshSupvHandler.h
index f938a771..259b70b6 100644
--- a/linux/payload/FreshSupvHandler.h
+++ b/linux/payload/FreshSupvHandler.h
@@ -57,6 +57,7 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
    */
   void startTransition(Mode_t newMode, Submode_t submode) override;
 
+  ReturnValue_t performDeviceOperationPreQueueHandling(uint8_t opCode) override;
   void handleTransitionToOn();
   void handleTransitionToOff();