Diff: main.cpp

Revision:

0:4a92d46bc788

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 02 18:07:41 2016 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"
+#include "ble/BLE.h"
+#include "KamalFilterRSSI.h"
+
+// q = very small, r= small, p=1
+static KamalFilterRSSI kalman(0.00001, 0.1, 1);
+
+// center beacons
+static BLEProtocol::AddressBytes_t vngCenterBeacon = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; 
+
+// distence to center
+static int vngBeaconPower = 0x56;
+static volatile double distanceToCenter = -1;
+
+static double calculateAccuracy(int txPower, double rssi) {
+    if (rssi == 0) {
+        return -1.0; // if we cannot determine accuracy, return -1.
+    }
+
+    double ratio = rssi*1.0/txPower;
+    if (ratio < 1.0) {
+        return pow(ratio,10);
+    }
+    else {
+        double accuracy =  (0.89976)* pow(ratio,7.7095) + 0.111;    
+        return accuracy;
+    }    
+}
+
+void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) {
+
+    // if vng_beacons, filter data
+    if (0 == memcmp(params->peerAddr, vngCenterBeacon, sizeof (vngCenterBeacon))) {
+           double filterdRSSI = kalman.kalmanUpdate(params->rssi);
+           distanceToCenter = calculateAccuracy(vngBeaconPower, filterdRSSI);
+    }
+}
+
+/**
+ * This function is called when the ble initialization process has failed
+ */
+void onBleInitError(BLE &ble, ble_error_t error)
+{
+    /* Initialization error handling should go here */
+}
+
+/**
+ * Callback triggered when the ble initialization process has finished
+ */
+void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
+{
+    BLE&        ble   = params->ble;
+    ble_error_t error = params->error;
+
+    if (error != BLE_ERROR_NONE) {
+        /* In case of error, forward the error handling to onBleInitError */
+        onBleInitError(ble, error);
+        return;
+    }
+
+    /* Ensure that it is the default instance of BLE */
+    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
+        return;
+    }
+ 
+    ble.gap().setScanParams(100 /* scan interval */, 100 /* scan window */);
+    ble.gap().startScan(advertisementCallback);
+}
+
+int main(void)
+{
+
+    BLE &ble = BLE::Instance();
+    ble.init(bleInitComplete);
+
+    while (true) {
+        ble.waitForEvent();
+    }
+}