File content as of revision 0:4a92d46bc788:

#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();
    }
}