Implement serial interaction

xQueueSend() copies the value, so accepting a reference is misleading

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/FreeRTOS/Mutex.cpp

@@ -0,0 +1,20 @@
+#include "Mutex.h"
+#include <etl/utility.h>
+
+freertos::Mutex::Mutex() : handle{xSemaphoreCreateMutex()} {}
+freertos::Mutex::Mutex(Mutex &&other) noexcept {
+  handle = etl::move(other.handle);
+}
+freertos::Mutex::~Mutex() { vSemaphoreDelete(handle); }
+auto freertos::Mutex::operator=(const Mutex &&other) noexcept -> Mutex & {
+  handle = etl::move(other.handle);
+  return *this;
+}
+auto freertos::Mutex::lock(TickType_t timeout) -> bool{
+  BaseType_t success = xSemaphoreTake(handle, timeout);
+  return success == pdTRUE ? true : false;
+}
+auto freertos::Mutex::unlock() -> bool {
+  BaseType_t success = xSemaphoreGive(handle);
+  return success == pdTRUE ? true : false;
+}

modules/control/Firmware/src/FreeRTOS/Mutex.h

@@ -0,0 +1,19 @@
+#pragma once
+#include <Arduino.h>
+
+namespace freertos {
+class Mutex {
+public:
+  Mutex();
+  Mutex(const Mutex &other) = delete;
+  Mutex(Mutex &&other) noexcept;
+  ~Mutex();
+  Mutex &operator=(const Mutex &other) = delete;
+  Mutex &operator=(const Mutex &&other) noexcept;
+  bool lock(TickType_t timeout = portMAX_DELAY);
+  bool unlock();
+
+private:
+  SemaphoreHandle_t handle;
+};
+} // namespace freertos

modules/control/Firmware/src/FreeRTOS/Queue.h

@@ -26,7 +26,7 @@ public:
   size_t size() { return uxQueueMessagesWaiting(handle); }
   size_t available() { return uxQueueSpacesAvailable(handle); }
   size_t capacity() { return queueCapacity; }
-  bool push(const T &item, const TickType_t ticksToWait = portMAX_DELAY) {
+  bool push(const T item, const TickType_t ticksToWait = portMAX_DELAY) {
     return xQueueSend(handle, &item, ticksToWait) == pdTRUE;
   }
   etl::optional<T> pop(const TickType_t ticksToWait = portMAX_DELAY) {

modules/control/Firmware/src/FreeRTOS/Util.cpp

@@ -0,0 +1,3 @@
+#include "Util.h"
+void freertos::sleep(int time) { vTaskDelay(time * portTICK_PERIOD_MS); }
+void freertos::sleepForever() { vTaskDelay(portMAX_DELAY); }

modules/control/Firmware/src/FreeRTOS/Util.h

@@ -4,5 +4,8 @@
 namespace freertos {
 /// @brief Blocks the task for the specified amount of time
 /// @param time in milliseconds
-void sleep(int time) { vTaskDelay(time * portTICK_PERIOD_MS); }
+void sleep(int time);
+
+/// @brief Sleep for the maximum delay possible
+void sleepForever();
 } // namespace freertos

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

modules/control/Firmware/src/Util/CircularBuffer.h

@@ -0,0 +1,21 @@
+#pragma once
+#include <Arduino.h>
+#include <etl/array.h>
+
+namespace util {
+template <typename T, size_t size> class CircularBuffer {
+public:
+  T &peek() { return buffer[index]; }
+
+  /// @brief Warning: Does not deconstruct entry! If necessary use
+  /// etl::circular_buffer instead!
+  T &pop() {
+    return buffer[index];
+    index = (index + 1) % buffer.size();
+  }
+
+private:
+  etl::array<T, size> buffer;
+  size_t index;
+};
+} // namespace util