Diff: main.cpp

Revision:

3:d6f80e11a7f4

Parent:

2:4b53d13d9851

Child:

5:f6b6bf98c686

--- a/main.cpp	Sun Nov 22 11:51:48 2015 +0000
+++ b/main.cpp	Wed Mar 30 11:21:50 2016 +0000
@@ -2,13 +2,13 @@
 #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);
 
-DiscoveredCharacteristic accelChar;
-
 const uint8_t MPU6050_service_uuid[] = {
     0x45,0x35,0x56,0x80,0x0F,0xD8,0x5F,0xB5,0x51,0x48,0x30,0x27,0x06,0x9B,0x3F,0xD9
 };
@@ -17,18 +17,62 @@
     0x45,0x35,0x56,0x81,0x0F,0xD8,0x5F,0xB5,0x51,0x48,0x30,0x27,0x06,0x9B,0x3F,0xD9
 };
 
-
+DiscoveredCharacteristic accelChar;
 UUID serviceUUID(MPU6050_service_uuid);
 UUID accelUUID(MPU6050_Accel_Characteristic_uuid);
 
+#define NUMBER_OF_PERIPHERALS 3
+
+typedef struct {
+    Gap::Handle_t   handle;
+    Gap::Address_t  address;
+    bool    connected;
+    uint8_t*    deviceName;
+} peripheral_t;
+
+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;
+    
+    while(index<advdata_len)
+    {
+        field_length = p_advdata[index];
+        field_type   = p_advdata[index+1];
+        if(field_type == type)
+        {
+            memcpy(p_field_data, &p_advdata[index+2], (field_length-1));
+            *len = field_length - 1;
+            return NRF_SUCCESS;
+        }
+        index += field_length + 1;
+    }
+    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);
     
-    
-    ble.connect(params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
+    uint8_t len;
+    uint8_t adv_name[31];
+    if( NRF_SUCCESS == ble_advdata_parser(BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME,
+                             params->advertisingDataLen,
+                            (uint8_t *)params->advertisingData, &len, adv_name)){
+
+        for(uint8_t i=0; i<3; i++){
+            if(gs_peripheral[i].connected == false){
+                memcpy(gs_peripheral[i].address, params->peerAddr, sizeof(params->peerAddr)); 
+                gs_peripheral[i].deviceName = adv_name;
+                ble.connect(params->peerAddr, BLEProtocol::AddressType::RANDOM_STATIC, NULL, NULL);
+                break;
+            }
+        }
+        ble.stopScan();
+    }
 }
 
 void serviceDiscoveryCallback(const DiscoveredService *service) {
@@ -38,29 +82,30 @@
 void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP) {
     pc.printf("characteristicDiscoveryCallback\r\n");
     
-    const uint8_t *longUUIDBytes = characteristicP->getUUID().getBaseUUID();
-    bool flag = true;
-
-    if (characteristicP->getUUID().shortOrLong() != UUID::UUID_TYPE_SHORT) {
-        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++){
-            if( longUUIDBytes[i]!=MPU6050_Accel_Characteristic_uuid[i]){
-                flag=false;
-            }
-        }
-        if(flag == true) {
-            accelChar = *characteristicP;
-            ble.gattClient().read(characteristicP->getConnHandle(), characteristicP->getValueHandle(), 0);
+    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");
-    
+    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);
-        //ble.stopScan();
+        
     }
 }
 
@@ -69,34 +114,38 @@
 }
 
 void triggerRead(const GattReadCallbackParams *response) {
-    printf("triggerRead.....");
+    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("data %x", response->data[i]);  
+        pc.printf("%f ", (float)data[i]);  
     }
     pc.printf("\r\n");
     accelChar.read();
 }
 
-void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params){//(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) {
+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);
 }
 
-void your_radio_callback_handler(bool radio_active)
-{
-
-}
-
 int main(void) {
     pc.baud(9600);
     wait(8.0);
     pc.printf("start\r\n");
 
     ble.init();
-    ble_radio_notification_init(NRF_APP_PRIORITY_HIGH,
-                                NRF_RADIO_NOTIFICATION_DISTANCE_4560US,
-                                your_radio_callback_handler);
+
     ble.onConnection(connectionCallback);
     ble.onDisconnection(disconnectionCallback);
     ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
@@ -107,5 +156,6 @@
     ble.gap().startScan(scanCallback);
 
     while (true) {
+        ble.waitForEvent();
     }
 }
\ No newline at end of file