File content as of revision 5:f6b6bf98c686:

#include "mbed.h"
#include "ble/BLE.h"
#include "ble/DiscoveredCharacteristic.h"
#include "ble/DiscoveredService.h"
#include "ble/GapScanningParams.h"
#include "ble_radio_notification.h"
#include "ble_gap.h"

BLE ble;
Serial pc(USBTX, USBRX);

/*const uint8_t MPU6050_service_uuid[] = {
    0x45,0x35,0x56,0x80,0x0F,0xD8,0x5F,0xB5,0x51,0x48,0x30,0x27,0x06,0x9B,0x3F,0xD9
};*/
const uint8_t MPU6050_service_uuid[] = {
    0x00,0x00,0xFF,0xE5,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0x80,0x5F,0x9B,0x34,0xFB
};
/*const uint8_t MPU6050_Accel_Characteristic_uuid[] = {
    0x45,0x35,0x56,0x81,0x0F,0xD8,0x5F,0xB5,0x51,0x48,0x30,0x27,0x06,0x9B,0x3F,0xD9
};*/

const uint8_t MPU6050_Accel_Characteristic_uuid[] = {
    0x00,0x00,0xFF,0xE6,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0x80,0x5F,0x9B,0x34,0xFB
};
#define MSEC_TO_UNITS(TIME, RESOLUTION) (((TIME) * 1000) / (RESOLUTION))

enum
{
    UNIT_0_625_MS = 625,                                
    UNIT_1_25_MS  = 1250,                               
    UNIT_10_MS    = 10000                               
};

#define NUMBER_OF_PERIPHERALS 3

#define MIN_CONNECTION_INTERVAL   MSEC_TO_UNITS(7.5, UNIT_1_25_MS)          
#define MAX_CONNECTION_INTERVAL   MSEC_TO_UNITS(30, UNIT_1_25_MS)           
#define SLAVE_LATENCY             0                                         
#define SUPERVISION_TIMEOUT       MSEC_TO_UNITS(4000, UNIT_10_MS) 

//const char Dev_Mac_Addr[15]="5065838DA351";
DiscoveredCharacteristic accelChar;
UUID serviceUUID(MPU6050_service_uuid);
UUID accelUUID(MPU6050_Accel_Characteristic_uuid);


static const Gap::ConnectionParams_t m_connection_param =
{
    (uint16_t)MIN_CONNECTION_INTERVAL,
    (uint16_t)MAX_CONNECTION_INTERVAL,
    (uint16_t)SLAVE_LATENCY,
    (uint16_t)SUPERVISION_TIMEOUT
};
typedef struct {
    Gap::Handle_t   handle;
    Gap::Address_t  address;
    bool    connected;
    uint8_t*    deviceName;
} peripheral_t;
static const char adv_name[] = "LEDBlue-838DA351";
static peripheral_t gs_peripheral[NUMBER_OF_PERIPHERALS];


uint32_t ble_advdata_parser(uint8_t type, uint8_t advdata_len, uint8_t *p_advdata, uint8_t *len, uint8_t *p_field_data)
{
    uint8_t index=0;
    uint8_t field_length, field_type;
    pc.printf("advdata_parser1\n\r");
    pc.printf("type : %d Length : %d\n\r",type, advdata_len);
    while(index<advdata_len)
    {pc.printf("advdata_parser loop\n\r");
        field_length = p_advdata[index];
        field_type   = p_advdata[index+1];
        pc.printf("FieldLen : %d\n\r",p_advdata[index]);
        pc.printf("Field_Type : %d\n\r",p_advdata[index+1]);
        pc.printf("Fieldtype : %d\n\r",field_type);
        if(field_type == type)
        {
            pc.printf("Copy the data\n\r");
            //memcpy(p_field_data, &p_advdata[index+2], (field_length-1));
            p_field_data = &p_advdata[index+2];
            *len = field_length - 1;
            return NRF_SUCCESS;
        }
        index += field_length;
    }
    return NRF_ERROR_NOT_FOUND;
}

void scanCallback(const Gap::AdvertisementCallbackParams_t *params) {
    pc.printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %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);
           //pc.printf("params->peerAddr : %s \n\r",params->peerAddr);
      //  char peer_Addr[15];
         
   // pc.printf("params->advertisingData : %02x %02x\n\r",params->advertisingData[0],params->advertisingData[1]);
    pc.printf("Data Lenght = %d\n\r",params->advertisingDataLen);
    uint8_t len;
    
    uint8_t status;
    uint8_t stop_status;
    pc.printf("Connected1\n\r");
   /* if( NRF_SUCCESS == ble_advdata_parser(BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME,
                             params->advertisingDataLen,
                        (uint8_t *)params->advertisingData, &len, adv_name))
   {*/
    pc.printf("Connected2\n\r");
    uint8_t i=0;
       // for(uint8_t i=0; i<3; i++){
            //if(gs_peripheral[i].connected == false)
            if (params->peerAddr[0] == 0x51){
                pc.printf("Connected3\n\r");
                memcpy(gs_peripheral[i].address, params->peerAddr, sizeof(params->peerAddr)); 
                gs_peripheral[i].deviceName = (uint8_t*)adv_name;
                ble.gap().setDeviceName(gs_peripheral[i].deviceName);
                status = ble.gap().connect(params->peerAddr, BLEProtocol::AddressType::RANDOM_STATIC, &m_connection_param, NULL);
                pc.printf("Connection Status = %d\n\r",status);
                pc.printf("stop scan\n\r");
                stop_status = ble.gap().stopScan();  
                pc.printf("stop Status = %d\n\r",stop_status);           
               // break;
            }
       // }
       
        ble.stopScan();
   // }
}

void serviceDiscoveryCallback(const DiscoveredService *service) {
    pc.printf("service found...\r\n");    
}

void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP) {
    pc.printf("characteristicDiscoveryCallback\r\n");
    
    accelChar = *characteristicP;
    
    for(uint8_t i=0; i<3; i++){
        if(gs_peripheral[i].connected){
            ble.gattClient().read(characteristicP->getConnectionHandle(), characteristicP->getValueHandle(), 0);
        }
    }

}

void connectionCallback(const Gap::ConnectionCallbackParams_t *params) {
    pc.printf("GAP_EVT_CONNECTED\r\n");
    if (params->role == Gap::CENTRAL) {

        for(uint8_t i=0; i<3; i++){
            if(gs_peripheral[i].connected == false){
                gs_peripheral[i].handle = params->handle;
                gs_peripheral[i].connected = true;
                break;
            }
        }
                
        ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, serviceUUID, accelUUID);
        
    }
}

void discoveryTerminationCallback(Gap::Handle_t connectionHandle) {
    pc.printf("terminated SD for handle %u\r\n", connectionHandle);
}

void triggerRead(const GattReadCallbackParams *response) {
    pc.printf("triggerRead.....\r\n");
    
    pc.printf("len: %d\r\n", response->len);
    const uint8_t *data = response->data;
    pc.printf("data ");
    for(int i=0; i < response->len; i++){
        pc.printf("%f ", (float)data[i]);  
    }
    pc.printf("\r\n");
    accelChar.read();
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params){
    pc.printf("disconnected\r\n");
    
    for(uint8_t i=0; i<3; i++){
        if(gs_peripheral[i].handle == params->handle){
            gs_peripheral[i].connected = false;
            gs_peripheral[i].handle = 0xFFFF;
        }
    }
    wait(8.0);
   // ble.gap().startScan(scanCallback);
}

int main(void) {
    pc.baud(9600);
    wait(8.0);
    pc.printf("start\r\n");

    ble.init();

    ble.onConnection(connectionCallback);
    ble.onDisconnection(disconnectionCallback);
   // ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
   // ble.gattClient().onDataRead(triggerRead);
    //ble.gattClient().onDataWrite(triggerToggledWrite);
    
    ble.gap().setScanParams(600, 400);
    ble.gap().startScan(scanCallback);
    //ble.startAdvertising();
    while (true) {
        ble.waitForEvent();
    }
}