Diff: main.cpp

Revision:

9:7360ae6b9b1e

Parent:

7:d78080148084

Child:

10:0733d4800ed1

--- a/main.cpp	Sat Jul 18 06:56:25 2015 +0000
+++ b/main.cpp	Thu Jul 23 12:23:55 2015 +0000
@@ -16,56 +16,113 @@
 
 #include "mbed.h"
 #include "BLE.h"
+#include "advertiser_data.h"
+#define DEBUG 1
 
 BLE        ble;
+#ifdef DEBUG
+Serial pc(USBTX, USBRX);
+#endif
 
-typedef union _accleration_data {
-  float f;
-  uint8_t  b[4];
-} acceleration_data;
+// Note: if this is set >254 then lots of assumptions are broken...
+#define MAX_ADVERTISERS 10
+static advertiser_data advertisers[MAX_ADVERTISERS];
+static uint8_t advertiser_index[MAX_ADVERTISERS];
+static uint8_t advertiser_count = 0;
 
-typedef enum _parse_state {
-    PARSE_SIZE,
-    PARSE_TYPE,
-    PARSE_DATA
-} ParseState;
+uint8_t get_index(uint8_t address)
+{
+    /* Find in index array */
+    for (unsigned i = 0; i < advertiser_count; i++) {
+        if (advertiser_index[i] == address) return i;
+    }
+    /* Not found, insert if still room */
+    if (advertiser_count < MAX_ADVERTISERS) {
+        advertiser_count++;
+        advertiser_index[advertiser_count] = address;
+        advertiser_data_reset(&advertisers[advertiser_count]);
+        return advertiser_count;
+    } else {
+        /* TODO: delete oldest? */
+        return MAX_ADVERTISERS;
+    }
+}
 
-Serial pc(USBTX, USBRX);
+advertiser_data* get_advertiser_data(uint8_t address)
+{
+    /* Return NULL if index out of bounds */
+    uint8_t index = get_index(address);
+    return (index < MAX_ADVERTISERS) ? &advertisers[index] : NULL;
+}
+
+void get_manufacturer_data(const uint8_t *advData, uint8_t advDataLen, const uint8_t *&pManData, uint8_t &manDataLen)
+{
+    unsigned index = 0;
+    unsigned data_size = 0;
+    pManData = NULL;
+    manDataLen = 0;
+    while (index < advDataLen) {
+        data_size = advData[index];
+        if (GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA == advData[index+1]) {
+            pManData = advData + index + 2;
+            manDataLen = data_size - 2;
+            return;
+        }
+        index += data_size;
+    }
+}
+
 
 void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
 {
+#ifdef DEBUG
+    pc.printf("Found device...\n");
+#endif
     if (params->peerAddr[5] == 0xe5 && params->peerAddr[4] == 0x4f) {
-        
-        pc.printf("Adv peerAddr: [%02x %02x %02x %02x %02x %02x] rssi %d, ScanResp: %u, AdvType: %u\r\n",
-                params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
-                params->rssi, params->isScanResponse, params->type);
+#ifdef DEBUG
         for (unsigned index = 0; index < params->advertisingDataLen; index++) {
-           pc.printf("%02x ", params->advertisingData[index]);
+          pc.printf("%02x ", params->advertisingData[index]);
+        }
+        pc.printf("\n");
+#endif
+        /* Using only the 3rd part of address for indexing the stored data for now */
+        advertiser_data *pdata = get_advertiser_data(params->peerAddr[3]);
+        if (!pdata) {
+#ifdef DEBUG
+            pc.printf("Too many advertisers!\n");
+#endif
+            return;
         }
-        pc.printf("\r\n");
-        
-        /* Quick and dirty naive way */
-        acceleration_data acceleration;
-        acceleration.f = 4.5;
-        if (params->advertisingDataLen != 6) return;
-        for (unsigned i = 0; i < 4; i++) {
-            acceleration.b[i] = params->advertisingData[i+2];
-        }
-        pc.printf("Acceleration: %.2f", acceleration.f);
-        pc.printf("\r\n");
-        /*
-        unsigned index = 0;
-        uint8_t data_size = 0;
-        unsigned data_index = 0;
-        while (index < params->advertisingDataLen) {
-            data_size = params->advertisingData[index];
-            data_index = index;
-            // TODO: CHECK for size > dataLen
-            for (index; index < data_index + data_size; index++) { 
-                // do something with the data 
+
+        /* Always update the rssi */
+        advertiser_data_update_rssi(pdata, params->rssi);
+
+        /* Find the manufacturers data */
+        const uint8_t *manData = NULL;
+        uint8_t manDataLen = 0;
+        get_manufacturer_data(params->advertisingData, params->advertisingDataLen, manData, manDataLen);
+
+        /* If the data is empty, skip the counter check */
+        if (!advertiser_data_is_empty(pdata)) {
+            uint8_t diff = advertiser_data_counter_difference(pdata, manData, manDataLen);
+            if (diff == 0) {
+                /* Quit early if we've seen this packet */
+                return;
+            } else if (diff > 1) {
+#ifdef DEBUG
+                pc.printf("resetting: diff is %d\n", diff);
+#endif
+                /* Reset the data if we missed a packet (likely lost signal for a while) */
+                advertiser_data_reset(pdata);
+                advertiser_data_update_rssi(pdata, params->rssi);
             }
         }
-        */
+
+        /* Update everything from the manufacturer data */
+        advertiser_data_update(pdata, manData, manDataLen);
+
+        // TODO: implement the rest of the algorithms
+        advertiser_data_print(pdata);
     }
 }
 
@@ -73,14 +130,19 @@
 {
     ble.init();
 
+#ifdef DEBUG
+    pc.baud(9600);
+    wait(8);
+    pc.printf("Started scanning...\n\r");
+#endif
+
     // Set scan to be constant by interval == window
     ble.gap().setScanParams(500 /* scan interval */, 500 /* scan window */);
     ble.gap().startScan(advertisementCallback);
-    
-    pc.baud(9600);
-    wait(8);
-    pc.printf("Started scanning...\n\r");
-    
+
+    for (int i = 0; i < MAX_ADVERTISERS; i++) {
+        advertiser_data_reset(&advertisers[i]);
+    }
 
     while (true) {
         ble.waitForEvent();