Diff: source/BleSlaveService.cpp

Revision:

1:110b5e896bc9

Parent:

0:1902469bdd2d

Child:

2:1525f4848cf2

--- a/source/BleSlaveService.cpp	Tue Feb 20 11:21:41 2018 +0000
+++ b/source/BleSlaveService.cpp	Mon Feb 26 10:10:37 2018 +0000
@@ -156,21 +156,21 @@
     //printf("\r\nnotifyMaster\n");//DEBUG
     ble_error_t notifyError;
 
-    if (discoveryCompleted == 1){
         
         notifyError = BLE::Instance().gattServer().write(attribute_handle+1, attribute_value, data_length, false);
 
         if (notifyError != BLE_ERROR_NONE){
             printf("\r\n(notifyMaster) Error (%d) while updating characteristic\n", notifyError);//BLE_STACK_BUSY
-            //HAL_Delay(1000);//giving some time to slowdown the rate
-            return;
+            if (notifyError == BLE_STACK_BUSY){
+                stackBusy = 1;
+                return;
+            }
+        }else {
+                writeDescriptorCompleted =1;
+                stackBusy = 0;
+                return;
         }
-        
-    } else {
-        //HAL_Delay(1000);//giving some time to slowdown the rate
-        //dOING SOMETHING !
-        return;
-    }   
+    
 }
 /*----------------------------------------------------------------------------*/
 
@@ -188,7 +188,7 @@
     GattService starService(sUuid, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
 
     /*---- Add the hw service ----*/
-    error = BLE::Instance().addService(starService);
+    error = BLE::Instance().gattServer().addService(starService);
     slaveDev.star_hw_serv_handle     = starService.getHandle();
     slaveDev.star_config_char_handle = configChar.getValueHandle()-1;   //-1 for decl handle
     slaveDev.star_data_char_handle   = dataChar.getValueHandle()-1;     //-1 for decl handle
@@ -262,14 +262,12 @@
         printf("\r\nSet OFF the %s notifications on node %d\n", feat, j);
 
         if (disableErr != BLE_ERROR_NONE){
-            printf("\r\n(disableAllNotifications) Write charac descriptor failed (%d)\n", disableErr);
+            printf("\r\n(disableAllNotifications) Write charac descriptor failed (err: %d, stat: %d)\n", disableErr, perDevs.status);
             writeDescriptorCompleted=1;
             //HAL_Delay(500);
             //disableAllNotifications();
         }//if-error
     }//for
-    
-
 }
 /*----------------------------------------------------------------------------*/
 
@@ -522,7 +520,7 @@
                     /* Then, call the Change_Notification_Status from the readCallback with these params */
                 
                 }else if (readCompleted == 1) {
-                    notificationPending = 0;
+                    //notificationPending = 0;
                     Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_MIC, notification_freq);
                 }
                 perDevs.prx_on[i] = attr_value[0];
@@ -555,13 +553,13 @@
                     notifyP->conn_handle  = conn_handle;
                     notifyP->i            = i;
                     notifyP->feature_mask = FEATURE_MASK_PROX;
-                    notifyP->frequency    = SENDING_INTERVAL_100MS_MULTIPLE;
+                    notifyP->frequency    = notification_freq*SENDING_INTERVAL_100MS_MULTIPLE;
                 
                     /* Then, call the Change_Notification_Status from the readCallback with these params */
                 
                 }else if (readCompleted == 1) {
-                    notificationPending = 0;
-                    Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_PROX, SENDING_INTERVAL_100MS_MULTIPLE);
+                    //notificationPending = 0;
+                    Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_PROX, notification_freq*SENDING_INTERVAL_100MS_MULTIPLE);
                 }    
                     perDevs.prx_on[i] = attr_value[0];
                     printf("\r\nPRX notifications [%d] %s (status %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), perDevs.status);
@@ -601,7 +599,7 @@
                     /* Then, call the Change_Notification_Status from the readCallback with these params */
                 
                 }else if (readCompleted == 1) {
-                    notificationPending = 0;
+                    //notificationPending = 0;
                     Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_ACC, SENDING_INTERVAL_100MS_MULTIPLE);
                 }
                 perDevs.agm_on[i] = attr_value[0];
@@ -650,7 +648,7 @@
                         /* Then, call the Change_Notification_Status from the readCallback with these params */
             
                     }else if (readCompleted == 1) {
-                        notificationPending = 0;
+                        //notificationPending = 0;
                         Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_SENSORFUSION, SENDING_INTERVAL_100MS_MULTIPLE);
                     }
                 
@@ -707,9 +705,8 @@
  */
  void Change_Notification_Status(uint8_t *att_data, uint8_t  *attr_value, uint16_t conn_handle, uint8_t i,
                                  uint32_t feature_mask, uint8_t frequency) {
-    printf("\r\nChange_Notification_Status (status: %d) (read: %s) (write: %s)\n", perDevs.status, (readCompleted == 1 ? "completed" : "pending"),(writeDescriptorCompleted == 1 ? "completed" : "pending"));//DEBUG
-
-
+    //printf("\r\nChange_Notification_Status (status: %d) (read: %s) (write: %s)\n", perDevs.status, (readCompleted == 1 ? "completed" : "pending"),(writeDescriptorCompleted == 1 ? "completed" : "pending"));//DEBUG
+    
 
         uint8_t  value_len;
         uint16_t attr_handle;
@@ -766,46 +763,15 @@
             if (err != BLE_ERROR_NONE){
                 writeDescriptorCompleted=1;
                 printf("\r\nERROR Enabling/Disabling Notification (err: %d) (stat: %d)\n", err, perDevs.status);
-//                HAL_Delay(500);
-//                //retrying
-//                err = writeCharacDescriptorWithError(conn_handle, attr_handle, value_len, attr_value);
-//                if (err != BLE_ERROR_NONE){
-//                    writeDescriptorCompleted=1;
-//                    printf("\r\nERROR Enabling/Disabling Notification (err: %d) (stat: %d)\n", err, perDevs.status);
-//                }
+                
             }
 
-//
-//        
-//        // discovery not completed 
-//        }else {
-//            ble_error_t err;
-//        
-//            //waiting for scan stopping
-//            HAL_Delay(3000);
-//            writeDescriptorCompleted=0;
-//
-//            //Send the enable/disable
-//            err = writeCharacDescriptorWithError(conn_handle, attr_handle, value_len, attr_value);
-//
-//            if (err != BLE_ERROR_NONE){
-//                //printf("\r\nWrite charac descriptor failed! (%d)\n", err);
-//                HAL_Delay(500);
-//                //retrying
-//                err = writeCharacDescriptorWithError(conn_handle, attr_handle, value_len, attr_value);
-//                if (err != BLE_ERROR_NONE){
-//                    writeDescriptorCompleted=1;
-//                    printf("\r\nERROR Enabling/Disabling Notification (%d)\n", err);
-//                }
-//            }
-//        }//if-else-discovery-completed
-        
 
 
 
     
     /* WUP notification enable/disable */
-//    if(attr_value[0]==0x00){
+    if(attr_value[0]==0x00){
 //        for (uint8_t j=0; j<(perDevs.connDevices+perDevs.discDevices); j++) {
 //            if (!perDevs.wup_event[j] && perDevs.devInfo[j].dev_v == NODE_ME1 && perDevs.is_connected[j]) {
 //                attr_handle = perDevs.wup_char_handle[j] + 2;
@@ -837,38 +803,38 @@
 //        if (perDevs.wup_event_enabled == 1)
 //            //printf("\r\nAll WUP notifications turned ON\n");//DEBUG
 //        perDevs.status = ALL_DATA_READ;
-//
-//
-//    } else{
-//        for (uint8_t j=0; j<(perDevs.connDevices+perDevs.discDevices); j++) {
-//            if ((perDevs.wup_event[j]) && (perDevs.devInfo[j].dev_v == NODE_ME1) && (perDevs.is_connected[j])) {
-//                attr_handle = perDevs.wup_char_handle[j] + 2;
-//                attr_value[0] = 0x00;
-//
-//
-//                if (discoveryCompleted == 1){
-//                    
-//                    writeDescriptorCompleted=0;
-//                    ble_error_t wupErrOff = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);
-//
-//                    if (wupErrOff == BLE_ERROR_NONE){
-//                        perDevs.wup_event_enabled = 0;
-//                        perDevs.wup_event[j] = 0;
-//                        printf("\r\nWUP notification on node [%d] OFF\n", j);
-//
-//                    } else {         
-//                        printf("\r\nWrite WUP char descriptor failed! (%d)\n", wupErrOff);
-//
-//                    }
-//                    writeDescriptorCompleted=1;
-//                    
-//                }//if-discovery-completed
-//            }//if
-//        }//for
-//        
-//        perDevs.status = NOTIFICATIONS_DATA_READ;
-//
-//    }//if-else-wup
+
+
+    } else{
+        for (uint8_t j=0; j<(perDevs.connDevices+perDevs.discDevices); j++) {
+            if ((perDevs.wup_event[j]) && (perDevs.devInfo[j].dev_v == NODE_ME1) && (perDevs.is_connected[j])) {
+                attr_handle = perDevs.wup_char_handle[j] + 2;
+                attr_value[0] = 0x00;
+
+
+                if (discoveryCompleted == 1){
+                    
+                    writeDescriptorCompleted=0;
+                    ble_error_t wupErrOff = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);
+
+                    if (wupErrOff == BLE_ERROR_NONE){
+                        perDevs.wup_event_enabled = 0;
+                        perDevs.wup_event[j] = 0;
+                        printf("\r\nWUP notification on node [%d] OFF\n", j);
+
+                    } else {         
+                        printf("\r\nWrite WUP char descriptor failed! (%d)\n", wupErrOff);
+
+                    }
+                    writeDescriptorCompleted=1;
+                    
+                }//if-discovery-completed
+            }//if
+        }//for
+        
+        perDevs.status = NOTIFICATIONS_DATA_READ;
+
+    }//if-else-wup
 
         attr_value[0] = att_data[3];
         perDevs.readDeviceIdx = i;