Merge pull request 'MPSoC Handler Update' (#219) from meier/plocMPSoC into develop

Reviewed-on: #219
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

bsp_te0720_1cfa/InitMission.cpp

@@ -82,7 +82,7 @@ void initmission::initTasks() {
 
   std::vector<PeriodicTaskIF*> pstTasks;
   FixedTimeslotTaskIF* pst = factory->createFixedTimeslotTask(
-      "UART_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc);
+      "UART_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc);
   result = pst::pstUart(pst);
   if (result != HasReturnvaluesIF::RETURN_OK) {
     sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;

bsp_te0720_1cfa/ObjectFactory.cpp

@@ -44,16 +44,15 @@ void ObjectFactory::produce(void* args) {
   ObjectFactory::produceGenericObjects();
 
 #if OBSW_ADD_PLOC_MPSOC == 1
-  UartCookie* mpsocUartCookie =
+  UartCookie* mpsocUartCookie = new UartCookie(objects::PLOC_MPSOC_HANDLER, te0720_1cfa::MPSOC_UART,
-      new UartCookie(objects::PLOC_MPSOC_HANDLER, te0720_1cfa::MPSOC_UART, UartModes::NON_CANONICAL,
+                                               uart::PLOC_MPSOC_BAUD, mpsoc::MAX_REPLY_SIZE);
-                     uart::PLOC_MPSOC_BAUD, mpsoc::MAX_REPLY_SIZE);
   mpsocUartCookie->setNoFixedSizeReply();
   PlocMPSoCHelper* plocMpsocHelper = new PlocMPSoCHelper(objects::PLOC_MPSOC_HELPER);
   new UartComIF(objects::UART_COM_IF);
   auto dummyGpioIF = new DummyGpioIF();
-  PlocMPSoCHandler* plocMPSoCHandler =
+  PlocMPSoCHandler* plocMPSoCHandler = new PlocMPSoCHandler(
-      new PlocMPSoCHandler(objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocUartCookie,
+      objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocUartCookie, plocMpsocHelper,
-                           plocMpsocHelper, Gpio(gpioIds::ENABLE_MPSOC_UART, dummyGpioIF));
+      Gpio(gpioIds::ENABLE_MPSOC_UART, dummyGpioIF), objects::PLOC_SUPERVISOR_HANDLER);
   plocMPSoCHandler->setStartUpImmediately();
 #endif /* OBSW_ADD_PLOC_MPSOC == 1 */
 

fsfw

@@ -1 +1 @@
-Subproject commit e949368b062e8703c35d2043ece8d7258cd2608b
+Subproject commit 613dbe9592c30d9acf4cdb95d81d9f216f07374b

linux/devices/devicedefinitions/PlocMPSoCDefinitions.h

@@ -26,6 +26,9 @@ static const DeviceCommandId_t TC_REPLAY_WRITE_SEQUENCE = 13;
 static const DeviceCommandId_t TC_DOWNLINK_PWR_ON = 14;
 static const DeviceCommandId_t TC_DOWNLINK_PWR_OFF = 15;
 static const DeviceCommandId_t TC_MODE_REPLAY = 16;
+static const DeviceCommandId_t TC_CAM_CMD_SEND = 17;
+static const DeviceCommandId_t TC_MODE_IDLE = 18;
+static const DeviceCommandId_t TM_CAM_CMD_RPT = 19;
 
 // Will reset the sequence count of the OBSW
 static const DeviceCommandId_t OBSW_RESET_SEQ_COUNT = 50;
@@ -33,6 +36,7 @@ static const DeviceCommandId_t OBSW_RESET_SEQ_COUNT = 50;
 static const uint16_t SIZE_ACK_REPORT = 14;
 static const uint16_t SIZE_EXE_REPORT = 14;
 static const uint16_t SIZE_TM_MEM_READ_REPORT = 18;
+static const uint16_t SIZE_TM_CAM_CMD_RPT = 18;
 
 /**
  * SpacePacket apids of PLOC telecommands and telemetry.
@@ -49,18 +53,23 @@ static const uint16_t TC_FLASHFOPEN = 0x119;
 static const uint16_t TC_FLASHFCLOSE = 0x11A;
 static const uint16_t TC_FLASHDELETE = 0x11C;
 static const uint16_t TC_MODE_REPLAY = 0x11F;
+static const uint16_t TC_MODE_IDLE = 0x11E;
 static const uint16_t TC_DOWNLINK_PWR_OFF = 0x124;
+static const uint16_t TC_CAM_CMD_SEND = 0x12C;
 static const uint16_t TM_MEMORY_READ_REPORT = 0x404;
 static const uint16_t ACK_SUCCESS = 0x400;
 static const uint16_t ACK_FAILURE = 0x401;
 static const uint16_t EXE_SUCCESS = 0x402;
 static const uint16_t EXE_FAILURE = 0x403;
+static const uint16_t TM_CAM_CMD_RPT = 0x407;
 }  // namespace apid
 
 /** Offset from first byte in space packet to first byte of data field */
 static const uint8_t DATA_FIELD_OFFSET = 6;
 static const size_t MEM_READ_RPT_LEN_OFFSET = 10;
 static const char NULL_TERMINATOR = '\0';
+static const uint8_t MIN_SPACE_PACKET_LENGTH = 7;
+static const uint8_t SPACE_PACKET_HEADER_SIZE = 6;
 
 /**
  * The size of payload data which will be forwarded to the requesting object. e.g. PUS Service
@@ -117,11 +126,11 @@ class TcBase : public SpacePacket, public MPSoCReturnValuesIF {
   virtual ReturnValue_t createPacket(const uint8_t* commandData, size_t commandDataLen) {
     ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
     result = initPacket(commandData, commandDataLen);
-    if (result != HasReturnvaluesIF::HasReturnvaluesIF::RETURN_OK) {
+    if (result != HasReturnvaluesIF::RETURN_OK) {
       return result;
     }
     result = addCrc();
-    if (result != HasReturnvaluesIF::HasReturnvaluesIF::RETURN_OK) {
+    if (result != HasReturnvaluesIF::RETURN_OK) {
       return result;
     }
     return result;
@@ -615,6 +624,45 @@ class TcModeReplay : public TcBase {
   }
 };
 
+/**
+ * @brief   Class to build mode idle command
+ */
+class TcModeIdle : public TcBase {
+ public:
+  TcModeIdle(uint16_t sequenceCount) : TcBase(apid::TC_MODE_IDLE, sequenceCount) {}
+
+  ReturnValue_t createPacket() {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    result = addCrc();
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+      return result;
+    }
+    this->setPacketDataLength(static_cast<uint16_t>(CRC_SIZE - 1));
+    return HasReturnvaluesIF::RETURN_OK;
+  }
+};
+
+class TcCamcmdSend : public TcBase {
+public:
+  TcCamcmdSend(uint16_t sequenceCount) : TcBase(apid::TC_CAM_CMD_SEND, sequenceCount) {}
+
+protected:
+    ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) {
+      if (commandDataLen > MAX_DATA_LENGTH) {
+        return INVALID_LENGTH;
+      }
+      std::memcpy(this->getPacketData(), commandData, commandDataLen);
+      *(this->getPacketData() + commandDataLen) = CARRIAGE_RETURN;
+      uint16_t trueLength = commandDataLen + sizeof(CARRIAGE_RETURN) + CRC_SIZE;
+      this->setPacketDataLength(trueLength - 1);
+      return HasReturnvaluesIF::RETURN_OK;
+    }
+private:
+    static const uint8_t MAX_DATA_LENGTH = 10;
+    static const uint8_t CARRIAGE_RETURN = 0xD;
+
+};
+
 }  // namespace mpsoc
 
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ */

linux/devices/ploc/PlocMPSoCHandler.cpp

@@ -133,6 +133,7 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI
 }
 
 void PlocMPSoCHandler::doStartUp() {
+#ifdef XIPHOS_Q7S
   switch (powerState) {
     case PowerState::OFF:
       commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
@@ -145,6 +146,9 @@ void PlocMPSoCHandler::doStartUp() {
     default:
       break;
   }
+#else
+  setMode(_MODE_TO_ON);
+#endif /* XIPHOS_Q7S */
 }
 
 void PlocMPSoCHandler::doShutDown() {
@@ -211,6 +215,14 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device
       result = prepareTcModeReplay();
       break;
     }
+    case (mpsoc::TC_MODE_IDLE): {
+      result = prepareTcModeIdle();
+      break;
+    }
+    case (mpsoc::TC_CAM_CMD_SEND): {
+      result = prepareTcCamCmdSend(commandData, commandDataLen);
+      break;
+    }
     default:
       sif::debug << "PlocMPSoCHandler::buildCommandFromCommand: Command not implemented"
                  << std::endl;
@@ -238,16 +250,21 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() {
   this->insertInCommandMap(mpsoc::TC_DOWNLINK_PWR_OFF);
   this->insertInCommandMap(mpsoc::TC_REPLAY_WRITE_SEQUENCE);
   this->insertInCommandMap(mpsoc::TC_MODE_REPLAY);
-  this->insertInReplyMap(mpsoc::ACK_REPORT, 1, nullptr, mpsoc::SIZE_ACK_REPORT);
+  this->insertInCommandMap(mpsoc::TC_MODE_IDLE);
+  this->insertInCommandMap(mpsoc::TC_CAM_CMD_SEND);
+  this->insertInReplyMap(mpsoc::ACK_REPORT, 3, nullptr, mpsoc::SIZE_ACK_REPORT);
   this->insertInReplyMap(mpsoc::EXE_REPORT, 3, nullptr, mpsoc::SIZE_EXE_REPORT);
   this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT);
+   this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, SpacePacket::PACKET_MAX_SIZE);
 }
 
 ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                              DeviceCommandId_t* foundId, size_t* foundLen) {
   ReturnValue_t result = RETURN_OK;
 
-  uint16_t apid = (*(start) << 8 | *(start + 1)) & APID_MASK;
+  SpacePacket spacePacket;
+  std::memcpy(spacePacket.getWholeData(), start, remainingSize);
+  uint16_t apid = spacePacket.getAPID();
 
   switch (apid) {
     case (mpsoc::apid::ACK_SUCCESS):
@@ -262,6 +279,11 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain
       *foundLen = tmMemReadReport.rememberRequestedSize;
       *foundId = mpsoc::TM_MEMORY_READ_REPORT;
       break;
+    case (mpsoc::apid::TM_CAM_CMD_RPT):
+      *foundLen = spacePacket.getFullSize();
+      tmCamCmdRpt.rememberSpacePacketSize = *foundLen;
+      *foundId = mpsoc::TM_CAM_CMD_RPT;
+      break;
     case (mpsoc::apid::EXE_SUCCESS):
       *foundLen = mpsoc::SIZE_EXE_REPORT;
       *foundId = mpsoc::EXE_REPORT;
@@ -276,6 +298,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain
       return MPSoCReturnValuesIF::INVALID_APID;
     }
   }
+
   sequenceCount++;
   uint16_t recvSeqCnt = (*(start + 2) << 8 | *(start + 3)) & PACKET_SEQUENCE_COUNT_MASK;
   if (recvSeqCnt != sequenceCount) {
@@ -297,6 +320,10 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const
       result = handleMemoryReadReport(packet);
       break;
     }
+    case (mpsoc::TM_CAM_CMD_RPT): {
+      result = handleCamCmdRpt(packet);
+      break;
+    }
     case (mpsoc::EXE_REPORT): {
       result = handleExecutionReport(packet);
       break;
@@ -463,6 +490,37 @@ ReturnValue_t PlocMPSoCHandler::prepareTcModeReplay() {
   return RETURN_OK;
 }
 
+ReturnValue_t PlocMPSoCHandler::prepareTcModeIdle() {
+  ReturnValue_t result = RETURN_OK;
+  sequenceCount++;
+  mpsoc::TcModeIdle tcModeIdle(sequenceCount);
+  result = tcModeIdle.createPacket();
+  if (result != RETURN_OK) {
+    sequenceCount--;
+    return result;
+  }
+  memcpy(commandBuffer, tcModeIdle.getWholeData(), tcModeIdle.getFullSize());
+  rawPacket = commandBuffer;
+  rawPacketLen = tcModeIdle.getFullSize();
+  nextReplyId = mpsoc::ACK_REPORT;
+  return RETURN_OK;
+}
+
+ReturnValue_t PlocMPSoCHandler::prepareTcCamCmdSend(const uint8_t* commandData,
+                                                    size_t commandDataLen) {
+  ReturnValue_t result = RETURN_OK;
+  sequenceCount++;
+  mpsoc::TcCamcmdSend tcCamCmdSend(sequenceCount);
+  result = tcCamCmdSend.createPacket(commandData, commandDataLen);
+  if (result != RETURN_OK) {
+    sequenceCount--;
+    return result;
+  }
+  copyToCommandBuffer(&tcCamCmdSend);
+  nextReplyId = mpsoc::TM_CAM_CMD_RPT;
+  return RETURN_OK;
+}
+
 void PlocMPSoCHandler::copyToCommandBuffer(mpsoc::TcBase* tc) {
   if (tc == nullptr) {
     sif::debug << "PlocMPSoCHandler::copyToCommandBuffer: Invalid TC" << std::endl;
@@ -586,6 +644,25 @@ ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) {
   return result;
 }
 
+ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) {
+  ReturnValue_t result = RETURN_OK;
+  SpacePacket packet;
+  std::memcpy(packet.getWholeData(), data, tmCamCmdRpt.rememberSpacePacketSize);
+  result = verifyPacket(data, tmCamCmdRpt.rememberSpacePacketSize);
+  if (result == MPSoCReturnValuesIF::CRC_FAILURE) {
+    sif::warning << "PlocMPSoCHandler::handleCamCmdRpt: CRC failure" << std::endl;
+  }
+  const uint8_t* dataFieldPtr = data + mpsoc::SPACE_PACKET_HEADER_SIZE;
+  std::string camCmdRptMsg(reinterpret_cast<const char*>(
+      dataFieldPtr), tmCamCmdRpt.rememberSpacePacketSize - mpsoc::SPACE_PACKET_HEADER_SIZE - 3);
+  uint8_t ackValue = *(packet.getPacketData() + packet.getPacketDataLength() - 2);
+  sif::info << "CamCmdRpt message: " << camCmdRptMsg << std::endl;
+  sif::info << "CamCmdRpt Ack value: 0x" << std::hex << static_cast<unsigned int>(ackValue)
+            << std::endl;
+  handleDeviceTM(packet.getPacketData(), packet.getPacketDataLength() - 1, mpsoc::TM_CAM_CMD_RPT);
+  return result;
+}
+
 ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator command,
                                                       uint8_t expectedReplies, bool useAlternateId,
                                                       DeviceCommandId_t alternateReplyID) {
@@ -602,6 +679,7 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator
     case mpsoc::TC_DOWNLINK_PWR_OFF:
     case mpsoc::TC_REPLAY_WRITE_SEQUENCE:
     case mpsoc::TC_MODE_REPLAY:
+    case mpsoc::TC_MODE_IDLE:
       enabledReplies = 2;
       break;
     case mpsoc::TC_MEM_READ: {
@@ -611,6 +689,18 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator
       if (result != RETURN_OK) {
         sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id "
                    << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl;
+        return result;
+      }
+      break;
+    }
+    case mpsoc::TC_CAM_CMD_SEND: {
+      enabledReplies = 3;
+      result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true,
+                                                        mpsoc::TM_CAM_CMD_RPT);
+      if (result != RETURN_OK) {
+        sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id "
+                   << mpsoc::TM_CAM_CMD_RPT << " not in replyMap" << std::endl;
+        return result;
       }
       break;
     }
@@ -690,6 +780,11 @@ size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) {
         replyLen = tmMemReadReport.rememberRequestedSize;
         break;
       }
+      case mpsoc::TM_CAM_CMD_RPT:
+        // Read acknowledgment, camera and execution report in one go because length of camera
+        // report is not fixed
+        replyLen = SpacePacket::PACKET_MAX_SIZE;
+        break;
       default: {
         replyLen = iter->second.replyLen;
         break;

linux/devices/ploc/PlocMPSoCHandler.h

@@ -4,6 +4,7 @@
 #include <string>
 
 #include "PlocMPSoCHelper.h"
+#include "fsfw/tmtcpacket/SpacePacket.h"
 #include "fsfw/action/CommandActionHelper.h"
 #include "fsfw/action/CommandsActionsIF.h"
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
@@ -126,14 +127,26 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF {
   // Used to block incoming commands when MPSoC helper class is currently executing a command
   bool plocMPSoCHelperExecuting = false;
 
-  class TmMemReadReport {
+  struct TmMemReadReport {
-   public:
     static const uint8_t FIX_SIZE = 14;
     size_t rememberRequestedSize = 0;
   };
 
   TmMemReadReport tmMemReadReport;
 
+  struct TmCamCmdRpt {
+    size_t rememberSpacePacketSize = 0;
+  };
+
+  TmCamCmdRpt tmCamCmdRpt;
+
+  struct TelemetryBuffer {
+    uint16_t length = 0;
+    uint8_t buffer[SpacePacket::PACKET_MAX_SIZE];
+  };
+
+  TelemetryBuffer tmBuffer;
+
   enum class PowerState { OFF, BOOTING, SHUTDOWN, ON };
 
   PowerState powerState = PowerState::OFF;
@@ -151,7 +164,8 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF {
   ReturnValue_t prepareTcDownlinkPwrOn(const uint8_t* commandData, size_t commandDataLen);
   ReturnValue_t prepareTcDownlinkPwrOff();
   ReturnValue_t prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen);
-
+  ReturnValue_t prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcModeIdle();
   /**
    * @brief   Copies space packet into command buffer
    */
@@ -194,6 +208,8 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF {
    */
   ReturnValue_t handleMemoryReadReport(const uint8_t* data);
 
+  ReturnValue_t handleCamCmdRpt(const uint8_t* data);
+
   /**
    * @brief   Depending on the current active command, this function sets the reply id of the
    *          next reply after a successful acknowledgment report has been received. This is

tmtc

@@ -1 +1 @@
-Subproject commit 8a30f669f075c284494d7c1c6618e42e2aec8f15
+Subproject commit 45470f8c05ef214eb41940878ef0bfabf36a4891