File content as of revision 19:5b952d4f1024:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2015 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mbed.h"
#include "BLE.h"
#include "advertiser_data.h"
#define DEBUG 1

BLE        ble;
#ifdef DEBUG
Serial pc(USBTX, USBRX);
#endif

// Note: if this is set >254 then lots of assumptions are broken...
#define MAX_ADVERTISERS 10
static advertiser_data advertisers[MAX_ADVERTISERS];
static uint8_t advertiser_index[MAX_ADVERTISERS];
static uint8_t advertiser_count = 0;
static uint8_t currently_picked_up = 0;

uint8_t get_index(uint8_t address)
{
    /* Find in index array */
    for (unsigned i = 0; i < advertiser_count; i++) {
        if (advertiser_index[i] == address) return i;
    }
    /* Not found, insert if still room */
    if (advertiser_count < MAX_ADVERTISERS) {
        advertiser_count++;
        advertiser_index[advertiser_count] = address;
        advertiser_data_reset(&advertisers[advertiser_count]);
        return advertiser_count;
    } else {
        /* TODO: delete oldest? */
        return MAX_ADVERTISERS;
    }
}

advertiser_data* get_advertiser_data(uint8_t address)
{
    /* Return NULL if index out of bounds */
    uint8_t index = get_index(address);
    return (index < MAX_ADVERTISERS) ? &advertisers[index] : NULL;
}

void get_manufacturer_data(const uint8_t *advData, uint8_t advDataLen, const uint8_t *&pManData, uint8_t &manDataLen)
{
    unsigned index = 0;
    unsigned data_size = 0;
    pManData = NULL;
    manDataLen = 0;
    while (index < advDataLen) {
        data_size = advData[index];
        if (GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA == advData[index+1]) {
            pManData = advData + index + 2;
            manDataLen = data_size - 2;
            return;
        }
        index += data_size;
    }
}


void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
{
#ifdef DEBUG
    //pc.printf("Found device...\n\r");
#endif
    if (params->peerAddr[5] == 0xe5 && params->peerAddr[4] == 0x4f) {
#ifdef DEBUG
        /*for (unsigned index = 0; index < params->advertisingDataLen; index++) {
          pc.printf("%02x ", params->advertisingData[index]);
        }
        pc.printf("\n\r");*/
#endif
        /* Using only the 3rd part of address for indexing the stored data for now */
        advertiser_data *pdata = get_advertiser_data(params->peerAddr[3]);
        if (!pdata) {
#ifdef DEBUG
            pc.printf("Too many advertisers!\n\r");
#endif
            return;
        }

        /* Always update the rssi */
        uint8_t passdat = (uint8_t)(abs(params->rssi));
        advertiser_data_update_rssi(pdata, passdat);

        /* Find the manufacturers data */
        const uint8_t *manData = NULL;
        uint8_t manDataLen = 0;
        get_manufacturer_data(params->advertisingData, params->advertisingDataLen, manData, manDataLen);

        /* If the data is empty, skip the counter check */
        if (!advertiser_data_is_empty(pdata)) {
            uint8_t diff = advertiser_data_counter_difference(pdata, manData, manDataLen);
            if (diff == 0) {
                /* Quit early if we've seen this packet */
                return;
            } else if (diff > 1) {
#ifdef DEBUG
                pc.printf("resetting: diff is %d\n\r", diff);
#endif
                /* Reset the data if we missed a packet (likely lost signal for a while) */
                advertiser_data_reset(pdata);
                advertiser_data_update_rssi(pdata, passdat);
            }
        }

        /* Update everything from the manufacturer data */
        advertiser_data_update(pdata, manData, manDataLen);

        // TODO: implement the rest of the algorithms
        //advertiser_data_print(pdata,params->peerAddr[2]);
        
        if(pdata->picked_up){
            uint8_t temp = params->peerAddr[2];
            if(currently_picked_up!=temp){
                currently_picked_up = temp;
                pc.printf("CURRENTLY PICKING UP DEVICE %i\n\r",currently_picked_up);
            }
        }
    }
}

int main(void)
{
    ble.init();

#ifdef DEBUG
    pc.baud(9600);
    wait(8);
    pc.printf("Started scanning...\n\r");
#endif

    // Set scan to be constant by interval == window
    ble.gap().setScanParams(500 /* scan interval */, 500 /* scan window */);
    ble.gap().startScan(advertisementCallback);

    for (int i = 0; i < MAX_ADVERTISERS; i++) {
        advertiser_data_reset(&advertisers[i]);
    }

    while (true) {
        ble.waitForEvent();
    }
}