long dao / Mbed 2 deprecated VNG_BLEBeaconPostion

For VNG botwar

Dependencies:   BLE_API KalmanFilter mbed nRF51822

main.cpp@0:4a92d46bc788, 2016-08-02 (annotated)

Committer:
longdh
Date:
Tue Aug 02 18:07:41 2016 +0000
Revision:
0:4a92d46bc788
First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
longdh 0:4a92d46bc788 1 #include "mbed.h"
longdh 0:4a92d46bc788 2 #include "ble/BLE.h"
longdh 0:4a92d46bc788 3 #include "KamalFilterRSSI.h"
longdh 0:4a92d46bc788 4
longdh 0:4a92d46bc788 5 // q = very small, r= small, p=1
longdh 0:4a92d46bc788 6 static KamalFilterRSSI kalman(0.00001, 0.1, 1);
longdh 0:4a92d46bc788 7
longdh 0:4a92d46bc788 8 // center beacons
longdh 0:4a92d46bc788 9 static BLEProtocol::AddressBytes_t vngCenterBeacon = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
longdh 0:4a92d46bc788 10
longdh 0:4a92d46bc788 11 // distence to center
longdh 0:4a92d46bc788 12 static int vngBeaconPower = 0x56;
longdh 0:4a92d46bc788 13 static volatile double distanceToCenter = -1;
longdh 0:4a92d46bc788 14
longdh 0:4a92d46bc788 15 static double calculateAccuracy(int txPower, double rssi) {
longdh 0:4a92d46bc788 16 if (rssi == 0) {
longdh 0:4a92d46bc788 17 return -1.0; // if we cannot determine accuracy, return -1.
longdh 0:4a92d46bc788 18 }
longdh 0:4a92d46bc788 19
longdh 0:4a92d46bc788 20 double ratio = rssi*1.0/txPower;
longdh 0:4a92d46bc788 21 if (ratio < 1.0) {
longdh 0:4a92d46bc788 22 return pow(ratio,10);
longdh 0:4a92d46bc788 23 }
longdh 0:4a92d46bc788 24 else {
longdh 0:4a92d46bc788 25 double accuracy = (0.89976)* pow(ratio,7.7095) + 0.111;
longdh 0:4a92d46bc788 26 return accuracy;
longdh 0:4a92d46bc788 27 }
longdh 0:4a92d46bc788 28 }
longdh 0:4a92d46bc788 29
longdh 0:4a92d46bc788 30 void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) {
longdh 0:4a92d46bc788 31
longdh 0:4a92d46bc788 32 // if vng_beacons, filter data
longdh 0:4a92d46bc788 33 if (0 == memcmp(params->peerAddr, vngCenterBeacon, sizeof (vngCenterBeacon))) {
longdh 0:4a92d46bc788 34 double filterdRSSI = kalman.kalmanUpdate(params->rssi);
longdh 0:4a92d46bc788 35 distanceToCenter = calculateAccuracy(vngBeaconPower, filterdRSSI);
longdh 0:4a92d46bc788 36 }
longdh 0:4a92d46bc788 37 }
longdh 0:4a92d46bc788 38
longdh 0:4a92d46bc788 39 /**
longdh 0:4a92d46bc788 40 * This function is called when the ble initialization process has failed
longdh 0:4a92d46bc788 41 */
longdh 0:4a92d46bc788 42 void onBleInitError(BLE &ble, ble_error_t error)
longdh 0:4a92d46bc788 43 {
longdh 0:4a92d46bc788 44 /* Initialization error handling should go here */
longdh 0:4a92d46bc788 45 }
longdh 0:4a92d46bc788 46
longdh 0:4a92d46bc788 47 /**
longdh 0:4a92d46bc788 48 * Callback triggered when the ble initialization process has finished
longdh 0:4a92d46bc788 49 */
longdh 0:4a92d46bc788 50 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
longdh 0:4a92d46bc788 51 {
longdh 0:4a92d46bc788 52 BLE& ble = params->ble;
longdh 0:4a92d46bc788 53 ble_error_t error = params->error;
longdh 0:4a92d46bc788 54
longdh 0:4a92d46bc788 55 if (error != BLE_ERROR_NONE) {
longdh 0:4a92d46bc788 56 /* In case of error, forward the error handling to onBleInitError */
longdh 0:4a92d46bc788 57 onBleInitError(ble, error);
longdh 0:4a92d46bc788 58 return;
longdh 0:4a92d46bc788 59 }
longdh 0:4a92d46bc788 60
longdh 0:4a92d46bc788 61 /* Ensure that it is the default instance of BLE */
longdh 0:4a92d46bc788 62 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
longdh 0:4a92d46bc788 63 return;
longdh 0:4a92d46bc788 64 }
longdh 0:4a92d46bc788 65
longdh 0:4a92d46bc788 66 ble.gap().setScanParams(100 /* scan interval */, 100 /* scan window */);
longdh 0:4a92d46bc788 67 ble.gap().startScan(advertisementCallback);
longdh 0:4a92d46bc788 68 }
longdh 0:4a92d46bc788 69
longdh 0:4a92d46bc788 70 int main(void)
longdh 0:4a92d46bc788 71 {
longdh 0:4a92d46bc788 72
longdh 0:4a92d46bc788 73 BLE &ble = BLE::Instance();
longdh 0:4a92d46bc788 74 ble.init(bleInitComplete);
longdh 0:4a92d46bc788 75
longdh 0:4a92d46bc788 76 while (true) {
longdh 0:4a92d46bc788 77 ble.waitForEvent();
longdh 0:4a92d46bc788 78 }
longdh 0:4a92d46bc788 79 }