seems to work fine

linux/payload/PlocMpsocHandler.cpp

@@ -79,6 +79,11 @@ ReturnValue_t PlocMPSoCHandler::initialize() {
 }
 
 void PlocMPSoCHandler::performOperationHook() {
+  if (commandIsPending and cmdCountdown.hasTimedOut()) {
+    commandIsPending = false;
+    // TODO: Better returnvalue?
+    cmdDoneHandler(false, returnvalue::FAILED);
+  }
   EventMessage event;
   for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
        result = eventQueue->receiveMessage(&event)) {
@@ -106,6 +111,7 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI
                                               const uint8_t* data, size_t size) {
   ReturnValue_t result = returnvalue::OK;
   commandIsPending = true;
+  cmdCountdown.resetTimer();
   switch (actionId) {
     case mpsoc::SET_UART_TX_TRISTATE: {
       uartIsolatorSwitch.pullLow();
@@ -242,6 +248,7 @@ ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id)
   if (not commandIsPending) {
     *id = mpsoc::TC_GET_HK_REPORT;
     commandIsPending = true;
+    cmdCountdown.resetTimer();
     return buildCommandFromCommand(*id, nullptr, 0);
   }
   return NOTHING_TO_SEND;
@@ -439,6 +446,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain
   }
   // This sequence count ping pong does not make any sense but it is how the MPSoC expects it.
   sequenceCount++;
+
   return result;
 }
 
@@ -817,23 +825,9 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) {
 
   uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK;
 
-  auto cmdDoneHandler = [&](bool success) {
-    if (normalCmdPending) {
-      normalCmdPending = false;
-    }
-    commandIsPending = false;
-    auto commandIter = deviceCommandMap.find(getPendingCommand());
-    if (commandIter != deviceCommandMap.end()) {
-      commandIter->second.isExecuting = false;
-      if (commandIter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) {
-        actionHelper.finish(success, commandIter->second.sendReplyTo, getPendingCommand(), result);
-      }
-    }
-    disableAllReplies();
-  };
   switch (apid) {
     case (mpsoc::apid::EXE_SUCCESS): {
-      cmdDoneHandler(true);
+      cmdDoneHandler(true, result);
       break;
     }
     case (mpsoc::apid::EXE_FAILURE): {
@@ -850,7 +844,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) {
       printStatus(data);
       sendFailureReport(mpsoc::EXE_REPORT, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE);
       result = IGNORE_REPLY_DATA;
-      cmdDoneHandler(false);
+      cmdDoneHandler(false, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE);
       break;
     }
     default: {
@@ -1482,6 +1476,24 @@ LocalPoolDataSetBase* PlocMPSoCHandler::getDataSetHandle(sid_t sid) {
   return nullptr;
 }
 
+bool PlocMPSoCHandler::dontCheckQueue() {
+  // The TC and TMs need to be handled strictly sequentially, so while a command is pending,
+  // more specifically while replies are still expected, do not check the queue.s
+  return commandIsPending;
+}
+
+void PlocMPSoCHandler::cmdDoneHandler(bool success, ReturnValue_t result) {
+  commandIsPending = false;
+  auto commandIter = deviceCommandMap.find(getPendingCommand());
+  if (commandIter != deviceCommandMap.end()) {
+    commandIter->second.isExecuting = false;
+    if (commandIter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) {
+      actionHelper.finish(success, commandIter->second.sendReplyTo, getPendingCommand(), result);
+    }
+  }
+  disableAllReplies();
+}
+
 std::string PlocMPSoCHandler::getStatusString(uint16_t status) {
   switch (status) {
     case (mpsoc::status_code::UNKNOWN_APID): {

linux/payload/PlocMpsocHandler.h

@@ -28,7 +28,8 @@
  * @note The sequence count in the space packets must be incremented with each received and sent
  *       packet otherwise the MPSoC will reply with an acknowledgment failure report.
  *
- * @author	J. Meier
+ * NOTE: This is not an example for a good device handler, DO NOT USE THIS AS A REFERENCE HANDLER.
+ * @author	J. Meier, R. Mueller
  */
 class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF {
  public:
@@ -79,6 +80,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF {
   size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override;
   ReturnValue_t doSendReadHook() override;
   LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
+  bool dontCheckQueue() override;
 
  private:
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_MPSOC_HANDLER;
@@ -108,7 +110,6 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF {
 
   mpsoc::HkReport hkReport;
 
-  bool normalCmdPending = false;
   MessageQueueIF* eventQueue = nullptr;
   MessageQueueIF* commandActionHelperQueue = nullptr;
 
@@ -177,6 +178,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF {
   };
 
   TmMemReadReport tmMemReadReport;
+  Countdown cmdCountdown = Countdown(10000);
 
   struct TelemetryBuffer {
     uint16_t length = 0;
@@ -301,6 +303,8 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF {
 
   uint16_t getStatus(const uint8_t* data);
 
+  void cmdDoneHandler(bool success, ReturnValue_t result);
+
   void handleActionCommandFailure(ActionId_t actionId);
 
   std::string getStatusString(uint16_t status);