From 4e7aeee1d98bc660f9680878b571c97502f87ab7 Mon Sep 17 00:00:00 2001 From: philsson Date: Sun, 30 Sep 2018 21:00:46 +0200 Subject: [PATCH] Serial out First working example of sending data back to the remote. For now we send some dummy data. --- .vscode/settings.json | 3 +- NodeMCU/OSCWifiBridge/OSCWifiBridge.ino | 142 +++++++++++++++++++----- main.cpp | 54 ++++++--- src/serialization/RCProtocol.cpp | 61 ++++++++-- src/serialization/RCProtocol.h | 57 +++++++--- 5 files changed, 250 insertions(+), 67 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 5c6df01..fe6ef7a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -19,6 +19,7 @@ "string": "cpp", "initializer_list": "cpp", "string_view": "cpp", - "utility": "cpp" + "utility": "cpp", + "algorithm": "cpp" } } \ No newline at end of file diff --git a/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino b/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino index a114bc0..d8540d1 100644 --- a/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino +++ b/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino @@ -29,7 +29,7 @@ OSCErrorCode error; // Start "on" unsigned int ledState = HIGH; -typedef struct Packet { +typedef struct PacketOut { int16_t MagicWordLow; int16_t MagicWordHigh; @@ -46,21 +46,34 @@ typedef struct Packet { } __attribute__ ((__packed__)); -const int bufferSize = sizeof(Packet); // Size of Packet -uint8_t buffer[bufferSize]; +struct PacketIn { + + int16_t BatteryLevel; + + int16_t Mode; + + uint8_t checksum; + +} __attribute__ ((__packed__)); -Packet* pPacket = (Packet*)buffer; +const int bufferOutSize = sizeof(PacketOut); // Size of Packet +uint8_t bufferOut[bufferOutSize]; +PacketOut* pPacketOut = (PacketOut*)bufferOut; + +const int bufferInSize = sizeof(PacketIn); +uint8_t bufferIn[bufferInSize]; +PacketIn* pPacketIn = (PacketIn*)bufferIn; void calcChecksum() { uint16_t sum = 0; - for (int i = 4; i < (bufferSize - 1); i++) + for (int i = 4; i < (bufferOutSize - 1); i++) { - sum += buffer[i]; + sum += bufferOut[i]; } - pPacket->checksum = (uint8_t)(sum & 0xFF); + pPacketOut->checksum = (uint8_t)(sum & 0xFF); //Serial.print("sum: "); //Serial.println(sum); //Serial.print("checksum: "); @@ -79,22 +92,22 @@ void setup() { Udp.begin(localPort); // Initialize the packet - pPacket->MagicWordLow = 0x0DED; - pPacket->MagicWordHigh = 0x0DEC; + pPacketOut->MagicWordLow = 0x0DED; + pPacketOut->MagicWordHigh = 0x0DEC; - pPacket->Throttle = 1; - pPacket->Steering = 2; - pPacket->Kp = 3; - pPacket->Ki = 4; - pPacket->Kd = 5; - pPacket->Enabled = false; - pPacket->checksum = 0; + pPacketOut->Throttle = 1; + pPacketOut->Steering = 2; + pPacketOut->Kp = 3; + pPacketOut->Ki = 4; + pPacketOut->Kd = 5; + pPacketOut->Enabled = false; + pPacketOut->checksum = 0; } void sendPacket() { calcChecksum(); - Serial.write(buffer, bufferSize); + Serial.write(bufferOut, bufferOutSize); } void OSCMsgReceive() @@ -122,17 +135,24 @@ void OSCMsgReceive() void sendUdp(OSCMessage* pMessage) { IPAddress remoteIp = Udp.remoteIP(); + + /* Address is exchanged for 0.0.0.0 so when + * forwarding from the other board the + * package does not reach the phone + * TODO: Check for address other than 0.0.0.0 + * before saving if (remoteIp != outIp) { outIp = remoteIp; Serial.print("New source address: "); Serial.println(outIp); } - + */ + //send osc message back to controll object in TouchOSC //Local feedback is turned off in the TouchOSC interface. //The button is turned on in TouchOSC interface whe the conrol receives this message. - Udp.beginPacket(Udp.remoteIP(), outPort); + Udp.beginPacket(outIp, outPort); pMessage->send(Udp); // send the bytes Udp.endPacket(); // mark the end of the OSC Packet pMessage->empty(); // free space occupied by message @@ -146,7 +166,7 @@ void funcEnabled(OSCMessage &msg, int addrOffset){ msgOUT.add(!ledState); sendUdp(&msgOUT); - pPacket->Enabled = !ledState; + pPacketOut->Enabled = !ledState; //Serial.println( ledState ? "Robot disabled" : "Robot enabled"); } @@ -157,7 +177,7 @@ void funcThrottle(OSCMessage &msg, int addrOffset ){ OSCMessage msgOUT("/Fader/Throttle"); msgOUT.add(value); sendUdp(&msgOUT); - pPacket->Throttle = value; + pPacketOut->Throttle = value; } void funcSteering(OSCMessage &msg, int addrOffset ){ @@ -166,7 +186,7 @@ void funcSteering(OSCMessage &msg, int addrOffset ){ OSCMessage msgOUT("/Fader/Steering"); msgOUT.add(value); sendUdp(&msgOUT); - pPacket->Steering = value; + pPacketOut->Steering = value; } void funcKp(OSCMessage &msg, int addrOffset ){ @@ -175,7 +195,7 @@ void funcKp(OSCMessage &msg, int addrOffset ){ OSCMessage msgOUT("/Gain/Kp"); msgOUT.add(value); sendUdp(&msgOUT); - pPacket->Kp = value; + pPacketOut->Kp = value; // Redo this for label OSCMessage msgLABEL("/Gain/KpOut"); @@ -189,7 +209,7 @@ void funcKi(OSCMessage &msg, int addrOffset ){ OSCMessage msgOUT("/Gain/Ki"); msgOUT.add(value); sendUdp(&msgOUT); - pPacket->Ki = value; + pPacketOut->Ki = value; // Redo this for label OSCMessage msgLABEL("/Gain/KiOut"); @@ -203,7 +223,7 @@ void funcKd(OSCMessage &msg, int addrOffset ){ OSCMessage msgOUT("/Gain/Kd"); msgOUT.add(value); sendUdp(&msgOUT); - pPacket->Kd = value; + pPacketOut->Kd = value; // Redo this for label OSCMessage msgLABEL("/Gain/KdOut"); @@ -211,6 +231,72 @@ void funcKd(OSCMessage &msg, int addrOffset ){ sendUdp(&msgLABEL); } +// Send OSC packets updating touchOSC on the mobile +void remotePresentation() +{ + OSCMessage msgLABEL("/battery"); + msgLABEL.add(pPacketIn->BatteryLevel); + sendUdp(&msgLABEL); + + // TODO: Add the desired data to be forwarded from pPacketIn +} + +bool parsePacket() +{ + static uint8_t magicWord[4]; + static uint32_t* pMagicWord = (uint32_t*)magicWord; + + static uint8_t byteCounter = 0; + static const uint8_t payloadSize = sizeof(PacketIn); + static uint8_t payload[payloadSize]; + static PacketIn* pPacketInWorker = (PacketIn*)payload; + + uint8_t newByte = Serial.read(); + + // We have the header correct + if (*pMagicWord == 0xDEC0DED) + { + payload[byteCounter++] = newByte; + + static uint16_t sum = 0; + + if (byteCounter >= payloadSize) + { + *pMagicWord = 0; // Unvalidate the magic word + byteCounter = 0; // Reset the read counter + + uint8_t checksum = sum & 0xFF; + sum = 0; + + if (checksum == pPacketInWorker->checksum) + { + // If robot enabled we blink + // when receiving data + if (pPacketOut->Enabled) + { + ledState = !ledState; + digitalWrite(BUILTIN_LED, ledState); + } + + *pPacketIn = *((PacketIn*)payload); + + remotePresentation(); + + return true; // new package available + } + } + sum +=newByte; // placed here not to include the checksum + } + else + { + *pMagicWord = *pMagicWord >> 8 | newByte << 8*3; // Works on revo + //Serial.print("Byte: "); + //Serial.println(newByte); + } + + return false; +} + void loop() { OSCMsgReceive(); @@ -226,5 +312,11 @@ void loop() { previousMillis = currentMillis; sendPacket(); // Send on UART } + + if (Serial.available()) + { + parsePacket(); + } + } diff --git a/main.cpp b/main.cpp index df798bb..322b2d3 100644 --- a/main.cpp +++ b/main.cpp @@ -37,8 +37,8 @@ RawSerial serial(PA_2, PA_3, 250000); RCProtocol RC; // Pool to send Remote Control packages between threads -MemoryPool mpool; -Queue RCQueue; +MemoryPool mpool; +Queue RCQueue; // MPU setup SPI spi(PA_7, PA_6, PA_5); //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz @@ -73,12 +73,12 @@ void runControl() static ImuFusion imuFusion(&imu); static float controlOutput(0.0f); - static RCProtocol::Packet remote; + static RCProtocol::PacketIn remote; osEvent evt = RCQueue.get(0); if (evt.status == osEventMessage) { - RCProtocol::Packet* pPacket = (RCProtocol::Packet*)evt.value.p; - remote = *pPacket; - mpool.free(pPacket); + RCProtocol::PacketIn* pPacketIn = (RCProtocol::PacketIn*)evt.value.p; + remote = *pPacketIn; + mpool.free(pPacketIn); throttleControl.setGainScaling(remote.Ki, remote.Kd); //throttleControl.setGainScaling(0, 0); @@ -183,7 +183,7 @@ void runControl() /* -------------------------- Lastly the steering is added - straight on the output + straight on the output. -------------------------- */ float steering = remote.Steering*0.5; if (remote.Steering < 50 && remote.Steering > -50) {steering = 0;} @@ -204,20 +204,38 @@ void serialISR() // Receive while (serial.readable()) { - bool newPackage = RC.appendByte(serial.getc()); + bool newPackage = RC.readByte(serial.getc()); if (newPackage) { - RCProtocol::Packet* pPacket = mpool.alloc(); - *pPacket = RC.read(); - RCQueue.put(pPacket); + RCProtocol::PacketIn* pPacketIn = mpool.alloc(); + *pPacketIn = RC.readPacket(); + RCQueue.put(pPacketIn); } } - // Sleep some? - //Thread::wait(1); +} + +void serialWrite() +{ + RCProtocol::PacketOut packetOut; + packetOut.BatteryLevel = 123; + packetOut.Mode = 213; + RC.setOutput(packetOut); + bool done; - // Transmit - // serial.putc()... + while (true) + { + while (!serial.writeable()); + + serial.putc(RC.writeByte(done)); + + // If we successfully complete + // writing a packet we sleep + if (done) + { + Thread::wait(500); + } + } } // This context just pulses the blue LED @@ -284,7 +302,7 @@ int main() { // Start the pulsing blue led ledBlue.period_ms(10); // Any period. This will be overwritten by motors Thread ledPulseThread; - //ledPulseThread.start(callback(&pulseLedContext)); + ledPulseThread.start(callback(&pulseLedContext)); Thread::wait(100); // Serial in / out @@ -295,6 +313,10 @@ int main() { // ISR won't be called if the serial is not emptied first. serialISR(); + // Serial Write thread + Thread serialWriteThread; + serialWriteThread.start(callback(&serialWrite)); + // Enable/Activate the Gyro interrupt imu.enableInterrupt(); diff --git a/src/serialization/RCProtocol.cpp b/src/serialization/RCProtocol.cpp index d74c1c7..95a1cb6 100644 --- a/src/serialization/RCProtocol.cpp +++ b/src/serialization/RCProtocol.cpp @@ -43,14 +43,14 @@ void testParser() dummy[15] = 0x00; // !?? calculate - RCProtocol::Packet pkt = *((RCProtocol::Packet*)(dummy+4)); + RCProtocol::PacketIn pkt = *((RCProtocol::PacketIn*)(dummy+4)); for (int i = 0; i < 16; i++) { - bool newPackage = RCProt.appendByte(dummy[i]); + bool newPackage = RCProt.readByte(dummy[i]); if (newPackage) { - RCProtocol::Packet prot = RCProt.read(); + RCProtocol::PacketIn prot = RCProt.readPacket(); } } } @@ -58,26 +58,27 @@ void testParser() } RCProtocol::RCProtocol() -: m_packet() +: m_packetIn() +, m_packetOut() //, m_semaphore(0) { } -RCProtocol::Packet RCProtocol::read() +RCProtocol::PacketIn RCProtocol::readPacket() { //m_semaphore.wait(); - return m_packet; + return m_packetIn; //m_semaphore.release(); } -bool RCProtocol::appendByte(uint8_t newByte) +bool RCProtocol::readByte(uint8_t newByte) { static uint8_t header[4]; // 4 bytes for our header static uint32_t* pHeader32 = (uint32_t*)header; - static const int payloadSize = sizeof(RCProtocol::Packet); - static uint8_t payload[12]; - static RCProtocol::Packet* pPacket = (RCProtocol::Packet*)payload; + static const int payloadSize = sizeof(RCProtocol::PacketIn); + static uint8_t payload[payloadSize]; + static RCProtocol::PacketIn* pPacketIn = (RCProtocol::PacketIn*)payload; static int byteCounter = 0; @@ -95,11 +96,11 @@ bool RCProtocol::appendByte(uint8_t newByte) uint8_t checksum = sum & 0xFF; sum = 0; - if (checksum == pPacket->Checksum) + if (checksum == pPacketIn->Checksum) { // Create package //m_semaphore.wait(); - m_packet = *((Packet*)payload); + m_packetIn = *((PacketIn*)payload); //m_semaphore.release(); return true; // new package available } @@ -113,4 +114,40 @@ bool RCProtocol::appendByte(uint8_t newByte) return false; // Continue parsing } +uint8_t RCProtocol::writeByte(bool& done) +{ + static const uint8_t* pByte = (uint8_t*)&m_packetOut; + + static uint8_t i = 0; + static const uint8_t max = sizeof(RCProtocol::PacketOut); + + uint8_t outByte = pByte[i]; + i++; + + i = i % max; + + done = (i == 0) ? true : false; + + return outByte; +} + +void RCProtocol::setOutput(RCProtocol::PacketOut& packetOut) +{ + m_packetOut = packetOut; + + // Now we calculate the checksum + uint8_t* pByte = (uint8_t*)&m_packetOut; + + uint16_t sum = 0; + + for (int i = 4; i < sizeof(RCProtocol::PacketOut) - 1; i++) + { + sum += pByte[i]; + } + + uint8_t checksum = sum & 0xFF; + + m_packetOut.Checksum = checksum; +} + } \ No newline at end of file diff --git a/src/serialization/RCProtocol.h b/src/serialization/RCProtocol.h index 5e977cf..3fca330 100644 --- a/src/serialization/RCProtocol.h +++ b/src/serialization/RCProtocol.h @@ -11,7 +11,7 @@ class RCProtocol public: - struct Packet { + struct PacketIn { int16_t Throttle; int16_t Steering; @@ -24,13 +24,34 @@ public: uint8_t Checksum; - Packet() - : Throttle(0.0f) - , Steering(0.0f) - , Kp(0.0f) - , Ki(0.0f) - , Kd(0.0f) + PacketIn() + : Throttle(0) + , Steering(0) + , Kp(0) + , Ki(0) + , Kd(0) , Enabled(false) + , Checksum(0) + { + } + + } __attribute__ ((__packed__)); + + struct PacketOut { + + int32_t MagicWord; + + int16_t BatteryLevel; + + int16_t Mode; + + uint8_t Checksum; + + PacketOut() + : MagicWord(0xDEC0DED) + , BatteryLevel(0) + , Mode(0) + , Checksum(0) { } @@ -38,12 +59,20 @@ public: RCProtocol(); - //! Read the latest package - RCProtocol::Packet read(); - - //! Append a byte until we have a package to read. + //! Append a byte to read until we have a hole package. //! Returns true when a hole package is available - bool appendByte(uint8_t newByte); + bool readByte(uint8_t newByte); + + //! Read the latest package + RCProtocol::PacketIn readPacket(); + + //! Generate a byte to write + //! @done is set to true when a hole packet + //! has been passed + uint8_t writeByte(bool& done); + + //! Set the data to be written + void setOutput(RCProtocol::PacketOut& packetOut); //Semaphore m_semaphore; @@ -51,7 +80,9 @@ public: private: - RCProtocol::Packet m_packet; + RCProtocol::PacketIn m_packetIn; + + RCProtocol::PacketOut m_packetOut; };