scexDataHandler

mission/devices/devicedefinitions/ScexDefinitions.cpp

@@ -1,5 +1,36 @@
 #include "ScexDefinitions.h"
 
-uint8_t scex::createCmdByte(ScexCmds cmd, bool tempCheck) {
+#include <fsfw/globalfunctions/CRC.h>
+
+#include <cstring>
+
+uint8_t scex::createCmdByte(Cmds cmd, bool tempCheck) {
   return (IDLE_BIT_0_DEF_STATE << 7) | (IDLE_BIT_1_DEF_STATE << 6) | (cmd << 1) | tempCheck;
 }
+
+ReturnValue_t scex::prepareScexCmd(scex::Cmds cmd, bool tempCheck,
+                                   std::pair<uint8_t*, size_t> cmdBufPair, size_t& cmdLen,
+                                   std::pair<const uint8_t*, size_t> usrDataPair) {
+  using namespace scex;
+  uint8_t* cmdBuf = cmdBufPair.first;
+  const uint8_t* userData = usrDataPair.first;
+  // Send command
+  if (cmdBuf == nullptr or (cmdBufPair.second < usrDataPair.second + HEADER_LEN + CRC_LEN) or
+      (usrDataPair.second > 0 and userData == nullptr)) {
+    cmdLen = 0;
+    return HasReturnvaluesIF::RETURN_FAILED;
+  }
+  cmdBuf[0] = scex::createCmdByte(cmd, tempCheck);
+  // These two fields are the packet counter and the total packet count. Those are 1 and 1 for each
+  // telecommand so far
+  cmdBuf[1] = 1;
+  cmdBuf[2] = 1;
+  cmdBuf[3] = (usrDataPair.second >> 8) & 0xff;
+  cmdBuf[4] = usrDataPair.second & 0xff;
+  std::memcpy(cmdBuf + HEADER_LEN, userData, usrDataPair.second);
+  uint16_t crc = CRC::crc16ccitt(cmdBuf, usrDataPair.second + HEADER_LEN);
+  cmdBuf[usrDataPair.second + HEADER_LEN] = (crc >> 8) & 0xff;
+  cmdBuf[usrDataPair.second + HEADER_LEN + 1] = crc & 0xff;
+  cmdLen = usrDataPair.second + HEADER_LEN + CRC_LEN;
+  return HasReturnvaluesIF::RETURN_OK;
+}