Extacts correct Eddyston Service Data

Author: SuGlider

libraries/BLE/examples/BLE_Beacon_Scanner/BLE_Beacon_Scanner.ino

@@ -14,8 +14,6 @@
 #include <BLEEddystoneTLM.h>
 #include <BLEBeacon.h>
 
-#define ENDIAN_CHANGE_U16(x) ((((x)&0xFF00) >> 8) + (((x)&0xFF) << 8))
-
 int scanTime = 5; //In seconds
 BLEScan *pBLEScan;
 
@@ -69,23 +67,35 @@ class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks
       }
 
       uint8_t *payLoad = advertisedDevice.getPayload();
+      // search for Eddystone Service Data in the advertising payload
+      // *payload shall point to eddystone data or to its end when not found
+      const uint8_t serviceDataEddystone[3] = {0x16, 0xAA, 0xFE}; // it has Eddystone BLE UUID
+      const size_t payLoadLen = advertisedDevice.getPayloadLength();
+      uint8_t *payLoadEnd = payLoad + payLoadLen - 1; // address of the end of payLoad space
+      while (payLoad < payLoadEnd) {
+        if (payLoad[1] == serviceDataEddystone[0] && payLoad[2] == serviceDataEddystone[1] && payLoad[3] == serviceDataEddystone[2]) {
+          // found!
+          payLoad += 4;
+          break;
+        }
+        payLoad += *payLoad + 1;  // payLoad[0] has the field Length
+      }
 
-      BLEUUID checkUrlUUID = (uint16_t)0xfeaa;
-
-      if (advertisedDevice.getServiceUUID().equals(checkUrlUUID))
+      if (payLoad < payLoadEnd) // Eddystone Service Data and respective BLE UUID were found
       {
-        if (payLoad[11] == 0x10)
+        if (*payLoad == 0x10)
         {
           Serial.println("Found an EddystoneURL beacon!");
           BLEEddystoneURL foundEddyURL = BLEEddystoneURL();
-          std::string eddyContent((char *)&payLoad[11]); // incomplete EddystoneURL struct!
+          std::string eddyContent((char *)payLoad); // incomplete EddystoneURL struct!
 
           foundEddyURL.setData(eddyContent);
           std::string bareURL = foundEddyURL.getURL();
           if (bareURL[0] == 0x00)
           {
-            size_t payLoadLen = advertisedDevice.getPayloadLength();
+            // dumps all bytes in advertising payload
             Serial.println("DATA-->");
+            uint8_t *payLoad = advertisedDevice.getPayload();
             for (int idx = 0; idx < payLoadLen; idx++)
             {
               Serial.printf("0x%08X ", payLoad[idx]);
@@ -98,16 +108,20 @@ class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks
           Serial.printf("Decoded URL: %s\n", foundEddyURL.getDecodedURL().c_str());
           Serial.printf("TX power %d\n", foundEddyURL.getPower());
           Serial.println("\n");
-        }
-        if (advertisedDevice.getPayloadLength() >= 22 && payLoad[22] == 0x20)
+        } 
+        else if (*payLoad == 0x20)
         {
           Serial.println("Found an EddystoneTLM beacon!");
+ 
           BLEEddystoneTLM eddystoneTLM;
-          eddystoneTLM.setData(std::string((char*)payLoad+22, advertisedDevice.getPayloadLength() - 22));
+          eddystoneTLM.setData(std::string((char*)payLoad, 14));
           Serial.printf("Reported battery voltage: %dmV\n", eddystoneTLM.getVolt());
           Serial.printf("Reported temperature: %.2f°C (raw data=0x%04X)\n", eddystoneTLM.getTemp(), eddystoneTLM.getRawTemp());
           Serial.printf("Reported advertise count: %d\n", eddystoneTLM.getCount());
           Serial.printf("Reported time since last reboot: %ds\n", eddystoneTLM.getTime());
+          Serial.println("\n");
+          Serial.print(eddystoneTLM.toString().c_str());
+          Serial.println("\n");
         }
       }
     }