Serial out

First working example of sending data back to the remote. For now we send some dummy data.

.vscode/settings.json

@@ -19,6 +19,7 @@
         "string": "cpp",
         "initializer_list": "cpp",
         "string_view": "cpp",
-        "utility": "cpp"
+        "utility": "cpp",
+        "algorithm": "cpp"
     }
 }

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();
+  }
+  
 }
 

main.cpp

@@ -37,8 +37,8 @@ RawSerial serial(PA_2, PA_3, 250000);
 RCProtocol RC;
 
 // Pool to send Remote Control packages between threads
-MemoryPool<RCProtocol::Packet, 16> mpool;
-Queue<RCProtocol::Packet, 16> RCQueue;
+MemoryPool<RCProtocol::PacketIn, 16> mpool;
+Queue<RCProtocol::PacketIn, 16> 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();
 

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;
+}
+
 }

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;
 
 };