Define composable command interface

Switch from mutex to bin-semaphore (thread overarching) & Proper failure handling

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

@@ -16,7 +16,12 @@ public:
                  UBaseType_t priority, BaseType_t coreID)
       : port{port}, messageQueue{messageQueue},
         serialReceiverTask{
-            exec, "receive serial data", stackDepth, this, priority, coreID} {}
+            exec, "receive serial data", stackDepth, this, priority, coreID} {
+    for (size_t i = 0; i < bufferSize; ++i) {
+      message::Message &msg = buffer.pop();
+      msg.second.give();
+    }
+  }
 
 private:
   static void exec(void *receiverPtr) {
@@ -28,22 +33,32 @@ private:
       receiver->magicNumberBuf.clear();
       receiver->magicNumberBuf.resize(receiver->magicNumber.length());
       size_t availableBytes = receiver->port.available();
-      if (availableBytes >= receiver->magicNumber.length() + 1) {
+      if (receiver->port.available() > 0 && receiver->port.peek() != 'H') {
+        receiver->port.read();
+        receiver->port.println("flush");
+      } else if (availableBytes >= receiver->magicNumber.length() + 1) {
         receiver->port.readBytes(receiver->magicNumberBuf.data(),
                                  receiver->magicNumber.length());
-        receiver->port.println(receiver->magicNumberBuf ==
-                               receiver->magicNumber);
+        receiver->port.println("Received Magic Number:");
+        receiver->port.println(receiver->magicNumberBuf == receiver->magicNumber
+                                   ? "correct"
+                                   : "incorrect");
 
-        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);
+        if (receiver->magicNumberBuf == receiver->magicNumber) {
+          uint8_t size;
+          receiver->port.readBytes(&size, sizeof(size));
+          if (receiver->port.available() >= size) {
+            message::Message &msg = receiver->buffer.pop();
+            receiver->port.printf("Current semaphore count: %u\n",
+                                  msg.second.getCount());
+            msg.second.take();
+            msg.first.clear();
+            msg.first.resize(size);
+            receiver->port.readBytes(msg.first.data(), size);
+            // msg.second.unlock();
+            receiver->port.println("Push message to queue");
+            receiver->messageQueue.push(&msg);
+          }
         }
       }
     }

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

@@ -0,0 +1,26 @@
+#include <FreeRTOS/BinarySemaphore.h>
+#include <etl/utility.h>
+
+freertos::BinarySemaphore::BinarySemaphore()
+    : handle{xSemaphoreCreateBinary()} {}
+freertos::BinarySemaphore::BinarySemaphore(BinarySemaphore &&other) noexcept {
+  handle = etl::move(other.handle);
+}
+freertos::BinarySemaphore::~BinarySemaphore() { vSemaphoreDelete(handle); }
+auto freertos::BinarySemaphore::operator=(
+    const BinarySemaphore &&other) noexcept -> BinarySemaphore & {
+  handle = etl::move(other.handle);
+  return *this;
+}
+auto freertos::BinarySemaphore::take(TickType_t timeout) -> bool {
+  BaseType_t success = xSemaphoreTake(handle, timeout);
+  return success == pdTRUE;
+}
+auto freertos::BinarySemaphore::give() -> bool {
+  BaseType_t success = xSemaphoreGive(handle);
+  return success == pdTRUE;
+}
+
+auto freertos::BinarySemaphore::getCount() -> size_t {
+  return uxSemaphoreGetCount(handle);
+}

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

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

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

@@ -12,9 +12,9 @@ auto freertos::Mutex::operator=(const Mutex &&other) noexcept -> Mutex & {
 }
 auto freertos::Mutex::lock(TickType_t timeout) -> bool{
   BaseType_t success = xSemaphoreTake(handle, timeout);
-  return success == pdTRUE ? true : false;
+  return success == pdTRUE;
 }
 auto freertos::Mutex::unlock() -> bool {
   BaseType_t success = xSemaphoreGive(handle);
-  return success == pdTRUE ? true : false;
+  return success == pdTRUE;
 }

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

@@ -1,9 +1,21 @@
 #pragma once
 #include "Protocol.h"
+#include <etl/optional.h>
+#include <etl/span.h>
 #include <etl/string.h>
 
 namespace message {
 namespace composer {
+enum class ComposableType {
+  MESSAGE_LIGHTDATA,
+  MESSAGE_INFO,
+  MESSAGE_WARNING,
+  MESSAGE_ERROR,
+  MESSAGE_SUCCESS,
+  COMMAND_HELP,
+  COMMAND_VERSION,
+};
+using Composable = etl::pair<ComposableType, etl::optional<etl::span<uint8_t>>>;
 bool sendMessage(protocol::Message type, etl::string_view content);
 } // namespace composer
 } // namespace message

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

@@ -1,12 +1,13 @@
 #pragma once
 #include <Arduino.h>
-#include <FreeRTOS/Mutex.h>
+#include <FreeRTOS/BinarySemaphore.h>
 #include <Messages/Protocol.h>
 #include <etl/utility.h>
 #include <etl/vector.h>
 
 namespace message {
+/// @brief first: data, second: data ready to reuse
 using Message =
     etl::pair<etl::vector<uint8_t, message::protocol::MAX_PAYLOAD_LEN>,
-              freertos::Mutex>;
+              freertos::BinarySemaphore>;
 } // namespace message

modules/control/Firmware/src/Messages/Parser.cpp

@@ -1,58 +1,23 @@
 #include "Parser.h"
+#include <FreeRTOS/Util.h>
+#include <PinMap/PinMap.h>
 #include <etl/vector.h>
 
 message::parser::StateVisitor::StateVisitor(
-    etl::byte_stream_reader &&sreader,
-    etl::function<void, etl::string_view> invalidCallback,
-    etl::function<void, etl::span<const char>> messageLightDataCallback,
-    etl::function<void, etl::span<const char>> messageInfoCallback,
-    etl::function<void, etl::span<const char>> messageWarningCallback,
-    etl::function<void, etl::span<const char>> messageErrorCallback,
-    etl::function<void, etl::span<const char>> messageSuccessCallback,
-    etl::function<void, uint32_t> settingsSetBaudCallback,
-    etl::function<void, etl::span<const char>> settingsWifiPasswordCallback,
-    etl::function<void, etl::span<const char>> settingsWifiSSIDCallback,
-    etl::function<void, void> lightControlOnCallback,
-    etl::function<void, void> lightControlOffCallback,
-    etl::function<void, uint8_t> lightControlToggleCallback,
-    etl::function<void, uint8_t> lightControlIncreaseCallback,
-    etl::function<void, uint8_t> lightControlDecreaseCallback,
-    etl::function<void, void> commandRequestLightDataCallback,
-    etl::function<void, void> commandEnterConsoleFlashingCallback,
-    etl::function<void, void> commandExitConsoleFlashingCallback,
-    etl::function<void, void> commandPairBluetoothCallback,
-    etl::function<void, void> commandHelpCallback,
-    etl::function<void, void> commandVersionCallback)
-    : stream{etl::move(sreader)}, invalidCallback{invalidCallback},
-      messageLightDataCallback{messageLightDataCallback},
-      messageInfoCallback{messageInfoCallback},
-      messageWarningCallback{messageWarningCallback},
-      messageErrorCallback{messageErrorCallback},
-      messageSuccessCallback{messageSuccessCallback},
-      settingsSetBaudCallback{settingsSetBaudCallback},
-      settingsWifiPasswordCallback{settingsWifiPasswordCallback},
-      settingsWifiSSIDCallback{settingsWifiSSIDCallback},
-      lightControlOnCallback{lightControlOnCallback},
-      lightControlOffCallback{lightControlOffCallback},
-      lightControlToggleCallback{lightControlToggleCallback},
-      lightControlIncreaseCallback{lightControlIncreaseCallback},
-      lightControlDecreaseCallback{lightControlDecreaseCallback},
-      commandRequestLightDataCallback{commandRequestLightDataCallback},
-      commandEnterConsoleFlashingCallback{commandEnterConsoleFlashingCallback},
-      commandExitConsoleFlashingCallback{commandExitConsoleFlashingCallback},
-      commandPairBluetoothCallback{commandPairBluetoothCallback},
-      commandHelpCallback{commandHelpCallback}, commandVersionCallback{
-                                                    commandVersionCallback} {}
+    message::Message *message,
+    freertos::Queue<LightController::LightAction> &lightActionQueue)
+    : stream{message->first.begin(), message->first.size(), etl::endian::big},
+      message{message}, lightActionQueue{lightActionQueue} {}
 
 auto message::parser::StateVisitor::operator()(state::ModeSelection) -> State {
   if (!stream.available<uint8_t>()) {
-    invalidCallback("ModeSelection: Stream not available");
+    // invalidCallback("ModeSelection: Stream not available");
     return state::Invalid{};
   }
 
   auto mode = stream.read<uint8_t>();
   if (!mode.has_value()) {
-    invalidCallback("ModeSelection: Mode has no value");
+    // invalidCallback("ModeSelection: Mode has no value");
     return state::Invalid{};
   }
 
@@ -66,20 +31,20 @@ auto message::parser::StateVisitor::operator()(state::ModeSelection) -> State {
   case static_cast<uint8_t>(protocol::Mode::Command):
     return state::Command{};
   default:
-    invalidCallback("ModeSelection: Invalid index");
+    // invalidCallback("ModeSelection: Invalid index");
     return state::Invalid{};
   }
 }
 
 auto message::parser::StateVisitor::operator()(state::Message) -> State {
   if (!stream.available<uint8_t>()) {
-    invalidCallback("Message: Stream not available");
+    // invalidCallback("Message: Stream not available");
     return state::Invalid{};
   }
 
   auto mode = stream.read<uint8_t>();
   if (!mode.has_value()) {
-    invalidCallback("Message: Mode has no value");
+    // invalidCallback("Message: Mode has no value");
     return state::Invalid{};
   }
 
@@ -95,7 +60,7 @@ auto message::parser::StateVisitor::operator()(state::Message) -> State {
   case static_cast<uint8_t>(protocol::Message::Success):
     return state::MessageSuccess{};
   default:
-    invalidCallback("Message: Invalid index");
+    // invalidCallback("Message: Invalid index");
     return state::Invalid{};
   }
 }
@@ -103,84 +68,84 @@ auto message::parser::StateVisitor::operator()(state::Message) -> State {
 auto message::parser::StateVisitor::operator()(state::MessageLightData)
     -> State {
   if (!stream.available<char>()) {
-    invalidCallback("MessageLightData: Stream not available");
+    // invalidCallback("MessageLightData: Stream not available");
     return state::Invalid{};
   }
 
   auto msg = stream.read<char>(stream.available<char>());
   if (!msg.has_value()) {
-    invalidCallback("MessageLightData: Message has no value");
+    // invalidCallback("MessageLightData: Message has no value");
     return state::Invalid{};
   }
-  messageLightDataCallback(msg.value());
-  return state::Complete{};
+
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::MessageInfo) -> State {
   if (!stream.available<char>()) {
-    invalidCallback("MessageInfo: Stream not available");
+    // invalidCallback("MessageInfo: Stream not available");
     return state::Invalid{};
   }
 
   auto msg = stream.read<char>(stream.available<char>());
   if (!msg.has_value()) {
-    invalidCallback("MessageInfo: Message has no value");
+    // invalidCallback("MessageInfo: Message has no value");
     return state::Invalid{};
   }
-  messageInfoCallback(msg.value());
-  return state::Complete{};
+
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::MessageWarning) -> State {
   if (!stream.available<char>()) {
-    invalidCallback("MessageWarning: Stream not available");
+    // invalidCallback("MessageWarning: Stream not available");
     return state::Invalid{};
   }
 
   auto msg = stream.read<char>(stream.available<char>());
   if (!msg.has_value()) {
-    invalidCallback("MessageWarning: Message has no value");
+    // invalidCallback("MessageWarning: Message has no value");
     return state::Invalid{};
   }
-  messageWarningCallback(msg.value());
-  return state::Complete{};
+
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::MessageError) -> State {
   if (!stream.available<char>()) {
-    invalidCallback("MessageError: Stream not available");
+    // invalidCallback("MessageError: Stream not available");
     return state::Invalid{};
   }
 
   auto msg = stream.read<char>(stream.available<char>());
   if (!msg.has_value()) {
-    invalidCallback("MessageError: Message has no value");
+    // invalidCallback("MessageError: Message has no value");
     return state::Invalid{};
   }
-  messageErrorCallback(msg.value());
-  return state::Complete{};
+
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::MessageSuccess) -> State {
   if (!stream.available<char>()) {
-    invalidCallback("MessageSuccess: Stream not available");
+    // invalidCallback("MessageSuccess: Stream not available");
     return state::Invalid{};
   }
 
   auto msg = stream.read<char>(stream.available<char>());
   if (!msg.has_value()) {
-    invalidCallback("MessageSuccess: Message has no value");
+    // invalidCallback("MessageSuccess: Message has no value");
     return state::Invalid{};
   }
-  messageSuccessCallback(msg.value());
-  return state::Complete{};
+
+  return state::Invalid{};
 }
 
 auto message::parser::StateVisitor::operator()(state::Settings) -> State {
   if (!stream.available<uint8_t>()) {
-    invalidCallback("Settings: Stream not available");
+    // invalidCallback("Settings: Stream not available");
     return state::Invalid{};
   }
 
   auto mode = stream.read<uint8_t>();
   if (!mode.has_value()) {
-    invalidCallback("Settings: Mode has no value");
+    // invalidCallback("Settings: Mode has no value");
     return state::Invalid{};
   }
 
@@ -192,69 +157,66 @@ auto message::parser::StateVisitor::operator()(state::Settings) -> State {
   case static_cast<uint8_t>(protocol::Settings::SetWifiSSID):
     return state::SettingsWifiSSID{};
   default:
-    invalidCallback("Settings: Invalid index");
+    // invalidCallback("Settings: Invalid index");
     return state::Invalid{};
   }
 }
 auto message::parser::StateVisitor::operator()(state::SettingsSetBaud)
     -> State {
   if (!stream.available<uint32_t>()) {
-    invalidCallback("Settings: Stream not available");
+    // invalidCallback("Settings: Stream not available");
     return state::Invalid{};
   }
 
   auto baud = stream.read<uint32_t>();
 
   if (!baud.has_value()) {
-    invalidCallback("SettingsSetBaud: Baud has no value");
+    // invalidCallback("SettingsSetBaud: Baud has no value");
     return state::Invalid{};
   }
 
-  settingsSetBaudCallback(baud.value());
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::SettingsWifiPassword)
     -> State {
   if (!stream.available<char>()) {
-    invalidCallback("SettingsWifiPassword: Stream not available");
+    // invalidCallback("SettingsWifiPassword: Stream not available");
     return state::Invalid{};
   }
 
   auto passwd = stream.read<char>(stream.available<char>());
   if (!passwd.has_value()) {
-    invalidCallback("SettingsWifiPassword: Passwd has no value");
+    // invalidCallback("SettingsWifiPassword: Passwd has no value");
     return state::Invalid{};
   }
 
-  settingsWifiPasswordCallback(passwd.value());
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::SettingsWifiSSID)
     -> State {
   if (!stream.available<char>()) {
-    invalidCallback("SettingsWifiSSID: Stream not available");
+    // invalidCallback("SettingsWifiSSID: Stream not available");
     return state::Invalid{};
   }
 
   auto ssid = stream.read<char>(stream.available<char>());
   if (!ssid.has_value()) {
-    invalidCallback("SettingsWifiSSID: SSID has no value");
+    // invalidCallback("SettingsWifiSSID: SSID has no value");
     return state::Invalid{};
   }
 
-  settingsWifiSSIDCallback(ssid.value());
-  return state::Complete{};
+  return state::Invalid{};
 }
 
 auto message::parser::StateVisitor::operator()(state::LightControl) -> State {
   if (!stream.available<uint8_t>()) {
-    invalidCallback("LightControl: Stream not available");
+    // invalidCallback("LightControl: Stream not available");
     return state::Invalid{};
   }
 
   auto mode = stream.read<uint8_t>();
   if (!mode.has_value()) {
-    invalidCallback("LightControl: Mode has no value");
+    // invalidCallback("LightControl: Mode has no value");
     return state::Invalid{};
   }
 
@@ -270,83 +232,92 @@ auto message::parser::StateVisitor::operator()(state::LightControl) -> State {
   case static_cast<uint8_t>(protocol::LightControl::Decrease):
     return state::LightControlDecrease{};
   default:
-    invalidCallback("LightControl: Invalid index");
+    // invalidCallback("LightControl: Invalid index");
     return state::Invalid{};
   }
 }
 
 auto message::parser::StateVisitor::operator()(state::LightControlOn) -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("LightControlOn: Too many bytes left");
+    // invalidCallback("LightControlOn: Too many bytes left");
     return state::Invalid{};
   }
-  lightControlOnCallback();
+
+  message->second.give();
+  Serial.println("ON");
+  lightActionQueue.push({LightController::LightActionType::ON, etl::nullopt});
   return state::Complete{};
 }
 auto message::parser::StateVisitor::operator()(state::LightControlOff)
     -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("LightControlOff: Too many bytes left");
+    // invalidCallback("LightControlOff: Too many bytes left");
     return state::Invalid{};
   }
-  lightControlOffCallback();
+
+  message->second.give();
+  Serial.println("OFF");
+  lightActionQueue.push({LightController::LightActionType::OFF, etl::nullopt});
   return state::Complete{};
 }
 auto message::parser::StateVisitor::operator()(state::LightControlToggle)
     -> State {
   if (stream.available_bytes() > 1) {
-    invalidCallback("LightControlToggle: Too many bytes left");
+    // invalidCallback("LightControlToggle: Too many bytes left");
     return state::Invalid{};
   } else if (!stream.available<uint8_t>()) {
-    invalidCallback("LightControlToggle: Stream not available");
+    // invalidCallback("LightControlToggle: Stream not available");
     return state::Invalid{};
   }
 
   auto id = stream.read<uint8_t>();
-
-  lightControlToggleCallback(id.value());
+  message->second.give();
+  Serial.println("Toggle");
+  lightActionQueue.push({LightController::LightActionType::TOGGLE, id});
   return state::Complete{};
 }
 auto message::parser::StateVisitor::operator()(state::LightControlIncrease)
     -> State {
   if (stream.available_bytes() > 1) {
-    invalidCallback("LightControlIncrease: Too many bytes left");
+    // invalidCallback("LightControlIncrease: Too many bytes left");
     return state::Invalid{};
   } else if (!stream.available<uint8_t>()) {
-    invalidCallback("LightControlIncrease: Stream not available");
+    // invalidCallback("LightControlIncrease: Stream not available");
     return state::Invalid{};
   }
 
   auto id = stream.read<uint8_t>();
-
-  lightControlIncreaseCallback(id.value());
+  message->second.give();
+  Serial.println("Increase");
+  lightActionQueue.push({LightController::LightActionType::INCREASE, id});
   return state::Complete{};
 }
 auto message::parser::StateVisitor::operator()(state::LightControlDecrease)
     -> State {
   if (stream.available_bytes() > 1) {
-    invalidCallback("LightControlDecrease: Too many bytes left");
+    // invalidCallback("LightControlDecrease: Too many bytes left");
     return state::Invalid{};
   } else if (!stream.available<uint8_t>()) {
-    invalidCallback("LightControlDecrease: Stream not available");
+    // invalidCallback("LightControlDecrease: Stream not available");
     return state::Invalid{};
   }
 
   auto id = stream.read<uint8_t>();
-
-  lightControlDecreaseCallback(id.value());
+  message->second.give();
+  Serial.println("Decrease");
+  lightActionQueue.push({LightController::LightActionType::DECREASE, id});
   return state::Complete{};
 }
 
 auto message::parser::StateVisitor::operator()(state::Command) -> State {
   if (!stream.available<uint8_t>()) {
-    invalidCallback("Command: Stream not available");
+    // invalidCallback("Command: Stream not available");
     return state::Invalid{};
   }
 
   auto mode = stream.read<uint8_t>();
   if (!mode.has_value()) {
-    invalidCallback("Command: Mode has no value");
+    // invalidCallback("Command: Mode has no value");
     return state::Invalid{};
   }
 
@@ -363,59 +334,62 @@ auto message::parser::StateVisitor::operator()(state::Command) -> State {
     return state::CommandHelp{};
   case static_cast<uint8_t>(protocol::Command::Version):
     return state::CommandVersion{};
+  case static_cast<uint8_t>(protocol::Command::Reset):
+    return state::CommandReset{};
   default:
-    invalidCallback("Command: Invalid index");
+    // invalidCallback("Command: Invalid index");
     return state::Invalid{};
   }
 }
 auto message::parser::StateVisitor::operator()(state::CommandRequestLightData)
     -> State {
-  commandRequestLightDataCallback();
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(
     state::CommandEnterConsoleFlashing) -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("CommandEnterConsoleFlashing: Too many bytes left");
+    // invalidCallback("CommandEnterConsoleFlashing: Too many bytes left");
     return state::Invalid{};
   }
-  commandEnterConsoleFlashingCallback();
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(
     state::CommandExitConsoleFlashing) -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("CommandExitConsoleFlashing: Too many bytes left");
+    // invalidCallback("CommandExitConsoleFlashing: Too many bytes left");
     return state::Invalid{};
   }
-  commandExitConsoleFlashingCallback();
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::CommandPairBluetooth)
     -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("CommandPairBluetooth: Too many bytes left");
+    // invalidCallback("CommandPairBluetooth: Too many bytes left");
     return state::Invalid{};
   }
-  commandPairBluetoothCallback();
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::CommandHelp) -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("CommandHelp: Too many bytes left");
+    // invalidCallback("CommandHelp: Too many bytes left");
     return state::Invalid{};
   }
-
-  commandHelpCallback();
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::CommandVersion) -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("CommandVersion: Too many bytes left");
+    // invalidCallback("CommandVersion: Too many bytes left");
     return state::Invalid{};
   }
+  return state::Invalid{};
+}
 
-  commandVersionCallback();
+auto message::parser::StateVisitor::operator()(state::CommandReset) -> State {
+  if (stream.available_bytes() > 0) {
+    // invalidCallback("CommandVersion: Too many bytes left");
+    return state::Invalid{};
+  }
+  ESP.restart();
   return state::Complete{};
 }
 
@@ -423,55 +397,36 @@ auto message::parser::StateVisitor::operator()(state::Complete) -> State {
   return state::Complete{};
 }
 auto message::parser::StateVisitor::operator()(state::Invalid) -> State {
+  message->second.give();
   return state::Invalid{};
 }
 
 auto message::parser::parse(
-    etl::byte_stream_reader &&reader,
-    etl::function<void, etl::string_view> invalidCallback,
-    etl::function<void, etl::span<const char>> messageLightDataCallback,
-    etl::function<void, etl::span<const char>> messageInfoCallback,
-    etl::function<void, etl::span<const char>> messageWarningCallback,
-    etl::function<void, etl::span<const char>> messageErrorCallback,
-    etl::function<void, etl::span<const char>> messageSuccessCallback,
-    etl::function<void, uint32_t> settingsSetBaudCallback,
-    etl::function<void, etl::span<const char>> settingsWifiPasswordCallback,
-    etl::function<void, etl::span<const char>> settingsWifiSSIDCallback,
-    etl::function<void, void> lightControlOnCallback,
-    etl::function<void, void> lightControlOffCallback,
-    etl::function<void, uint8_t> lightControlToggleCallback,
-    etl::function<void, uint8_t> lightControlIncreaseCallback,
-    etl::function<void, uint8_t> lightControlDecreaseCallback,
-    etl::function<void, void> commandRequestLightDataCallback,
-    etl::function<void, void> commandEnterConsoleFlashingCallback,
-    etl::function<void, void> commandExitConsoleFlashingCallback,
-    etl::function<void, void> commandPairBluetoothCallback,
-    etl::function<void, void> commandHelpCallback,
-    etl::function<void, void> commandVersionCallback) -> void {
-  StateVisitor visitor{etl::move(reader),
-                       invalidCallback,
-                       messageLightDataCallback,
-                       messageInfoCallback,
-                       messageWarningCallback,
-                       messageErrorCallback,
-                       messageSuccessCallback,
-                       settingsSetBaudCallback,
-                       settingsWifiPasswordCallback,
-                       settingsWifiSSIDCallback,
-                       lightControlOnCallback,
-                       lightControlOffCallback,
-                       lightControlToggleCallback,
-                       lightControlIncreaseCallback,
-                       lightControlDecreaseCallback,
-                       commandRequestLightDataCallback,
-                       commandEnterConsoleFlashingCallback,
-                       commandExitConsoleFlashingCallback,
-                       commandPairBluetoothCallback,
-                       commandHelpCallback,
-                       commandVersionCallback};
+    message::Message *message,
+    freertos::Queue<LightController::LightAction> &lightActionQueue) -> void {
+  StateVisitor visitor{message, lightActionQueue};
   State state{state::ModeSelection{}};
   while (!etl::holds_alternative<state::Invalid>(state) &&
          !etl::holds_alternative<state::Complete>(state)) {
     state = etl::visit(visitor, state);
   }
 }
+
+message::parser::Parser::Parser(
+    freertos::Queue<LightController::LightAction> &lightActionQueue,
+    UBaseType_t queueCapacity, UBaseType_t priority, BaseType_t coreID)
+    : parserTask{exec,  "parse received data", stackDepth, this, priority,
+                 coreID},
+      messages{queueCapacity}, lightActionQueue{lightActionQueue} {}
+
+void message::parser::Parser::exec(void *parserPtr) {
+  Parser *parser = static_cast<Parser *>(parserPtr);
+
+  while (true) {
+    auto message = parser->messages.pop();
+    Serial.println("starting parse");
+    if (message.has_value()) {
+      parse(message.value(), parser->lightActionQueue);
+    }
+  }
+}

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

@@ -1,5 +1,9 @@
 #pragma once
 #include "Protocol.h"
+#include <FreeRTOS/Queue.h>
+#include <FreeRTOS/Task.h>
+#include <LightController/Controller.h>
+#include <Messages/Message.h>
 #include <etl/byte_stream.h>
 #include <etl/function.h>
 #include <etl/span.h>
@@ -34,6 +38,7 @@ struct CommandExitConsoleFlashing {};
 struct CommandPairBluetooth {};
 struct CommandHelp {};
 struct CommandVersion {};
+struct CommandReset {};
 struct Complete {};
 } // namespace state
 
@@ -47,55 +52,16 @@ using State = etl::variant<
     state::LightControlIncrease, state::LightControlDecrease, state::Command,
     state::CommandRequestLightData, state::CommandEnterConsoleFlashing,
     state::CommandExitConsoleFlashing, state::CommandPairBluetooth,
-    state::CommandHelp, state::CommandVersion, state::Complete>;
+    state::CommandHelp, state::CommandVersion, state::CommandReset,
+    state::Complete>;
 
-void parse(
-    etl::byte_stream_reader &&reader,
-    etl::function<void, etl::string_view> invalidCallback,
-    etl::function<void, etl::span<const char>> messageLightDataCallback,
-    etl::function<void, etl::span<const char>> messageInfoCallback,
-    etl::function<void, etl::span<const char>> messageWarningCallback,
-    etl::function<void, etl::span<const char>> messageErrorCallback,
-    etl::function<void, etl::span<const char>> messageSuccessCallback,
-    etl::function<void, uint32_t> settingsSetBaudCallback,
-    etl::function<void, etl::span<const char>> settingsWifiPasswordCallback,
-    etl::function<void, etl::span<const char>> settingsWifiSSIDCallback,
-    etl::function<void, void> lightControlOnCallback,
-    etl::function<void, void> lightControlOffCallback,
-    etl::function<void, uint8_t> lightControlToggleCallback,
-    etl::function<void, uint8_t> lightControlIncreaseCallback,
-    etl::function<void, uint8_t> lightControlDecreaseCallback,
-    etl::function<void, void> commandRequestLightDataCallback,
-    etl::function<void, void> commandEnterConsoleFlashingCallback,
-    etl::function<void, void> commandExitConsoleFlashingCallback,
-    etl::function<void, void> commandPairBluetoothCallback,
-    etl::function<void, void> commandHelpCallback,
-    etl::function<void, void> commandVersionCallback);
+void parse(message::Message *message,
+           freertos::Queue<LightController::LightAction> &lightActionQueue);
 
 class StateVisitor {
 public:
-  StateVisitor(
-      etl::byte_stream_reader &&sreader,
-      etl::function<void, etl::string_view> invalidCallback,
-      etl::function<void, etl::span<const char>> messageLightDataCallback,
-      etl::function<void, etl::span<const char>> messageInfoCallback,
-      etl::function<void, etl::span<const char>> messageWarningCallback,
-      etl::function<void, etl::span<const char>> messageErrorCallback,
-      etl::function<void, etl::span<const char>> messageSuccessCallback,
-      etl::function<void, uint32_t> settingsSetBaudCallback,
-      etl::function<void, etl::span<const char>> settingsWifiPasswordCallback,
-      etl::function<void, etl::span<const char>> settingsWifiSSIDCallback,
-      etl::function<void, void> lightControlOnCallback,
-      etl::function<void, void> lightControlOffCallback,
-      etl::function<void, uint8_t> lightControlToggleCallback,
-      etl::function<void, uint8_t> lightControlIncreaseCallback,
-      etl::function<void, uint8_t> lightControlDecreaseCallback,
-      etl::function<void, void> commandRequestLightDataCallback,
-      etl::function<void, void> commandEnterConsoleFlashingCallback,
-      etl::function<void, void> commandExitConsoleFlashingCallback,
-      etl::function<void, void> commandPairBluetoothCallback,
-      etl::function<void, void> commandHelpCallback,
-      etl::function<void, void> commandVersionCallback);
+  StateVisitor(message::Message *message,
+               freertos::Queue<LightController::LightAction> &lightActionQueue);
   State operator()(state::ModeSelection);
   State operator()(state::Message);
   State operator()(state::MessageLightData);
@@ -122,33 +88,29 @@ public:
   State operator()(state::CommandPairBluetooth);
   State operator()(state::CommandHelp);
   State operator()(state::CommandVersion);
+  State operator()(state::CommandReset);
 
   State operator()(state::Complete);
   State operator()(state::Invalid);
 
 private:
   etl::byte_stream_reader stream;
-  // TODO: callback function parameter types
-  etl::function<void, etl::string_view> invalidCallback;
-  etl::function<void, etl::span<const char>> messageLightDataCallback;
-  etl::function<void, etl::span<const char>> messageInfoCallback;
-  etl::function<void, etl::span<const char>> messageWarningCallback;
-  etl::function<void, etl::span<const char>> messageErrorCallback;
-  etl::function<void, etl::span<const char>> messageSuccessCallback;
-  etl::function<void, uint32_t> settingsSetBaudCallback;
-  etl::function<void, etl::span<const char>> settingsWifiPasswordCallback;
-  etl::function<void, etl::span<const char>> settingsWifiSSIDCallback;
-  etl::function<void, void> lightControlOnCallback;
-  etl::function<void, void> lightControlOffCallback;
-  etl::function<void, uint8_t> lightControlToggleCallback;
-  etl::function<void, uint8_t> lightControlIncreaseCallback;
-  etl::function<void, uint8_t> lightControlDecreaseCallback;
-  etl::function<void, void> commandRequestLightDataCallback;
-  etl::function<void, void> commandEnterConsoleFlashingCallback;
-  etl::function<void, void> commandExitConsoleFlashingCallback;
-  etl::function<void, void> commandPairBluetoothCallback;
-  etl::function<void, void> commandHelpCallback;
-  etl::function<void, void> commandVersionCallback;
+  message::Message *message;
+  freertos::Queue<LightController::LightAction> &lightActionQueue;
+};
+
+class Parser {
+public:
+  Parser(freertos::Queue<LightController::LightAction> &lightActionQueue,
+         UBaseType_t queueCapacity, UBaseType_t priority, BaseType_t coreID);
+  freertos::Queue<message::Message *> &getMessageQueue() { return messages; }
+
+private:
+  static void exec(void *parserPtr);
+  freertos::Task parserTask;
+  static const uint32_t stackDepth = 2048;
+  freertos::Queue<message::Message *> messages;
+  freertos::Queue<LightController::LightAction> &lightActionQueue;
 };
 } // namespace parser
 } // namespace message

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

@@ -47,6 +47,7 @@ enum class Command : uint8_t {
   PairBluetooth = 0x3,
   Help = 0x4,
   Version = 0x5,
+  Reset = 0x6,
 };
 
 using Submode = etl::variant<Mode, Message, Settings, LightControl, Command>;

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

@@ -10,8 +10,8 @@ public:
   /// @brief Warning: Does not deconstruct entry! If necessary use
   /// etl::circular_buffer instead!
   T &pop() {
-    return buffer[index];
     index = (index + 1) % buffer.size();
+    return buffer[index];
   }
 
 private:

modules/debugger/debugger.py

@@ -0,0 +1,10 @@
+import serial
+import time
+ser = serial.Serial(port='/dev/ttyUSB0', baudrate=115200)
+ser.write(b'HX\x02\x02\x00')
+time.sleep(1)
+ser.write(b'HX\x02\x02\x01')
+print(f"Listening for messages on {ser.name}:")
+while True:
+    print(ser.readline())
+ser.close()

modules/debugger/poetry.lock

@@ -0,0 +1,48 @@
+# This file is automatically @generated by Poetry 1.4.0 and should not be changed by hand.
+
+[[package]]
+name = "autopep8"
+version = "2.0.2"
+description = "A tool that automatically formats Python code to conform to the PEP 8 style guide"
+category = "dev"
+optional = false
+python-versions = ">=3.6"
+files = [
+    {file = "autopep8-2.0.2-py2.py3-none-any.whl", hash = "sha256:86e9303b5e5c8160872b2f5ef611161b2893e9bfe8ccc7e2f76385947d57a2f1"},
+    {file = "autopep8-2.0.2.tar.gz", hash = "sha256:f9849cdd62108cb739dbcdbfb7fdcc9a30d1b63c4cc3e1c1f893b5360941b61c"},
+]
+
+[package.dependencies]
+pycodestyle = ">=2.10.0"
+
+[[package]]
+name = "pycodestyle"
+version = "2.10.0"
+description = "Python style guide checker"
+category = "dev"
+optional = false
+python-versions = ">=3.6"
+files = [
+    {file = "pycodestyle-2.10.0-py2.py3-none-any.whl", hash = "sha256:8a4eaf0d0495c7395bdab3589ac2db602797d76207242c17d470186815706610"},
+    {file = "pycodestyle-2.10.0.tar.gz", hash = "sha256:347187bdb476329d98f695c213d7295a846d1152ff4fe9bacb8a9590b8ee7053"},
+]
+
+[[package]]
+name = "pyserial"
+version = "3.5"
+description = "Python Serial Port Extension"
+category = "main"
+optional = false
+python-versions = "*"
+files = [
+    {file = "pyserial-3.5-py2.py3-none-any.whl", hash = "sha256:c4451db6ba391ca6ca299fb3ec7bae67a5c55dde170964c7a14ceefec02f2cf0"},
+    {file = "pyserial-3.5.tar.gz", hash = "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb"},
+]
+
+[package.extras]
+cp2110 = ["hidapi"]
+
+[metadata]
+lock-version = "2.0"
+python-versions = "^3.11"
+content-hash = "e62c345d7dc7473afcf2d0b5bdf8471dec4b4834efc156b4cef53daeb59c89ff"

modules/debugger/pyproject.toml

@@ -0,0 +1,18 @@
+[tool.poetry]
+name = "debugger"
+version = "0.1.0"
+description = ""
+authors = ["GHOSCHT <31184695+GHOSCHT@users.noreply.github.com>"]
+readme = "README.md"
+
+[tool.poetry.dependencies]
+python = "^3.11"
+pyserial = "^3.5"
+
+
+[tool.poetry.group.dev.dependencies]
+autopep8 = "^2.0.2"
+
+[build-system]
+requires = ["poetry-core"]
+build-backend = "poetry.core.masonry.api"