Locator Team / Mbed 2 deprecated Locator_mobile

Locator mobile firmware

Dependencies:   BLE_API mbed nRF51822

Fork of BLE_Observer by Bluetooth Low Energy

main.cpp@9:3e967b414bd5, 2017-04-30 (annotated)

Committer:
sycorax
Date:
Sun Apr 30 12:30:24 2017 +0000
Revision:
9:3e967b414bd5
Parent:
8:1b030068f28c 
Added a PosService for Broadcasting the measured distances

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sycorax 9:3e967b414bd5 1 #include <map>
sycorax 9:3e967b414bd5 2 #include <string>
sycorax 9:3e967b414bd5 3 #include "posService.h"
rgrover1 0:332983584a9c 4 #include "mbed.h"
andresag 7:88f50499af9a 5 #include "ble/BLE.h"
rgrover1 0:332983584a9c 6
andresag 7:88f50499af9a 7 DigitalOut led1(LED1, 1);
andresag 7:88f50499af9a 8 Ticker ticker;
sycorax 9:3e967b414bd5 9 typedef pair<long,int> map_val;
sycorax 9:3e967b414bd5 10 map<string, map_val > dmap;
sycorax 9:3e967b414bd5 11
sycorax 9:3e967b414bd5 12 typedef uint8_t dist_t[3];
sycorax 9:3e967b414bd5 13 dist_t poss;
sycorax 9:3e967b414bd5 14
sycorax 9:3e967b414bd5 15 const static char DEVICE_NAME[] = "Locator Mobile";
sycorax 9:3e967b414bd5 16 static const uint16_t serviceList[] = {
sycorax 9:3e967b414bd5 17 0x2601
sycorax 9:3e967b414bd5 18 };
rgrover1 0:332983584a9c 19
rgrover1 0:332983584a9c 20 void periodicCallback(void)
rgrover1 0:332983584a9c 21 {
rgrover1 0:332983584a9c 22 led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
rgrover1 0:332983584a9c 23 }
rgrover1 0:332983584a9c 24
sycorax 9:3e967b414bd5 25 double calculateAccuracy(int txPower, double rssi) {
sycorax 9:3e967b414bd5 26 if (rssi == 0) {
sycorax 9:3e967b414bd5 27 return -1.0; // if we cannot determine accuracy, return -1.
sycorax 9:3e967b414bd5 28 }
sycorax 9:3e967b414bd5 29
sycorax 9:3e967b414bd5 30 double ratio = rssi*1.0/txPower;
sycorax 9:3e967b414bd5 31 if (ratio < 1.0) {
sycorax 9:3e967b414bd5 32 return pow(ratio,10);
sycorax 9:3e967b414bd5 33 }
sycorax 9:3e967b414bd5 34 else {
sycorax 9:3e967b414bd5 35 double accuracy = (0.89976) * pow(ratio,7.7095) + 0.111;
sycorax 9:3e967b414bd5 36 return accuracy;
sycorax 9:3e967b414bd5 37 }
sycorax 9:3e967b414bd5 38 }
sycorax 9:3e967b414bd5 39
sycorax 9:3e967b414bd5 40
rgrover1 3:50a7d47912b2 41 void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) {
rgrover1 3:50a7d47912b2 42
PostaL 8:1b030068f28c 43 if (params->type == 3) {
sycorax 9:3e967b414bd5 44 /*printf("Adv peerAddr: [%02x %02x %02x %02x %02x %02x] rssi %d, ScanResp: %u, AdvType: %u\r\n",
PostaL 8:1b030068f28c 45 params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
PostaL 8:1b030068f28c 46 params->rssi, params->isScanResponse, params->type);
sycorax 9:3e967b414bd5 47
PostaL 8:1b030068f28c 48 for (unsigned index = 0; index < params->advertisingDataLen; index++) {
PostaL 8:1b030068f28c 49 printf("%02x ", params->advertisingData[index]);
PostaL 8:1b030068f28c 50 }
sycorax 9:3e967b414bd5 51 */
sycorax 9:3e967b414bd5 52 char buf[20];
sycorax 9:3e967b414bd5 53 sprintf(buf, "%02x%02x%02x%02x%02x%02x",params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0]);
sycorax 9:3e967b414bd5 54 printf("ID=%s ", buf);
sycorax 9:3e967b414bd5 55 if (dmap.count(buf) > 0) { //element exists
sycorax 9:3e967b414bd5 56 map_val val = dmap[buf];
sycorax 9:3e967b414bd5 57 val.first += params->rssi;
sycorax 9:3e967b414bd5 58 val.second++;
sycorax 9:3e967b414bd5 59 dmap[buf] = val;
sycorax 9:3e967b414bd5 60 if (val.second==8)//reset
sycorax 9:3e967b414bd5 61 dmap[buf] = map_val((int)val.first/val.second, 1);
sycorax 9:3e967b414bd5 62 }
sycorax 9:3e967b414bd5 63 else { //new element
sycorax 9:3e967b414bd5 64 dmap[buf] = map_val(0,0);
sycorax 9:3e967b414bd5 65 }
sycorax 9:3e967b414bd5 66
sycorax 9:3e967b414bd5 67 map_val val = dmap[buf];
sycorax 9:3e967b414bd5 68 printf("%ld/%d=%.2f ", val.first, val.second, (float)val.first/val.second);
sycorax 9:3e967b414bd5 69 printf("dist=%.2lf ", calculateAccuracy(-65,(double)val.first/val.second));
PostaL 8:1b030068f28c 70 printf("\r\n");
sycorax 9:3e967b414bd5 71
sycorax 9:3e967b414bd5 72 int i = 0;
sycorax 9:3e967b414bd5 73 map<string, map_val >::iterator it,end;
sycorax 9:3e967b414bd5 74 for (it = dmap.begin(), end = dmap.end();
sycorax 9:3e967b414bd5 75 i<3 && it != end ; ++it, ++i)
sycorax 9:3e967b414bd5 76 {
sycorax 9:3e967b414bd5 77 map_val val = it->second;
sycorax 9:3e967b414bd5 78 poss[i] = val.first;
sycorax 9:3e967b414bd5 79 }
rgrover1 0:332983584a9c 80 }
rgrover1 0:332983584a9c 81 }
andresag 7:88f50499af9a 82 /**
andresag 7:88f50499af9a 83 * This function is called when the ble initialization process has failed
andresag 7:88f50499af9a 84 */
andresag 7:88f50499af9a 85 void onBleInitError(BLE &ble, ble_error_t error)
andresag 7:88f50499af9a 86 {
andresag 7:88f50499af9a 87 /* Initialization error handling should go here */
andresag 7:88f50499af9a 88 }
andresag 7:88f50499af9a 89
andresag 7:88f50499af9a 90 /**
andresag 7:88f50499af9a 91 * Callback triggered when the ble initialization process has finished
andresag 7:88f50499af9a 92 */
andresag 7:88f50499af9a 93 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
andresag 7:88f50499af9a 94 {
andresag 7:88f50499af9a 95 BLE& ble = params->ble;
andresag 7:88f50499af9a 96 ble_error_t error = params->error;
andresag 7:88f50499af9a 97
andresag 7:88f50499af9a 98 if (error != BLE_ERROR_NONE) {
andresag 7:88f50499af9a 99 /* In case of error, forward the error handling to onBleInitError */
andresag 7:88f50499af9a 100 onBleInitError(ble, error);
andresag 7:88f50499af9a 101 return;
andresag 7:88f50499af9a 102 }
andresag 7:88f50499af9a 103
andresag 7:88f50499af9a 104 /* Ensure that it is the default instance of BLE */
andresag 7:88f50499af9a 105 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
andresag 7:88f50499af9a 106 return;
andresag 7:88f50499af9a 107 }
andresag 7:88f50499af9a 108
sycorax 9:3e967b414bd5 109 ble.gap().setScanParams(200 /* scan interval */, 200 /* scan window */);
andresag 7:88f50499af9a 110 ble.gap().startScan(advertisementCallback);
andresag 7:88f50499af9a 111 }
andresag 7:88f50499af9a 112
rgrover1 0:332983584a9c 113 int main(void)
sycorax 9:3e967b414bd5 114 {
rgrover1 0:332983584a9c 115 ticker.attach(periodicCallback, 1);
rgrover1 0:332983584a9c 116
andresag 7:88f50499af9a 117 BLE &ble = BLE::Instance();
sycorax 9:3e967b414bd5 118 PosService posService(ble);
sycorax 9:3e967b414bd5 119 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
sycorax 9:3e967b414bd5 120 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)serviceList, sizeof(serviceList));
sycorax 9:3e967b414bd5 121 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
sycorax 9:3e967b414bd5 122 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED);
sycorax 9:3e967b414bd5 123
andresag 7:88f50499af9a 124 ble.init(bleInitComplete);
rgrover1 0:332983584a9c 125
rgrover1 0:332983584a9c 126 while (true) {
rgrover1 0:332983584a9c 127 ble.waitForEvent();
sycorax 9:3e967b414bd5 128 posService.updatePos(poss);
rgrover1 0:332983584a9c 129 }
rgrover1 0:332983584a9c 130 }