Make receiver work

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/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