From 2c6ac6a0e973a7d30018c7bebf7bb3d0388509b0 Mon Sep 17 00:00:00 2001 From: philsson Date: Thu, 20 Sep 2018 21:24:29 +0200 Subject: [PATCH] Checksum added to remote packet --- NodeMCU/OSCWifiBridge/OSCWifiBridge.ino | 52 +++++++++++++++++++++++-- src/serialization/RCProtocol.cpp | 35 +++++++++++------ src/serialization/RCProtocol.h | 16 ++++++++ 3 files changed, 88 insertions(+), 15 deletions(-) diff --git a/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino b/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino index 433a56c..2cf8a17 100644 --- a/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino +++ b/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 + } } diff --git a/src/serialization/RCProtocol.cpp b/src/serialization/RCProtocol.cpp index 6d2e9f5..d74c1c7 100644 --- a/src/serialization/RCProtocol.cpp +++ b/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 { diff --git a/src/serialization/RCProtocol.h b/src/serialization/RCProtocol.h index fc3cad6..5e977cf 100644 --- a/src/serialization/RCProtocol.h +++ b/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: