done

mission/acs/rwHelpers.cpp

@@ -0,0 +1,54 @@
+#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_
+#define MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_
+
+#include "rwHelpers.h"
+
+void rws::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, uint8_t* encodedBuffer,
+                     size_t& encodedLen) {
+  encodedBuffer[0] = rws::FRAME_DELIMITER;
+  encodedLen = 1;
+  for (size_t sourceIdx = 0; sourceIdx < sourceLen; sourceIdx++) {
+    if (sourceBuf[sourceIdx] == 0x7E) {
+      encodedBuffer[encodedLen++] = 0x7D;
+      encodedBuffer[encodedLen++] = 0x5E;
+    } else if (sourceBuf[sourceIdx] == 0x7D) {
+      encodedBuffer[encodedLen++] = 0x7D;
+      encodedBuffer[encodedLen++] = 0x5D;
+    } else {
+      encodedBuffer[encodedLen++] = sourceBuf[sourceIdx];
+    }
+  }
+  encodedBuffer[encodedLen++] = rws::FRAME_DELIMITER;
+}
+
+size_t rws::idToPacketLen(DeviceCommandId_t id) {
+  switch (id) {
+    case (rws::GET_RW_STATUS): {
+      return rws::SIZE_GET_RW_STATUS;
+    }
+    case (rws::SET_SPEED): {
+      return rws::SIZE_SET_SPEED_REPLY;
+    }
+    case (rws::CLEAR_LAST_RESET_STATUS): {
+      return rws::SIZE_CLEAR_RESET_STATUS;
+    }
+    case (rws::GET_LAST_RESET_STATUS): {
+      return rws::SIZE_GET_RESET_STATUS;
+    }
+    case (rws::GET_TEMPERATURE): {
+      return rws::SIZE_GET_TEMPERATURE_REPLY;
+    }
+    case (rws::GET_TM): {
+      return rws::SIZE_GET_TELEMETRY_REPLY;
+    }
+    case (rws::INIT_RW_CONTROLLER): {
+      return rws::SIZE_INIT_RW;
+    }
+    default: {
+      sif::error << "no reply buffer for rw command " << id << std::endl;
+      return 0;
+    }
+  }
+}
+
+#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ */