this should work better

linux/payload/FreshSupvHandler.cpp

@@ -8,6 +8,7 @@
 
 #include "eive/definitions.h"
 #include "eive/objects.h"
+#include "fsfw/globalfunctions/arrayprinter.h"
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/QueueFactory.h"
 #include "fsfw/objectmanager/ObjectManagerIF.h"
@@ -70,7 +71,9 @@ void FreshSupvHandler::performDeviceOperation(uint8_t opCode) {
         handleTransitionToOff();
       }
     } else {
-      if (mode == MODE_NORMAL) {
+      // I think the SUPV is not able to process multiple commands consecutively, so only send
+      // normal command if no other command is pending.
+      if (mode == MODE_NORMAL and not isCommandPending()) {
         auto cmdIter = activeActionCmds.find(
             buildActiveCmdKey(Apid::HK, static_cast<uint8_t>(tc::HkId::GET_REPORT)));
         if (cmdIter == activeActionCmds.end() or not cmdIter->second.isPending) {
@@ -525,6 +528,9 @@ void FreshSupvHandler::handleTransitionToOff() {
 
 ReturnValue_t FreshSupvHandler::sendCommand(DeviceCommandId_t commandId, TcBase& tc,
                                             bool replyExpected, uint32_t cmdCountdownMs) {
+  if (interCmdCd.isBusy()) {
+    TaskFactory::delayTask(interCmdCd.getRemainingMillis());
+  }
   if (supv::DEBUG_PLOC_SUPV) {
     if (not(supv::REDUCE_NORMAL_MODE_PRINTOUT and mode == MODE_NORMAL and
             commandId == supv::GET_HK_REPORT)) {
@@ -552,7 +558,10 @@ ReturnValue_t FreshSupvHandler::sendCommand(DeviceCommandId_t commandId, TcBase&
     activeCmdIter->second.cmdCountdown.setTimeout(cmdCountdownMs);
     activeCmdIter->second.cmdCountdown.resetTimer();
   }
-  return uartManager->sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen());
+  ReturnValue_t result =
+      uartManager->sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen());
+  interCmdCd.resetTimer();
+  return result;
 }
 
 ReturnValue_t FreshSupvHandler::initialize() {
@@ -824,13 +833,15 @@ ReturnValue_t FreshSupvHandler::parseTmPackets() {
     }
     tmReader.setReadOnlyData(receivedData, receivedSize);
     if (tmReader.checkCrc() != returnvalue::OK) {
-      sif::warning << "PlocSupervisorHandler::parseTmPackets: CRC failure" << std::endl;
+      sif::warning << "PlocSupervisorHandler::parseTmPackets: CRC failure for packet with size "
+                   << receivedSize << std::endl;
+      arrayprinter::print(receivedData, receivedSize);
       continue;
     }
-    uint16_t apid = tmReader.getModuleApid();
     if (supv::DEBUG_PLOC_SUPV) {
       handlePacketPrint();
     }
+    uint16_t apid = tmReader.getModuleApid();
     switch (apid) {
       case (Apid::TMTC_MAN): {
         switch (tmReader.getServiceId()) {
@@ -952,7 +963,7 @@ void FreshSupvHandler::handlePacketPrint() {
         sif::warning << "PlocSupervisorHandler: Parsing EXE failed" << std::endl;
       }
       const char* printStr = "???";
-      if (supv::REDUCE_NORMAL_MODE_PRINTOUT and
+      if (mode == MODE_NORMAL and supv::REDUCE_NORMAL_MODE_PRINTOUT and
           exe.getRefModuleApid() == (uint8_t)supv::Apid::HK and
           exe.getRefServiceId() == (uint8_t)supv::tc::HkId::GET_REPORT) {
         return;
@@ -1539,3 +1550,12 @@ ReturnValue_t FreshSupvHandler::genericHandleTm(const char* contextString, const
   confirmReplyPacketReceived(apid, serviceId);
   return result;
 }
+
+bool FreshSupvHandler::isCommandPending() const {
+  for (const auto& info : activeActionCmds) {
+    if (info.second.isPending) {
+      return true;
+    }
+  }
+  return false;
+}

linux/payload/FreshSupvHandler.h

@@ -101,6 +101,7 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
   Countdown switchTimeout = Countdown(2000);
   // Vorago nees some time to boot properly
   Countdown bootTimeout = Countdown(supv::BOOT_TIMEOUT_MS);
+  Countdown interCmdCd = Countdown(supv::INTER_COMMAND_DELAY);
 
   PoolEntry<uint16_t> adcRawEntry = PoolEntry<uint16_t>(16);
   PoolEntry<uint16_t> adcEngEntry = PoolEntry<uint16_t>(16);
@@ -181,6 +182,8 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
   ReturnValue_t genericHandleTm(const char* contextString, const uint8_t* data,
                                 LocalPoolDataSetBase& set, supv::Apid apid, uint8_t serviceId);
   ReturnValue_t handleLatchupStatusReport(const uint8_t* data);
+
+  bool isCommandPending() const;
 };
 
 #endif /* LINUX_PAYLOAD_FRESHSUPVHANDLER_H_ */

linux/payload/plocSupvDefs.h

@@ -47,6 +47,7 @@ static const Event SUPV_ACK_UNKNOWN_COMMAND = MAKE_EVENT(9, severity::LOW);
 static const Event SUPV_EXE_ACK_UNKNOWN_COMMAND = MAKE_EVENT(10, severity::LOW);
 
 extern std::atomic_bool SUPV_ON;
+static constexpr uint32_t INTER_COMMAND_DELAY = 20;
 static constexpr uint32_t BOOT_TIMEOUT_MS = 4000;
 static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = BOOT_TIMEOUT_MS + 2000;
 static constexpr uint32_t MAX_TRANSITION_TIME_TO_OFF_MS = 1000;