File content as of revision 9:3e967b414bd5:

#include <map>
#include <string>
#include "posService.h"
#include "mbed.h"
#include "ble/BLE.h"

DigitalOut led1(LED1, 1);
Ticker     ticker;
typedef pair<long,int> map_val;
map<string, map_val > dmap;

         typedef uint8_t  dist_t[3];
         dist_t poss;

const static char DEVICE_NAME[] = "Locator Mobile";
static const uint16_t serviceList[] = {
    0x2601
};

void periodicCallback(void)
{
    led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
}

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 (params->type == 3) {
        /*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);
        
        for (unsigned index = 0; index < params->advertisingDataLen; index++) {
            printf("%02x ", params->advertisingData[index]);
        }
        */
        char buf[20];
        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]);
        printf("ID=%s  ", buf);
        if (dmap.count(buf) > 0) { //element exists
            map_val val = dmap[buf];
            val.first += params->rssi;
            val.second++;
            dmap[buf] = val;
            if (val.second==8)//reset
                dmap[buf] = map_val((int)val.first/val.second, 1);
            }
        else { //new element
            dmap[buf] = map_val(0,0);
        }
 
 map_val val = dmap[buf];
        printf("%ld/%d=%.2f  ", val.first, val.second, (float)val.first/val.second);
        printf("dist=%.2lf  ", calculateAccuracy(-65,(double)val.first/val.second));
        printf("\r\n");
        
        int i = 0;
        map<string, map_val >::iterator it,end;
        for (it = dmap.begin(), end = dmap.end();
                       i<3 && it != end ; ++it, ++i)
            {
                map_val val = it->second;
                poss[i] = val.first;
            }
    }
}
/**
 * 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(200 /* scan interval */, 200 /* scan window */);
    ble.gap().startScan(advertisementCallback);
}

int main(void)
{   
    ticker.attach(periodicCallback, 1);

    BLE &ble = BLE::Instance();
    PosService posService(ble);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)serviceList, sizeof(serviceList));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED);
    
    ble.init(bleInitComplete);

    while (true) {
        ble.waitForEvent();
        posService.updatePos(poss);
    }
}