diff --git a/linux/payload/FreshSupvHandler.cpp b/linux/payload/FreshSupvHandler.cpp index edd40868..eefd64af 100644 --- a/linux/payload/FreshSupvHandler.cpp +++ b/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(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; +} diff --git a/linux/payload/FreshSupvHandler.h b/linux/payload/FreshSupvHandler.h index be968564..86cc9147 100644 --- a/linux/payload/FreshSupvHandler.h +++ b/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 adcRawEntry = PoolEntry(16); PoolEntry adcEngEntry = PoolEntry(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_ */ diff --git a/linux/payload/plocSupvDefs.h b/linux/payload/plocSupvDefs.h index 0ac9d9b1..4423bbb3 100644 --- a/linux/payload/plocSupvDefs.h +++ b/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;