Checksum added to remote packet

NodeMCU/OSCWifiBridge/OSCWifiBridge.ino

@@ -11,6 +11,8 @@
 char ssid[] = "ROBOT";
 char pass[] = "myrobotisawesome";
 
+const long sendInterval = 25; // in ms
+
 WiFiUDP Udp;
 
 // remote IP (not needed for receive)
@@ -39,25 +41,54 @@ typedef struct Packet {
   int16_t Kd;
   
   bool Enabled;
-};
+
+  uint8_t checksum;
+  
+} __attribute__ ((__packed__));
 
 const int bufferSize = sizeof(Packet); // Size of Packet
 uint8_t buffer[bufferSize];
 
 Packet* pPacket = (Packet*)buffer;
 
+void calcChecksum()
+{
+  uint16_t sum = 0;
+  
+  for (int i = 4; i < (bufferSize - 1); i++)
+  {
+    sum += buffer[i];
+  }
+
+  pPacket->checksum = (uint8_t)(sum & 0xFF);
+  //Serial.print("sum: ");
+  //Serial.println(sum);
+  //Serial.print("checksum: ");
+  //Serial.println(pPacket->checksum);
+}
+
 void setup() {
   WiFi.softAP(ssid, pass);
   IPAddress apip = WiFi.softAPIP();
   pinMode(LED_BUILTIN, OUTPUT);
+  //Serial.begin(115200);
   Serial.begin(57600);
   Serial.println();
   Serial.print("AP IP: ");
   Serial.println(apip);
   Udp.begin(localPort);
 
+  // Initialize the packet
   pPacket->MagicWordLow = 0x0DED;
   pPacket->MagicWordHigh = 0x0DEC;
+
+  pPacket->Throttle = 0;
+  pPacket->Steering = 0;
+  pPacket->Kp = 0;
+  pPacket->Ki = 0;
+  pPacket->Kd = 0;
+  pPacket->Enabled = false;
+  pPacket->checksum = 0;
 }
 
 void sendPacket()
@@ -105,8 +136,8 @@ void sendUdp(OSCMessage* pMessage)
   pMessage->send(Udp); // send the bytes
   Udp.endPacket(); // mark the end of the OSC Packet
   pMessage->empty(); // free space occupied by message
-  
-  sendPacket(); // Send on UART at the same time
+
+  calcChecksum();
 }
 
 void toggleEnabled(OSCMessage &msg, int addrOffset){
@@ -119,7 +150,7 @@ void toggleEnabled(OSCMessage &msg, int addrOffset){
   sendUdp(&msgOUT);
   pPacket->Enabled = ledState;
 
-  Serial.println( ledState ? "Robot disabled" : "Robot enabled");
+  //Serial.println( ledState ? "Robot disabled" : "Robot enabled");
 }
 
 void funcThrottle(OSCMessage &msg, int addrOffset ){
@@ -184,5 +215,18 @@ void funcKd(OSCMessage &msg, int addrOffset ){
 
 void loop() {
   OSCMsgReceive();
+
+  static unsigned long previousMillis = 0;
+
+  long currentMillis = millis();
+
+  if (currentMillis - previousMillis >= sendInterval)
+  {
+    // If something can be printed there is enough time
+    // between iterations
+    //Serial.println("Sending data");
+    previousMillis = currentMillis;
+    sendPacket(); // Send on UART
+  }
 }
 

src/serialization/RCProtocol.cpp

@@ -9,7 +9,7 @@ void testParser()
 {
     static RCProtocol RCProt;
 
-    uint8_t dummy[15];
+    uint8_t dummy[16];
 
     // Magic word 233573869 / 0xDEC0DED
     dummy[0] = 0xED; // 11101101
@@ -41,9 +41,11 @@ void testParser()
     // Bool True
     dummy[14] = 0x01;
 
+    dummy[15] = 0x00; // !?? calculate
+
     RCProtocol::Packet pkt = *((RCProtocol::Packet*)(dummy+4));
 
-    for (int i = 0; i < 15; i++)
+    for (int i = 0; i < 16; i++)
     {
         bool newPackage = RCProt.appendByte(dummy[i]);
         if (newPackage)
@@ -57,13 +59,15 @@ void testParser()
 
 RCProtocol::RCProtocol()
 : m_packet()
+//, m_semaphore(0)
 {
 }
 
 RCProtocol::Packet RCProtocol::read()
 {
-    // TODO: Some mutex / Semaphore
+    //m_semaphore.wait();
     return m_packet;
+    //m_semaphore.release();
 }
 
 bool RCProtocol::appendByte(uint8_t newByte)
@@ -71,27 +75,36 @@ 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 const int payloadSize = sizeof(RCProtocol::Packet);
+    static uint8_t payload[12];
+    static RCProtocol::Packet* pPacket = (RCProtocol::Packet*)payload;
 
     static int byteCounter = 0;
 
-    int temp = *pHeader32;
-
     if (*pHeader32 == MAGIC_WORD)
     {
         payload[byteCounter++] = newByte;
 
+        static uint16_t sum = 0;
+
         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
+            uint8_t checksum = sum & 0xFF;
+            sum = 0;
+
+            if (checksum == pPacket->Checksum)
+            {
+                // Create package
+                //m_semaphore.wait();
+                m_packet = *((Packet*)payload);
+                //m_semaphore.release();
+                return true; // new package available
+            }
         }
+        sum +=newByte; // placed here not to include the checksum
     }
     else // Read header
     {

src/serialization/RCProtocol.h

@@ -12,6 +12,7 @@ class RCProtocol
 public:
 
     struct Packet {
+        
         int16_t Throttle;
         int16_t Steering;
 
@@ -20,6 +21,19 @@ public:
         int16_t Kd;
 
         bool Enabled;
+
+        uint8_t Checksum;
+        
+        Packet()
+        : Throttle(0.0f)
+        , Steering(0.0f)
+        , Kp(0.0f)
+        , Ki(0.0f)
+        , Kd(0.0f)
+        , Enabled(false)
+        {
+        }
+
     } __attribute__ ((__packed__));
 
     RCProtocol();
@@ -31,6 +45,8 @@ public:
     //! Returns true when a hole package is available
     bool appendByte(uint8_t newByte);
 
+    //Semaphore m_semaphore;
+
     const static uint32_t MAGIC_WORD = 0xDEC0DED;
 
 private: