Lorenzo Invidia
This software setup a central node of a star topology network
Dependencies: MQTT target_st_bluenrg
Fork of
ble-star-mbed
by 
Lorenzo Invidia
Diff: source/BleMasterService.cpp
- Revision:
- 4:4af40af2530e
- Parent:
- 3:3f35e80ed848
--- a/source/BleMasterService.cpp Tue Mar 13 16:31:07 2018 +0000
+++ b/source/BleMasterService.cpp Sat Mar 31 15:10:54 2018 +0000
@@ -7,6 +7,10 @@
EventQueue eventQ(/* event count */ 128 * EVENTS_EVENT_SIZE);
+/* 1 to disable reading after master disconnection */
+uint8_t discFlag = 0;
+
+
/* This var defines the central role
* 0 if master
* 1 if slave */
@@ -21,7 +25,6 @@
-
/* Flag to indicate if the device discovery is completed successful
* 1 successful
* 0 or anything else failed */
@@ -54,6 +57,16 @@
+/* Connection Parameters struct */
+Gap::ConnectionParams_t connection_parameters = {
+ 0x0032, // min connection interval
+ 0x0087, // max connection interval
+ 0, // slave latency
+ 0x0C80 // connection supervision timeout
+};
+
+
+
/* Header pointer of the DiscoveredCharacteristic list */
DiscoveredCharacteristicNode * headCharacteristic[MAX_NUM_OF_NODES];
DiscoveredCharacteristicNode * tmp[MAX_NUM_OF_NODES];
@@ -79,16 +92,12 @@
UUID::LongUUIDBytes_t LUX_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x00,0x01};
UUID::LongUUIDBytes_t CONFIG_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x0F,0x00,0x02,0x00,0x00,0x00};
-/*----- -----*/
-uint8_t wifi_data[256];
-uint8_t new_data = 0;
-uint8_t *data;
-uint8_t wifi_present;
+extern uint8_t wifi_data[256];
+extern uint8_t new_data;
+extern uint8_t *data;
+extern uint8_t wifi_present;
char print_msg_buff[512];
-//UART_HandleTypeDef UartMsgHandle;
-
-//uint16_t connection_handle = 0;
uint8_t attribute_value[20];
uint8_t star_attr_value[256];
@@ -180,7 +189,7 @@
if ((perDevs.status != 10) && (perDevs.status != 11) && (perDevs.status != 12) && (perDevs.status != 13) && (perDevs.status != 14)
&& (perDevs.status != 15) && (perDevs.status != 16) && (perDevs.status != 17) && (perDevs.status != 18)
&& (perDevs.status != 19) && (perDevs.status != 20) && (perDevs.status != 21)){
- printf("\r\nCurrent Status (status: %d) (read: %s) (write: %s)\n", perDevs.status, (readCompleted == 1 ? "completed" : "pending"),(writeDescriptorCompleted == 1 ? "completed" : "pending"));
+ printf("\r\nCurrent Status (stat: %d) (read: %s) (write: %s)\n", perDevs.status, (readCompleted == 1 ? "completed" : "pending"),(writeDescriptorCompleted == 1 ? "completed" : "pending"));
}
}
/*----------------------------------------------------------------------------*/
@@ -203,7 +212,7 @@
void connectionProcess(void){
- //printf("\r\nconnectionProcess(%d)\n", perDevs.status);//DEBUG
+
@@ -212,7 +221,8 @@
if ( (perDevs.connDevices < MAX_NUM_OF_NODES) && (perDevs.discovery_enabled) ) {
/* Start discovery of new peripheral nodes and connect them */
//startDiscovery();
- eventQ.call(startDiscovery);
+ if (notificationPending == 0)
+ eventQ.call(startDiscovery);
}
else {
perDevs.status = DEVICE_CONNECTED;
@@ -229,7 +239,8 @@
if ( perDevs.device_found == true ) {
/* Establishing connection with a peripheral device */
perDevs.status = START_DEVICE_CONNECTION;
- connectPeripheral();
+ eventQ.call(connectPeripheral);
+ //connectPeripheral();
}else {
perDevs.status = DEVICE_NOT_FOUND;
}
@@ -332,13 +343,13 @@
/* Function called to connect a peripheral */
void connectPeripheral(void){
- ble_error_t e0, e1;
+ ble_error_t e1;
BLE& ble = BLE::Instance();
/* if 1 to enable */
-#if 1
- e0 = ble.gap().stopScan();
+#if 0
+ ble_error_t e0 = ble.gap().stopScan();
if (e0 != BLE_ERROR_NONE){
printf("\r\nError while stopping scan\n");
}
@@ -354,14 +365,20 @@
role = 0;
- e1 = ble.gap().connect(perDevs.devInfo[index].bdaddr, BLEProtocol::AddressType::RANDOM_STATIC, NULL, NULL);
-
-
+ e1 = ble.gap().connect(perDevs.devInfo[index].bdaddr, BLEProtocol::AddressType::RANDOM_STATIC, &connection_parameters, NULL);
+
if (e1 != BLE_ERROR_NONE){
- printf("Error while starting connection with peripheral %d (%02x%02x).\n",
- index+1, perDevs.devInfo[index].bdaddr[NODE_ID_B2], perDevs.devInfo[index].bdaddr[NODE_ID_B1]);
+ printf("\r\nError (%d) while connecting per %d (%02x%02x) (stat: %d) (read: %s) (write: %s)\n", e1, index+1,
+ perDevs.devInfo[index].bdaddr[NODE_ID_B2], perDevs.devInfo[index].bdaddr[NODE_ID_B1], perDevs.status,
+ (readCompleted == 1 ? "completed" : "pending"),(writeDescriptorCompleted == 1 ? "completed" : "pending"));
+
+
perDevs.is_unconnectable[index] = true;
- }
+ //perDevs.is_unconnectable[index] = false;
+
+ perDevs.device_found = false;
+ perDevs.status = DEVICE_NOT_FOUND;
+ }//if-error
}
/*----------------------------------------------------------------------------*/
@@ -436,7 +453,11 @@
/* SLAVE ROLE */
if (handle == slaveDev.conn_handle) {
printf("\r\nMaster disconnected (adv: %d - status %d)\n", ble.gap().getState().advertising, perDevs.status);//DEBUG
-
+
+ /* Disable reading after master disconnection */
+ //discFlag = 1;
+
+
slaveDev.is_discoverable = true;
slaveDev.is_connected = false;
@@ -607,9 +628,8 @@
discoveryCompleted = 0;
- /* if 1 to enable
- * if 0 default because of the adv timer setup */
-
+/* if 1 to enable
+ * if 0 default because of the adv timer setup */
#if 0
if ((slaveDev.is_connected == false) && (slaveDev.is_discoverable == true)){
//printf("\r\nslaveDev.is_discoverable == true\n");//DEBUG
@@ -664,7 +684,6 @@
/* Function called as response of advertising */
void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) { //defined in Gap.h
- //printf("\r----> advertisementCallback\n");//DEBUG
static const size_t ADDR_LEN = 6;
//the current address
@@ -676,117 +695,14 @@
uint8_t alreadyIn = 1;
- uint8_t index;
- //uint8_t i;
+ uint8_t index, i;
uint8_t found_zero_pos = 0;
BLEProtocol::AddressBytes_t zeroAddr;
memset(zeroAddr, 0, 6);
-
-if (perDevs.status != DEVICE_FOUND) {
-
-
- for (uint8_t i = 0; i < params->advertisingDataLen; ++i) {
-
- const uint8_t record_length = params->advertisingData[i];
- if (record_length == 0) {
- continue;
- }
- const uint8_t type = params->advertisingData[i + 1];
- const uint8_t* value = params->advertisingData + i + 2;
- const uint8_t value_length = record_length - 1;
-
-
-
-
-
-
-
- /* Switch because per NAMES */
- if(type == GapAdvertisingData::COMPLETE_LOCAL_NAME) {
-
-
-
- /* Switch the peripheral nodes */
-
- // M O T E N V
- if (((value_length == sizeof(NODE_MOTENV)-1) && (memcmp(value, NODE_MOTENV, value_length) == 0))) {
-
-
- //initial per data
- uint8_t peripheral_v;
- uint8_t wup_event = 0;
- uint8_t mic_event = 0;
- uint8_t prx_event = 0;
- uint8_t agm_event = 0;
- uint8_t sfusion_event = 0;
-
- uint8_t peripheral_name_len = params->advertisingData[3];
- const uint8_t * manuf_data = params->advertisingData+4+peripheral_name_len;
- uint32_t features = 0;
-
-
- features = (manuf_data[4]<<24) | (manuf_data[5]<<16) | (manuf_data[6]<<8) | manuf_data[7];
- wup_event = (features & FEATURE_MASK_WAKEUP_EVENTS) >> WUP_POS;
- mic_event = (features & FEATURE_MASK_MIC) >> MIC_POS;
- prx_event = (features & FEATURE_MASK_PROX) >> PRX_POS;
- agm_event = (features & FEATURE_MASK_ACC) >> ACC_POS;
- sfusion_event = (features & FEATURE_MASK_SENSORFUSION) >> SFUS_POS;
- peripheral_v = NODE_ME1;//MOTENV
-
-
- for (i=0; i<MAX_NUM_OF_NODES; i++) {
- if (perDevs.is_disconnected[i]) {
- if (memcmp(zeroAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
- if (!found_zero_pos) {
- index = i;
- perDevs.connDeviceIdx = i;
- found_zero_pos = 1;
- alreadyIn = 0;
- }
- }
- else if (memcmp(tempAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
- index = i;
- perDevs.connDeviceIdx = i;
- alreadyIn = 0;
- perDevs.discDevices--;
- break;
- }
- else {
- if (!found_zero_pos) {
- index = i;
- perDevs.connDeviceIdx = i;
- alreadyIn = 0;
- }
- }
- }
- }//for
-
-
- if ((!alreadyIn) && (perDevs.connDevices < MAX_NUM_OF_NODES)) {
-
-
- /* Save the found peripheral device in the struct containing all the found peripheral devices */
- saveDeviceFound(params->type, tempAddr, params->advertisingDataLen, params->advertisingData, index,
- peripheral_v, wup_event, mic_event, prx_event, agm_event, sfusion_event);
-
- perDevs.status = DEVICE_FOUND;
-
- printf("\r\nPeripheral (MOTENV) %d inserted [%02x %02x %02x %02x %02x %02x] \n", perDevs.connDevices+1,
- tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2], tempAddr[1], tempAddr[0]);
-
- }//if-alreadyIn
-
-
- }//IF-MOTENV
-
-
-
-
- // F L I G H T
- if ((value_length == sizeof(NODE_FLIGHT)-1) && (memcmp(value, NODE_FLIGHT, value_length) == 0)) {
-
+ if (perDevs.status != DEVICE_FOUND) {
+
//initial per data
uint8_t peripheral_v;
uint8_t wup_event = 0;
@@ -798,148 +714,85 @@
uint8_t peripheral_name_len = params->advertisingData[3];
const uint8_t * manuf_data = params->advertisingData+4+peripheral_name_len;
uint32_t features = 0;
-
-
- features = (manuf_data[4]<<24) | (manuf_data[5]<<16) | (manuf_data[6]<<8) | manuf_data[7];
- wup_event = (features & FEATURE_MASK_WAKEUP_EVENTS) >> WUP_POS;
- mic_event = (features & FEATURE_MASK_MIC) >> MIC_POS;
- prx_event = (features & FEATURE_MASK_PROX) >> PRX_POS;
- agm_event = (features & FEATURE_MASK_ACC) >> ACC_POS;
- sfusion_event = (features & FEATURE_MASK_SENSORFUSION) >> SFUS_POS;
- peripheral_v = NODE_FL1;//FLIGHT
-
-
-
- for (i=0; i<MAX_NUM_OF_NODES; i++) {
- if (perDevs.is_disconnected[i]) {
- if (memcmp(zeroAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
- if (!found_zero_pos) {
- index = i;
- perDevs.connDeviceIdx = i;
- found_zero_pos = 1;
- alreadyIn = 0;
- }
- }
- else if (memcmp(tempAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
- index = i;
- perDevs.connDeviceIdx = i;
- alreadyIn = 0;
- perDevs.discDevices--;
- break;
- }
- else {
- if (!found_zero_pos) {
- index = i;
- perDevs.connDeviceIdx = i;
- alreadyIn = 0;
- }
- }
+
+ if ((manuf_data[1] == MANUF_SPECIFIC_TYPE) && ((manuf_data[3] == STM32_NUCLEO)
+ || (manuf_data[3] == SENSOR_TILE) || (manuf_data[3] == BLUE_COIN)) && (peripheral_name_len == 8)) {
+
+ features = (manuf_data[4]<<24) | (manuf_data[5]<<16) | (manuf_data[6]<<8) | manuf_data[7];
+ wup_event = (features & FEATURE_MASK_WAKEUP_EVENTS) >> WUP_POS;
+ mic_event = (features & FEATURE_MASK_MIC) >> MIC_POS;
+ prx_event = (features & FEATURE_MASK_PROX) >> PRX_POS;
+ agm_event = (features & FEATURE_MASK_ACC) >> ACC_POS;
+ sfusion_event = (features & FEATURE_MASK_SENSORFUSION) >> SFUS_POS;
+
+
+ /* Switch the kind of node */
+ if (mic_event) {
+ peripheral_v = NODE_AM1;
+
+ }else if (prx_event) {
+ peripheral_v = NODE_FL1;
+
+ }else {
+ peripheral_v = NODE_ME1;
}
- }//for
-
-
- if ((!alreadyIn) && (perDevs.connDevices < MAX_NUM_OF_NODES)) {
-
-
- /* Save the found peripheral device in the struct containing all the found peripheral devices */
- saveDeviceFound(params->type, tempAddr, params->advertisingDataLen, params->advertisingData, index,
- peripheral_v, wup_event, mic_event, prx_event, agm_event, sfusion_event);
-
- perDevs.status = DEVICE_FOUND;
-
- printf("\r\nPeripheral (FLIGHT) %d inserted [%02x %02x %02x %02x %02x %02x] \n", perDevs.connDevices+1,
- tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2], tempAddr[1], tempAddr[0]);
- }//if-alreadyIn
-
- }//IF-FLIGHT
-
-
-
-
-
-
-
- // A L L M E M S
- if ((value_length == sizeof(NODE_ALLMEMS)-1) && (memcmp(value, NODE_ALLMEMS, value_length) == 0)){
-
-
- //initial per data
- uint8_t peripheral_v;
- uint8_t wup_event = 0;
- uint8_t mic_event = 0;
- uint8_t prx_event = 0;
- uint8_t agm_event = 0;
- uint8_t sfusion_event = 0;
-
- uint8_t peripheral_name_len = params->advertisingData[3];
- const uint8_t * manuf_data = params->advertisingData+4+peripheral_name_len;
- uint32_t features = 0;
-
-
- features = (manuf_data[4]<<24) | (manuf_data[5]<<16) | (manuf_data[6]<<8) | manuf_data[7];
- wup_event = (features & FEATURE_MASK_WAKEUP_EVENTS) >> WUP_POS;
- mic_event = (features & FEATURE_MASK_MIC) >> MIC_POS;
- prx_event = (features & FEATURE_MASK_PROX) >> PRX_POS;
- agm_event = (features & FEATURE_MASK_ACC) >> ACC_POS;
- sfusion_event = (features & FEATURE_MASK_SENSORFUSION) >> SFUS_POS;
- peripheral_v = NODE_AM1;//ALLMEMS
-
-
- for (i=0; i<MAX_NUM_OF_NODES; i++) {
- if (perDevs.is_disconnected[i]) {
- if (memcmp(zeroAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
- if (!found_zero_pos) {
- index = i;
- perDevs.connDeviceIdx = i;
- found_zero_pos = 1;
- alreadyIn = 0;
+
+
+ for (i=0; i<MAX_NUM_OF_NODES; i++) {
+ if (perDevs.is_disconnected[i]) {
+ if (memcmp(zeroAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
+ if (!found_zero_pos) {
+ index = i;
+ perDevs.connDeviceIdx = i;
+ found_zero_pos = 1;
+ alreadyIn = 0;
+ }
+
+ }else if (memcmp(tempAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
+ index = i;
+ perDevs.connDeviceIdx = i;
+ alreadyIn = 0;
+ perDevs.discDevices--;
+ break;
+
+ }else {
+ if (!found_zero_pos) {
+ index = i;
+ perDevs.connDeviceIdx = i;
+ alreadyIn = 0;
+ }
+ }
+ }//perDevs.is_disconnected[i]
+ }//for
+
+ if ((!alreadyIn) && (perDevs.connDevices < MAX_NUM_OF_NODES)) {
+
+ /* Save the found peripheral device in the struct containing all the found peripheral devices */
+ saveDeviceFound(params->type, tempAddr, params->advertisingDataLen, params->advertisingData, index,
+ peripheral_v, wup_event, mic_event, prx_event, agm_event, sfusion_event);
+
+ perDevs.status = DEVICE_FOUND;
+
+ switch(peripheral_v){
+ case 0x01: /* MOTENV1 */
+ printf("\r\nPeripheral (MOTENV) %d inserted [%02x %02x %02x %02x %02x %02x] \n", perDevs.connDevices+1,
+ tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2], tempAddr[1], tempAddr[0]);
+ break;
+
+ case 0x02: /* FLIGHT1 */
+ printf("\r\nPeripheral (FLIGHT) %d inserted [%02x %02x %02x %02x %02x %02x] \n", perDevs.connDevices+1,
+ tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2], tempAddr[1], tempAddr[0]);
+ break;
+
+ case 0x03: /* ALLMEMS1 */
+ printf("\r\nPeripheral (ALLMEMS) %d inserted [%02x %02x %02x %02x %02x %02x] \n", perDevs.connDevices+1,
+ tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2], tempAddr[1], tempAddr[0]);
+ break;
}
- }
- else if (memcmp(tempAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
- index = i;
- perDevs.connDeviceIdx = i;
- alreadyIn = 0;
- perDevs.discDevices--;
- break;
- }
- else {
- if (!found_zero_pos) {
- index = i;
- perDevs.connDeviceIdx = i;
- alreadyIn = 0;
- }
- }
- }
- }//for
-
-
- if ((!alreadyIn) && (perDevs.connDevices < MAX_NUM_OF_NODES)) {
-
-
- /* Save the found peripheral device in the struct containing all the found peripheral devices */
- saveDeviceFound(params->type, tempAddr, params->advertisingDataLen, params->advertisingData, index,
- peripheral_v, wup_event, mic_event, prx_event, agm_event, sfusion_event);
-
- perDevs.status = DEVICE_FOUND;
-
-
- printf("\r\nPeripheral (ALLMEMS) %d inserted [%02x %02x %02x %02x %02x %02x] \n", perDevs.connDevices+1,
- tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2], tempAddr[1], tempAddr[0]);
-
- }//if-alreadyIn
-
-
- }//IF-ALLMEMS
-
-
- }//IF-typeCOMPLETE_LOCAL_NAME
-
-
- i += record_length;
- }//for-MAIN
-
-}//if status != DEVICE_FOUND
-
+
+ }//IF-alreadyIn
+ }//IF-ST-hw
+ }//IF-!DEVICE_FOUND
}
/*----------------------------------------------------------------------------*/
@@ -2338,6 +2191,7 @@
}
+ /* Enable notification here because a reading operation was pending */
if (notificationPending == 1){
readCompleted = 1;
Change_Notification_Status(notifyP->att_data, notifyP->attr_value, notifyP->conn_handle, notifyP->i, notifyP->feature_mask, notifyP->frequency);
@@ -2353,7 +2207,6 @@
/* This function retrieves the peripheral device index and address
* from the connection handle */
void getDeviceFromConnHandle(uint16_t handle, uint8_t *index, tBDAddr devAddr){
-
//printf("\r\ngetDeviceFromConnHandle\n\n");//DEBUG
uint8_t i;
@@ -2376,7 +2229,8 @@
pointer->connHandle = connection_handle;
pointer->len = 0;
- pointer->data = NULL;
+ //pointer->data = NULL;
+ pointer->data = 0;
return pointer;
@@ -2416,43 +2270,9 @@
/* start to read sensors data from environmental */
if ((perDevs.mic_event_enabled) || (perDevs.prx_event_enabled) || (perDevs.agm_event_enabled)
- || (perDevs.sfusion_event_enabled)){
+ || (perDevs.sfusion_event_enabled) || (discFlag)){
//printf("\r\nNotification event enabled, skip reading\n");//DEBUG
- if (stackBusy == 1){
- uint8_t j;
- uint16_t attr_handle;
- uint8_t value_len;
- uint8_t attr_value[2];
- char* feat = NULL;
-
- for (j=0; j<(perDevs.connDevices+perDevs.discDevices); j++) {
-
- if ((perDevs.prx_event[j]) && (perDevs.prx_event_enabled)) {
- feat = "PRX";
- perDevs.prx_event_enabled = 0;
- perDevs.prx_on[j] = 0;
- attr_handle = perDevs.prx_char_handle[j] + 2;
-
- } else {
- continue;
- }
-
- value_len = 2;
- attr_value[0] = DISABLE;
-
- writeDescriptorCompleted=0;
- ble_error_t disableErr = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);
- printf("\r\nSet OFF the %s notifications on node %d\n", feat, j);
-
- if (disableErr != BLE_ERROR_NONE){
- printf("\r\n(readingProcess) Write charac descriptor failed (err: %d, stat: %d)\n", disableErr, perDevs.status);
- writeDescriptorCompleted=1;
-
- }//if-error
- }//for
- }//if-stackBusy
-
} else {
perDevs.status = READING_ENVIRONMENTAL;
}
@@ -2647,23 +2467,20 @@
if ((perDevs.status == ALL_DATA_READ) && (readCompleted == 1) && (writeDescriptorCompleted ==1)) {
if (i>0) {
perDevs.readDeviceIdx--;
- }
+ }//i>0
+
perDevs.status = READ_INIT;
+ if ((slaveDev.is_connected == false) && (slaveDev.is_discoverable == true)) {
+#if ENABLE_MEMS
+ disableAllNotifications(); /* Called here to disable the SFUSION notifications */
+#endif
+ setSlaveDiscoverable();
+ }
// All peripherals are read!
if (i==0) {
-
- if ((slaveDev.is_connected == false) && (slaveDev.is_discoverable == true)) {
-#if ENABLE_MEMS
- disableAllNotifications(); /* Called here to disable the SFUSION notifications */
-#endif
- setSlaveDiscoverable();
-
- } else {
- perDevs.status = CONN_INIT;
- }//if-else-adv
-
- }//if-i=0
+ perDevs.status = CONN_INIT;
+ }//i=0
}//if-ALL_DATA_READ
@@ -2686,7 +2503,7 @@
/* Start connection process */
- eventQ.call(connectionProcess);
+ //eventQ.call(connectionProcess);
}
/*----------------------------------------------------------------------------*/
@@ -2699,29 +2516,19 @@
ble_error_t error;
-
-
-
- if (!perDevs.is_disconnected[index] && characteristic.getDeclHandle()){
-// printf("\r\n\nReading sensor data from periph %d (0x%04x - 0x%04x) - status=%d\n", index+1,
-// connection_handle, characteristic.getDeclHandle(), perDevs.status);//DEBUG
-
- error = characteristic.read();
-
- if (error != BLE_ERROR_NONE){
- printf("\r\nUnable to read data from periph %d (err %d, cHndl 0x%04x - dHdl 0x%04x)\n", index+1,
- error, connection_handle, characteristic.getDeclHandle());
-
- eventQ.call(setNewStatus);
- readCompleted = 1;
- }//if-failed
-
-
- } else {
+ if (!perDevs.is_disconnected[index] && characteristic.getDeclHandle()){
+ error = characteristic.read();
+
+ if (error != BLE_ERROR_NONE){
+ printf("\r\nUnable to read data from periph %d (err %d, cHndl 0x%04x - dHdl 0x%04x)\n", index+1,
+ error, connection_handle, characteristic.getDeclHandle());
eventQ.call(setNewStatus);
-
- }//if-else-isConnected
-
+ readCompleted = 1;
+ }//if-failed
+
+ } else {
+ eventQ.call(setNewStatus);
+ }//if-else
}
/*----------------------------------------------------------------------------*/