Implement serial interaction

modules/control/Firmware/src/Communication/Serial/SerialReceiver.h

@@ -0,0 +1,61 @@
+#pragma once
+#include <Arduino.h>
+#include <FreeRTOS/Queue.h>
+#include <FreeRTOS/Task.h>
+#include <FreeRTOS/Util.h>
+#include <Messages/Message.h>
+#include <Util/CircularBuffer.h>
+#include <etl/string.h>
+
+namespace comm {
+namespace serial {
+template <size_t bufferSize = 20> class SerialReceiver {
+public:
+  SerialReceiver(HardwareSerial &port,
+                 freertos::Queue<message::Message *> &messageQueue,
+                 UBaseType_t priority, BaseType_t coreID)
+      : port{port}, messageQueue{messageQueue},
+        serialReceiverTask{
+            exec, "receive serial data", stackDepth, this, priority, coreID} {}
+
+private:
+  static void exec(void *receiverPtr) {
+    freertos::sleep(20); // wait until Serial port has been initialized
+    SerialReceiver<bufferSize> *receiver =
+        static_cast<SerialReceiver<bufferSize> *>(receiverPtr);
+
+    while (true) {
+      receiver->magicNumberBuf.clear();
+      receiver->magicNumberBuf.resize(receiver->magicNumber.length());
+      size_t availableBytes = receiver->port.available();
+      if (availableBytes >= receiver->magicNumber.length() + 1) {
+        receiver->port.readBytes(receiver->magicNumberBuf.data(),
+                                 receiver->magicNumber.length());
+        receiver->port.println(receiver->magicNumberBuf ==
+                               receiver->magicNumber);
+
+        uint8_t size;
+        receiver->port.readBytes(&size, sizeof(size));
+        if (receiver->port.available() >= size) {
+          message::Message &msg = receiver->buffer.pop();
+          msg.second.lock();
+          msg.first.clear();
+          msg.first.resize(size);
+          receiver->port.readBytes(msg.first.data(), size);
+          msg.second.unlock();
+          receiver->messageQueue.push(&msg);
+        }
+      }
+    }
+  }
+
+  HardwareSerial &port;
+  freertos::Queue<message::Message *> &messageQueue;
+  etl::string<4> magicNumberBuf;
+  const etl::string<2> magicNumber{"HX"};
+  util::CircularBuffer<message::Message, bufferSize> buffer;
+  freertos::Task serialReceiverTask;
+  static const uint32_t stackDepth = 2048;
+};
+} // namespace serial
+} // namespace comm

modules/control/Firmware/src/Communication/Serial/SerialSender.cpp

@@ -0,0 +1,24 @@
+#include <Communication/Serial/SerialSender.h>
+
+comm::serial::SerialSender::SerialSender(HardwareSerial &port,
+                                         UBaseType_t queueCapacity,
+                                         UBaseType_t priority,
+                                         BaseType_t coreID)
+    : port{port}, messages{queueCapacity}, SerialSenderTask{
+                                               exec,       "send serial data",
+                                               stackDepth, this,
+                                               priority,   coreID} {}
+
+void comm::serial::SerialSender::exec(void *senderPtr) {
+  SerialSender *sender = static_cast<SerialSender *>(senderPtr);
+  freertos::sleep(20); // wait until Serial port has been initialized
+  while (true) {
+    if (!sender->port.availableForWrite()) {
+      continue;
+    }
+    auto message = sender->messages.pop();
+    if (message.has_value()) {
+      sender->port.write(message.value().data(), message.value().size());
+    }
+  }
+}

modules/control/Firmware/src/Communication/Serial/SerialSender.h

@@ -0,0 +1,27 @@
+#pragma once
+#include <Arduino.h>
+#include <FreeRTOS/Queue.h>
+#include <FreeRTOS/Task.h>
+#include <FreeRTOS/Util.h>
+#include <Messages/Message.h>
+#include <Util/CircularBuffer.h>
+#include <etl/span.h>
+#include <etl/string.h>
+
+namespace comm {
+namespace serial {
+class SerialSender {
+public:
+  SerialSender(HardwareSerial &port, UBaseType_t queueCapacity,
+               UBaseType_t priority, BaseType_t coreID);
+  freertos::Queue<etl::span<uint8_t>> &getMessageQueue() { return messages; }
+
+private:
+  static void exec(void *senderPtr);
+  HardwareSerial &port;
+  freertos::Queue<etl::span<uint8_t>> messages;
+  freertos::Task SerialSenderTask;
+  static const uint32_t stackDepth = 2048;
+};
+} // namespace serial
+} // namespace comm

modules/control/Firmware/src/Messages/Message.h

@@ -0,0 +1,12 @@
+#pragma once
+#include <Arduino.h>
+#include <FreeRTOS/Mutex.h>
+#include <Messages/Protocol.h>
+#include <etl/utility.h>
+#include <etl/vector.h>
+
+namespace message {
+using Message =
+    etl::pair<etl::vector<uint8_t, message::protocol::MAX_PAYLOAD_LEN>,
+              freertos::Mutex>;
+} // namespace message