Diff: main.cpp

Revision:

4:59628fe57da0

Parent:

3:d6f80e11a7f4

--- a/main.cpp	Wed Mar 30 11:21:50 2016 +0000
+++ b/main.cpp	Mon Sep 26 20:02:00 2016 +0000
@@ -6,32 +6,93 @@
 #include "ble_radio_notification.h"
 #include "ble_gap.h"
 
+
+#define NUMBER_OF_PERIPHERALS 1
+#define TXRX_BUF_LEN                     20  /** For radio message transmission*/
+
 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
-};
+// The Nordic UART Service
+static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
+static const uint8_t uart_tx_uuid[]   = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
+static const uint8_t uart_rx_uuid[]   = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
+static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71};
+static const uint8_t uart_tx_uuid_rev[] =   {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 3, 0, 0x3D, 0x71};
+static const uint8_t uart_rx_uuid_rev[] =   {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 2, 0, 0x3D, 0x71};
 
-const uint8_t MPU6050_Accel_Characteristic_uuid[] = {
-    0x45,0x35,0x56,0x81,0x0F,0xD8,0x5F,0xB5,0x51,0x48,0x30,0x27,0x06,0x9B,0x3F,0xD9
-};
+static uint8_t rx_buf[TXRX_BUF_LEN];
+static uint8_t rx_len=0;
+bool bReadingReady=false, bReadNow=false, bScanUartBLE[NUMBER_OF_PERIPHERALS] = {false,},bScanRqtBLE[NUMBER_OF_PERIPHERALS] = {false,};
+bool bScanPending[NUMBER_OF_PERIPHERALS] = {false,},bFirstIn[NUMBER_OF_PERIPHERALS] = {false,};
+uint8_t gNumOfClients=1, gNumOfDevConnected=0;
+UUID serviceUUID(uart_base_uuid);
+UUID accelTxUUID(uart_tx_uuid);
+UUID accelRxUUID(uart_rx_uuid);
+uint16_t gCounter=0;
 
-DiscoveredCharacteristic accelChar;
-UUID serviceUUID(MPU6050_service_uuid);
-UUID accelUUID(MPU6050_Accel_Characteristic_uuid);
-
-#define NUMBER_OF_PERIPHERALS 3
-
+Ticker periodicActions;
 typedef struct {
     Gap::Handle_t   handle;
     Gap::Address_t  address;
-    bool    connected;
+    DiscoveredCharacteristic TxChar;
+    DiscoveredCharacteristic RxChar;
+    bool connected;
+    bool active;
     uint8_t*    deviceName;
 } peripheral_t;
 
 static peripheral_t gs_peripheral[NUMBER_OF_PERIPHERALS];
 
+uint8_t txPayload[TXRX_BUF_LEN] = {0,};
+uint8_t rxPayload[TXRX_BUF_LEN] = {0,};
+
+/* function to read from a registered client*/
+void readFromClient(uint8_t clientNr){
+    if (clientNr <gNumOfDevConnected){
+        if(gs_peripheral[clientNr].connected){
+            //ble.gattClient().read(gs_peripheral[clientNr].RxChar.getConnectionHandle(), gs_peripheral[clientNr].RxChar.getValueHandle(), 0);
+            ble.gattClient().read(gs_peripheral[clientNr].handle, gs_peripheral[clientNr].RxChar.getValueHandle(), 0);
+        }
+    }
+}
+int8_t getClientIdFromHandle(Gap::Handle_t handle){
+uint8_t i=0;
+    while (i<gNumOfDevConnected){        
+        if (memcmp(&handle,&gs_peripheral[i].handle,sizeof(Gap::Handle_t))==0){
+            return i;
+        }
+        i++;         
+    }
+    return -1;
+}
+
+uint32_t ble_advdata_parser_all(uint8_t type, uint8_t advdata_len, uint8_t *p_advdata)
+{
+    uint8_t index=0,i,len;
+    char myString[32];
+    uint8_t my_field_data[32];
+    uint8_t field_type;
+    
+    while(index<advdata_len)
+    {
+        len = p_advdata[index]-1;
+        field_type   = p_advdata[index+1];
+         {
+            memcpy(my_field_data, &p_advdata[index+2], len);
+            
+            pc.printf("T:%x ",field_type);
+            for (i=0;i<len;i++)
+                pc.printf("%x",my_field_data[i]);            
+            memcpy(myString,my_field_data, len);
+            pc.printf(" %s\r\n",myString);
+            memset(my_field_data,0,32);   
+        }
+        index += len + 2;
+    }
+    return NRF_SUCCESS;
+}
+
 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;
@@ -53,59 +114,73 @@
 }
 
 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",
+    uint8_t len;char myString[32];
+    uint8_t adv_name[31];
+    
+    pc.printf("PeerAddr[%02x%02x%02x%02x%02x%02x], Rssi %d, isScanRsp %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);
     
-    uint8_t len;
-    uint8_t adv_name[31];
-    if( NRF_SUCCESS == ble_advdata_parser(BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME,
+    ble_advdata_parser_all(BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME, params->advertisingDataLen, (uint8_t *)params->advertisingData);
+   // if( NRF_SUCCESS == ble_advdata_parser(BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME,
+    if( NRF_SUCCESS == ble_advdata_parser(BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME,
                              params->advertisingDataLen,
                             (uint8_t *)params->advertisingData, &len, adv_name)){
-
-        for(uint8_t i=0; i<3; i++){
+        for(uint8_t i=0; i<gNumOfClients; i++){
             if(gs_peripheral[i].connected == false){
                 memcpy(gs_peripheral[i].address, params->peerAddr, sizeof(params->peerAddr)); 
+                memcpy(myString,adv_name,len);
+                pc.printf("%s - %c\r\n",myString,gs_peripheral[i].address);
                 gs_peripheral[i].deviceName = adv_name;
+                gs_peripheral[i].active=false;
                 ble.connect(params->peerAddr, BLEProtocol::AddressType::RANDOM_STATIC, NULL, NULL);
-                break;
-            }
+              //  break;
+            } 
         }
-        ble.stopScan();
+        if (gNumOfDevConnected==gNumOfClients)
+            ble.stopScan();
+            pc.printf("Stop Scan\r\n");
     }
 }
 
 void serviceDiscoveryCallback(const DiscoveredService *service) {
-    pc.printf("service found...\r\n");    
+    uint8_t buff[16];int16_t i=0;
+        
+    memcpy( buff,service->getUUID().getBaseUUID(),16);
+    /* Check only  for the UART service*/
+    if (memcmp(uart_base_uuid_rev,service->getUUID().getBaseUUID(),16)!=0)
+        pc.printf("UART Serv Not Found\r\n",i);        
 }
 
-void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP) {
-    pc.printf("characteristicDiscoveryCallback\r\n");
-    
-    accelChar = *characteristicP;
+void characteristicRxTxDiscoveryCallback(const DiscoveredCharacteristic *characteristicP) {
+    //char buff[30],i=0;
+    // Tx and Rx characteristic should be found !
+    uint8_t idx=0;
+    idx=getClientIdFromHandle(characteristicP->getConnectionHandle());
     
-    for(uint8_t i=0; i<3; i++){
-        if(gs_peripheral[i].connected){
-            ble.gattClient().read(characteristicP->getConnectionHandle(), characteristicP->getValueHandle(), 0);
-        }
+    if (memcmp(uart_tx_uuid_rev,characteristicP->getUUID().getBaseUUID(),characteristicP->getUUID().getLen())==0){        
+        gs_peripheral[idx].TxChar = *characteristicP;
+        pc.printf("C%c: Tx Service !\r\n",idx);
+        
+    } else if (memcmp(uart_rx_uuid_rev,characteristicP->getUUID().getBaseUUID(),characteristicP->getUUID().getLen())==0){
+        gs_peripheral[idx].RxChar = *characteristicP;
+        pc.printf("C%c: Rx Service !\r\n",idx);
     }
-
 }
 
 void connectionCallback(const Gap::ConnectionCallbackParams_t *params) {
-    pc.printf("GAP_EVT_CONNECTED\r\n");
+    //pc.printf("GAP_EVT_CONNECTED\r\n");
     if (params->role == Gap::CENTRAL) {
 
-        for(uint8_t i=0; i<3; i++){
+        for(uint8_t i=0; i<gNumOfClients; i++){
             if(gs_peripheral[i].connected == false){
+                gNumOfDevConnected++;
                 gs_peripheral[i].handle = params->handle;
                 gs_peripheral[i].connected = true;
-                break;
+                break;                
             }
-        }
-                
-        ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, serviceUUID, accelUUID);
-        
+       }                
+      ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicRxTxDiscoveryCallback, serviceUUID);    
     }
 }
 
@@ -114,47 +189,153 @@
 }
 
 void triggerRead(const GattReadCallbackParams *response) {
-    pc.printf("triggerRead.....\r\n");
-    
-    pc.printf("len: %d\r\n", response->len);
+    int idx; char msg[25]={'C',0,':',0}; 
+    //uint16_t counter=0;
+    static char pmsg[25];
+    //pc.printf("triggerRead.....\r\n");
+    //pc.printf("len: %d\r\n", response->len);
+    idx = getClientIdFromHandle(response->connHandle);
+    //pc.printf("trRead idx=%d",idx);
     const uint8_t *data = response->data;
-    pc.printf("data ");
-    for(int i=0; i < response->len; i++){
-        pc.printf("%f ", (float)data[i]);  
+    if (response->len>0){
+        msg[1]='0'+idx;
+        //pc.printf("C%c:", idx+'0');  
+        memcpy(&msg[4],data,response->len);              
+        if (bScanUartBLE[idx]==true){ 
+            if(bScanRqtBLE[idx]==true){
+                bFirstIn[idx] = true;
+                gCounter ++;
+                // stop condition
+                bScanPending[idx] = true;                    
+            //pc.printf("C%c:%x %d=%s\r\n",msg[1],msg[4],msg[5],&msg[6]);
+            } else {
+                //if (memcmp(msg,pmsg,25)!=0){    
+                     pc.printf("C%d:%s\r\n",idx,&msg[4]);
+                    memcpy(pmsg,msg,25);
+                //}
+            }
+        } else{  
+             //if (memcmp(msg,pmsg,25)!=0){    
+                    pc.printf("C%d:%s\r\n",idx,&msg[4]);
+                    memcpy(pmsg,msg,25);
+              //  }        
+             gs_peripheral[idx].active = false;            
+        }            
     }
-    pc.printf("\r\n");
-    accelChar.read();
 }
 
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params){
+    uint8_t idx;
     pc.printf("disconnected\r\n");
+    idx = getClientIdFromHandle(params->handle);
     
-    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;
+    if(gs_peripheral[idx].handle == params->handle){
+        gs_peripheral[idx].connected = false;
+        gs_peripheral[idx].handle = 0xFFFF;
+        gs_peripheral[idx].active = false;
+        gNumOfDevConnected --;
+    }
+    pc.printf("numOfDevConn %d",gNumOfDevConnected);
+    wait(3.0);
+    ble.gap().startScan(scanCallback);
+}
+
+void at_eachInstant(){
+    for(uint8_t i=0; i<gNumOfDevConnected; i++){
+        if (gs_peripheral[i].active == true){            
+            if ((bScanRqtBLE[i]==true)&&(bFirstIn[i]==true)){
+                if (bScanPending[i] == true){
+                    bScanPending[i] = false;     
+                } else {
+                    bScanRqtBLE[i]=false;
+                    pc.printf("gCounter %d",gCounter);
+                }       
+            }
+            readFromClient(i);    
         }
     }
-    wait(8.0);
-    ble.gap().startScan(scanCallback);
+}
+void decodeLocalCmd(uint8_t length,uint8_t * buffer){
+switch (buffer[0]){
+    case 's':{
+        /*scan options*/
+        if (buffer[1]=='0'){
+            /*stop reading scan */
+            bScanUartBLE[buffer[2]-'0'] = false;
+            
+        } else if (buffer[1]=='1'){
+            /*start reading scan*/
+            bScanUartBLE[buffer[2]-'0'] = true;
+        } else if (buffer[1]=='2'){
+            uint8_t rxBuff[5]={'x','f','4','\r','\n'};
+            /*start reading scan*/
+            gCounter =0;
+            bScanUartBLE[buffer[2]-'0'] = true;            
+            bScanRqtBLE[buffer[2]-'0'] = true;            
+            gs_peripheral[buffer[2]-'0'].TxChar.write(5,rxBuff);   
+            gs_peripheral[buffer[2]-'0'].active = true;
+        } else {
+            pc.printf("No existing cmd!\r\n");
+        }
+        break;
+    }
+    case 'r':{
+        break;
+    }
+    case 'i':{
+        break;
+    }
+    default:{
+        pc.printf("No existing cmd!\r\n");
+    }
+}
+
+}
+void uartCB(void)
+{    
+    while(pc.readable()) {
+        rx_buf[rx_len++] = pc.getc();
+        if(rx_len>=20 || rx_buf[rx_len-1]=='\0' || rx_buf[rx_len-1]=='\n') {
+            /* start with m then it is a command*/
+            if (rx_buf[0]=='m')
+            {
+                if (rx_buf[1]=='l'){
+                    /* local BLE = central BLE, a local command*/
+                    decodeLocalCmd(rx_len-2,&rx_buf[2]);
+                } else if ((rx_buf[1]>='0')&&(rx_buf[1]<('0'+NUMBER_OF_PERIPHERALS))){
+                     /* send command to indicated client*/
+                     gs_peripheral[rx_buf[1]-'0'].TxChar.write(rx_len-2,&rx_buf[2]);   
+                     gs_peripheral[rx_buf[1]-'0'].active = true;
+                     //bReadNow= true;
+                } else {
+                    pc.printf("Invalid Cmd ! No existing client %c\r\n",rx_buf[1]);
+                }            
+            }
+            rx_len= 0;
+            break;
+        }
+    }
 }
 
 int main(void) {
-    pc.baud(9600);
-    wait(8.0);
+    pc.baud(19200);
+    wait(5.0);
+    
+    pc.attach( uartCB , pc.RxIrq);
+    ble.init();
     pc.printf("start\r\n");
-
-    ble.init();
-
+    //ble.addService(uartService);
     ble.onConnection(connectionCallback);
     ble.onDisconnection(disconnectionCallback);
-    ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
+    //ble.onDataWritten(WrittenHandler);
+    //ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
     ble.gattClient().onDataRead(triggerRead);
-    //ble.gattClient().onDataWrite(triggerToggledWrite);
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,(const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
 
-    ble.gap().setScanParams(500, 400);
+    ble.gap().setScanParams(5, 5);
     ble.gap().startScan(scanCallback);
-
+    // 100 msec
+    periodicActions.attach_us(&at_eachInstant,1000u);        
     while (true) {
         ble.waitForEvent();
     }