with 2 advertisers
Dependencies: ADXL345_nRF51 BLE_API advertiser_data mbed nRF51822
Fork of BLE_GAP_Acceleration_Observer_2_Advertisers by
Diff: main.cpp
- Revision:
- 10:0733d4800ed1
- Parent:
- 8:122d77e4fc6a
- Parent:
- 9:7360ae6b9b1e
- Child:
- 11:8bfc379ad93f
diff -r 122d77e4fc6a -r 0733d4800ed1 main.cpp --- a/main.cpp Tue Jul 21 21:50:01 2015 +0000 +++ b/main.cpp Thu Jul 23 12:38:40 2015 +0000 @@ -16,64 +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("\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("\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("Acceleration: %.2f", acceleration.f); - pc.printf("\r\n");*/ - - - /* Printing the boolean for being picked up or not*/ - bool p = params->advertisingData[2]; - pc.printf("Picked up: %p",p); - 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); } } @@ -81,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();