RC Parser on UART implemented

The receiving end on the stm32.
TODO: Send back data to the NodeMCU

.vscode/settings.json

@@ -16,6 +16,9 @@
         "tuple": "cpp",
         "type_traits": "cpp",
         "vector": "cpp",
-        "string": "cpp"
+        "string": "cpp",
+        "initializer_list": "cpp",
+        "string_view": "cpp",
+        "utility": "cpp"
     }
 }

Makefile

@@ -648,6 +648,7 @@ OBJECTS += ./src/drivers/servo.o
 OBJECTS += ./src/control/ImuFusion.o
 OBJECTS += ./src/control/PID.o
 OBJECTS += ./src/math/Utilities.o
+OBJECTS += ./src/serialization/RCProtocol.o
 
 
 

main.cpp

@@ -18,6 +18,8 @@
 #include "src/control/lpf.h"
 #include "src/control/PID.h"
 #include "src/control/ImuFusion.h"
+// Serialization
+#include "src/serialization/RCProtocol.h"
 
 #define WHEEL_SIZE 0.09f
 
@@ -25,11 +27,12 @@ using namespace targets::revo_f4;
 using namespace drivers;
 using namespace control;
 using namespace math;
+using namespace serialization;
 
 EventQueue queue;
 
 // Serial port (Servo Outputs)
-Serial serialMotorOutputs(PA_2, PA_3, 115200);
+Serial serial(PA_2, PA_3, 57600);
 
 // MPU setup
 SPI spi(PA_7, PA_6, PA_5);   //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
@@ -143,18 +146,30 @@ void controlFunc()
     }
 }
 
-void readSerialContext()
+void serialContext()
 {
+    static RCProtocol RC;
+
     while (true)
     {
-        while (serialMotorOutputs.readable()) 
+        // Receive
+        while (serial.readable())
         {
-            serialMotorOutputs.putc(serialMotorOutputs.getc());
+            bool newPackage = RC.appendByte(serial.getc());
+
+            if (newPackage)
+            {
+                RCProtocol::Packet packet = RC.read();
+            }
         }
+        // Sleep some?
+
+        // Transmit 
+        // serial.putc()...
     }
-    Thread::wait(1000);
 }
 
+
 // This context just pulses the blue LED
 void pulseLedContext()
 {
@@ -177,10 +192,9 @@ void pulseLedContext()
 // main() runs in its own thread
 int main() {
 
-    serialMotorOutputs.baud(115200);
+    // Serial in / out
-    // Test thread for serial
+    Thread readUartThread;
-    Thread serialThread;
+    readUartThread.start(callback(&serialContext));
-    serialThread.start(callback(&readSerialContext));
 
     // MPU startup at 100hz
     if(imu.init(10,BITS_DLPF_CFG_188HZ)){

src/serialization/RCProtocol.cpp

@@ -0,0 +1,103 @@
+#include "RCProtocol.h"
+
+namespace serialization {
+
+namespace {
+
+//! Example package
+void testParser()
+{
+    static RCProtocol RCProt;
+
+    uint8_t dummy[15];
+
+    // Magic word 233573869 / 0xDEC0DED
+    dummy[0] = 0xED; // 11101101
+    dummy[1] = 0x0D; // 00001101
+    dummy[2] = 0xEC; // 11101100
+    dummy[3] = 0x0D; // 1101
+    
+
+    // Throttle int16 1
+    dummy[4] = 0x01;
+    dummy[5] = 0x00;
+
+    // Steering int16 -1
+    dummy[6] = 0xFF;
+    dummy[7] = 0xFF;
+
+    // Kp 1234 
+    dummy[8] = 0xD2;
+    dummy[9] = 0x04;
+
+    // Ki -1234 
+    dummy[10] = 0x2E;
+    dummy[11] = 0xFB;
+
+    // Kd 10000 
+    dummy[12] = 0x10;
+    dummy[13] = 0x27;
+
+    // Bool True
+    dummy[14] = 0x01;
+
+    RCProtocol::Packet pkt = *((RCProtocol::Packet*)(dummy+4));
+
+    for (int i = 0; i < 15; i++)
+    {
+        bool newPackage = RCProt.appendByte(dummy[i]);
+        if (newPackage)
+        {
+            RCProtocol::Packet prot = RCProt.read();
+        }
+    }
+}
+
+}
+
+RCProtocol::RCProtocol()
+: m_packet()
+{
+}
+
+RCProtocol::Packet RCProtocol::read()
+{
+    // TODO: Some mutex / Semaphore
+    return m_packet;
+}
+
+bool RCProtocol::appendByte(uint8_t newByte)
+{
+    static uint8_t header[4]; // 4 bytes for our header
+    static uint32_t* pHeader32 = (uint32_t*)header;
+
+    static int payloadSize = sizeof(RCProtocol::Packet);
+
+    static uint8_t payload[11];
+
+    static int byteCounter = 0;
+
+    int temp = *pHeader32;
+
+    if (*pHeader32 == MAGIC_WORD)
+    {
+        payload[byteCounter++] = newByte;
+
+        if (byteCounter >= payloadSize)
+        {
+            *pHeader32 = 0; // Unvalidate the magic word
+            byteCounter = 0; // Reset the read counter
+
+            // Create package - TODO: Some mutex / Semaphore
+            m_packet = *((Packet*)payload); // TODO: Some mutex
+            return true; // new package available
+        }
+    }
+    else // Read header
+    {
+        *pHeader32 = *pHeader32 >> 8 | newByte << 8*3;
+    }
+    return false; // Continue parsing
+}
+
+}

src/serialization/RCProtocol.h

@@ -0,0 +1,46 @@
+#include "mbed.h"
+
+#pragma once
+
+
+namespace serialization {
+
+//! Receive commands from the Touch OSC layout through a NodeMCU
+class RCProtocol
+{
+
+public:
+
+    struct Packet {
+        int16_t Throttle;
+        int16_t Steering;
+
+        int16_t Kp;
+        int16_t Ki;
+        int16_t Kd;
+
+        bool Enabled;
+    } __attribute__ ((__packed__));
+
+    RCProtocol();
+
+    void start();
+
+    //! Read the latest package
+    RCProtocol::Packet read();
+
+    //! Append a byte until we have a package to read.
+    //! Returns true when a hole package is available
+    bool appendByte(uint8_t newByte);
+
+    // static int size();
+
+    const static uint32_t MAGIC_WORD = 0xDEC0DED;
+
+private:
+
+    RCProtocol::Packet m_packet;
+
+};
+
+}