Create FreeRTOS C++ wrappers

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

@@ -0,0 +1,44 @@
+#pragma once
+#include <Arduino.h>
+#include <etl/optional.h>
+namespace freertos {
+template <typename T> class Queue {
+public:
+  Queue(UBaseType_t capacity) : queueCapacity{capacity} {
+    handle = xQueueCreate(capacity, sizeof(T));
+  }
+  Queue(const Queue &other) = delete;
+  Queue(Queue &&other)
+      : handle{etl::move(other.handle)}, queueCapacity{
+                                             etl::move(other.queueCapacity)} {}
+  ~Queue() { vQueueDelete(handle); }
+  Queue &operator=(const Queue &other) = delete;
+  Queue &operator=(const Queue &&other) {
+    handle = etl::move(other.handle);
+    queueCapacity = etl::move(other.queueCapacity);
+  }
+
+  etl::optional<T> peek(const TickType_t ticksToWait = portMAX_DELAY) {
+    T buf;
+    BaseType_t status = xQueuePeek(handle, &buf, ticksToWait);
+    return status == pdTRUE ? etl::make_optional<T>(buf) : etl::nullopt;
+  }
+  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) {
+    return xQueueSend(handle, &item, ticksToWait) == pdTRUE;
+  }
+  etl::optional<T> pop(const TickType_t ticksToWait = portMAX_DELAY) {
+    T buf;
+    BaseType_t status = xQueueReceive(handle, &buf, ticksToWait);
+    return status == pdTRUE ? etl::make_optional<T>(buf) : etl::nullopt;
+  }
+  void clear() { xQueueReset(handle); }
+  bool isValid() { return handle != NULL; }
+
+private:
+  QueueHandle_t handle;
+  UBaseType_t queueCapacity;
+};
+} // namespace freertos

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

@@ -0,0 +1,43 @@
+#include "Task.h"
+
+freertos::Task::Task(TaskFunction_t taskFunction, const etl::string_view name,
+                     const uint32_t stackDepth, void *const params,
+                     UBaseType_t priority, const BaseType_t coreID)
+    : name{name}, stackDepth{stackDepth}, priority{priority}, coreID{coreID} {
+  status = xTaskCreatePinnedToCore(taskFunction, name.data(), stackDepth,
+                                   params, priority, &handle, coreID);
+}
+
+freertos::Task::Task(Task &&other)
+    : name{etl::move(other.name)}, handle{etl::move(other.handle)},
+      stackDepth{etl::move(other.stackDepth)},
+      priority{etl::move(other.priority)}, coreID{etl::move(other.coreID)} {}
+
+freertos::Task::~Task() {
+  if (status == pdPASS) {
+    vTaskDelete(handle);
+  }
+}
+
+auto freertos::Task::operator=(const Task &&other) -> Task & {
+  name = etl::move(other.name);
+  handle = etl::move(other.handle);
+  stackDepth = etl::move(other.stackDepth);
+  priority = etl::move(other.priority);
+  coreID = etl::move(other.coreID);
+  return *this;
+}
+
+auto freertos::Task::getName() -> etl::string_view { return name; }
+
+auto freertos::Task::getStackDepth() -> uint32_t { return stackDepth; }
+
+auto freertos::Task::getPriority() -> UBaseType_t { return priority; }
+
+auto freertos::Task::setPriority(const UBaseType_t newPriority) -> void {
+  vTaskPrioritySet(handle, newPriority);
+}
+
+auto freertos::Task::getCoreID() -> BaseType_t { return coreID; }
+
+auto freertos::Task::isValid() -> bool { return status == pdPASS; }

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

@@ -0,0 +1,32 @@
+#pragma once
+#include <Arduino.h>
+#include <etl/string.h>
+
+namespace freertos {
+class Task {
+public:
+  Task(TaskFunction_t taskFunction, const etl::string_view name,
+       const uint32_t stackDepth, void *const params, UBaseType_t priority,
+       const BaseType_t coreID);
+  Task(const Task &other) = delete;
+  Task(Task &&other);
+  ~Task();
+  Task &operator=(const Task &other) = delete;
+  Task &operator=(const Task &&other);
+
+  etl::string_view getName();
+  uint32_t getStackDepth();
+  UBaseType_t getPriority();
+  void setPriority(const UBaseType_t newPriority);
+  BaseType_t getCoreID();
+  bool isValid();
+
+private:
+  etl::string_view name;
+  TaskHandle_t handle;
+  uint32_t stackDepth;
+  UBaseType_t priority;
+  BaseType_t coreID;
+  BaseType_t status;
+};
+} // namespace freertos