Revision 2:1525f4848cf2, committed 2018-02-26

Comitter:

lorevee

Date:

Mon Feb 26 12:29:03 2018 +0000

Parent:

1:110b5e896bc9

Child:

3:3f35e80ed848

Commit message:

Ble Star release 1.0

Changed in this revision

--- a/inc/BleMasterService.h	Mon Feb 26 10:10:37 2018 +0000
+++ b/inc/BleMasterService.h	Mon Feb 26 12:29:03 2018 +0000
@@ -257,7 +257,6 @@
   NOTIFICATIONS_DATA_READ,                     //44
   DISABLE_NOTIFICATIONS,                       //45
   READING_DISCONNECTION,                       //46
-  CHANGE_NOTIFICATION_STATUS                   //47
 } Status_t;
 
 
@@ -553,7 +552,7 @@
 /* Method called when there is a notification from the sever */
 void onNotificationCallback                     (const GattHVXCallbackParams* event);
 
-
+void scanTimeOut                                (void);
 /*----------------------------------------------------------------------------*/
 
 

--- a/source/BleMasterService.cpp	Mon Feb 26 10:10:37 2018 +0000
+++ b/source/BleMasterService.cpp	Mon Feb 26 12:29:03 2018 +0000
@@ -210,15 +210,13 @@
 
     if ( perDevs.status == CONN_INIT ) {
         if ( (perDevs.connDevices < MAX_NUM_OF_NODES) && (perDevs.discovery_enabled) ) {
-         //printf("\r\nIF 1\n");//DEBUG
             /* Start discovery of new peripheral nodes and connect them */
             //startDiscovery();
             eventQ.call(startDiscovery);
         }
         else {
             perDevs.status = DEVICE_CONNECTED;
-            //printf("\r\nstatus: DEVICE_CONNECTED\n");//DEBUG
-        }
+         }
 
     }//if
 
@@ -228,15 +226,12 @@
 
 
     if ( perDevs.status == DEVICE_DISCOVERY_COMPLETE ) {
-        //printf("\r\nDEVICE_DISCOVERY_COMPLETE\n");//DEBUG
         if ( perDevs.device_found == true ) {
             /* Establishing connection with a peripheral device */
             perDevs.status = START_DEVICE_CONNECTION;
-            //printf("\r\n\nstatus: START_DEVICE_CONNECTION\n");//DEBUG
             connectPeripheral();
         }else {
             perDevs.status = DEVICE_NOT_FOUND;
-            //printf("\r\n\nstatus: DEVICE_NOT_FOUND (%d)\n", DEVICE_NOT_FOUND);//DEBUG
         }
     }//if
 
@@ -247,19 +242,15 @@
 
     // if all devices are connected or no devices are discovered start reading
     if ((perDevs.status == DEVICE_CONNECTED) || (perDevs.status == DEVICE_NOT_FOUND) ){
-        //printf("\r\nDEVICE_CONNECTED or DEVICE_NOT_FOUND\n");//DEBUG
-
-
+     
         if (perDevs.device_found == true) {
             perDevs.status = DISCOVERABLE_MODE_SET;
             perDevs.device_found = false;
         }
 
-        //perDevs.device_found == false -- start reading
         else {
             perDevs.readDeviceIdx = perDevs.connDevices+perDevs.discDevices-1;
             perDevs.status = READ_INIT;
-            //printf("\r\nstatus: READ_INIT (%u)\n", READ_INIT);
         }
 
     }//if-main
@@ -301,7 +292,6 @@
             perDevs.is_connected[perDevs.connDeviceIdx] = true;
             perDevs.connDeviceIdx++;
             perDevs.connDevices++;
-            //printf("\r\nConnected Devices %d\n", perDevs.connDevices);//DEBUG
         }
 
         perDevs.readDeviceIdx = perDevs.connDevices-1;
@@ -314,7 +304,7 @@
 
     /* Start reading process */
     eventQ.call(readingProcess);
-    //eventQ.call(connectionProcess);//DEBUG_ONLY /* Call in loop coonnectionProcess */
+  
 }
 /*----------------------------------------------------------------------------*/
 
@@ -396,8 +386,7 @@
 
     //if-MAIN
     if (handle == perDevs.connection_handle[i]){
-        //printf("\r\n\nif (handle == perDevs.connection_handle[i]) - BleMasterService.cpp\n\n");//DEBUG
-
+     
         //Delete its DCList
         deleteDCNList(headCharacteristic[i]);
         headCharacteristic[i] = NULL;
@@ -415,8 +404,7 @@
 
 
             if (perDevs.is_connected[i] == true){
-                //printf("\r\nperDevs.is_connected[i] = true - connDevices: %d\n", perDevs.connDevices);//DEBUG
-
+             
                 perDevs.connDevices--;
                 //disconnected_devices
                 perDevs.discDevices++;
@@ -428,11 +416,9 @@
 
             readCharacteristicCallback(nullGattReadCallbackP(perDevs.connection_handle[i]));
 
-            if (writeDescriptorCompleted == 1){
-                writeDescriptorCompleted =0;
-                notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
-                         slaveDev.notification_data.attribute_handle);    
-            }
+
+            notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
+                         slaveDev.notification_data.attribute_handle);
             perDevs.status = CONN_INIT;
 
 
@@ -599,8 +585,6 @@
       perDevs.status = DEVICE_DISCOVERY_COMPLETE;
       printf("\r\nDevice Discovery Complete (%d)\n", perDevs.status);
 
-      //HAL_Delay(100);
-
       discoveryCompleted = 1;
 
 }
@@ -649,9 +633,11 @@
     if ( e1 == BLE_ERROR_NONE ) {
         //printf("\r\nScan started correctly\n");//DEBUG
     }else {
-        //printf("\r\nERROR starting scan (err: %d stat: %d)\n", e1, perDevs.status);//DEBUG
-        perDevs.status = DEVICE_DISCOVERY_COMPLETE;
-        discoveryCompleted = 1;
+        //perDevs.status = DEVICE_DISCOVERY_COMPLETE;
+        //discoveryCompleted = 1;
+        
+        /* Set the same timeout */
+        eventQ.call_in(3000, scanTimeOut);
     }
 
  }//if-MAIN
@@ -659,6 +645,21 @@
 /*----------------------------------------------------------------------------*/
 
 
+void scanTimeOut(void){
+    if (perDevs.status == DEVICE_FOUND) {
+        perDevs.device_found = true;
+      }
+    else {
+        perDevs.device_found = false;
+    }
+    perDevs.status = DEVICE_DISCOVERY_COMPLETE;
+    printf("\r\nDevice Discovery Complete (%d)\n", perDevs.status);
+
+    discoveryCompleted = 1;
+}
+/*----------------------------------------------------------------------------*/
+
+
 
 
 /* Function called as response of advertising */
@@ -711,9 +712,7 @@
 
           // M O T E N V
             if (((value_length == sizeof(NODE_MOTENV)-1) && (memcmp(value, NODE_MOTENV, value_length) == 0))) {
-                //printf("\r(If statement 1)\n\n");//DEBUG
-
-
+            
 
                 //initial per data
                 uint8_t peripheral_v;
@@ -787,10 +786,7 @@
 
           // F L I G H T
             if ((value_length == sizeof(NODE_FLIGHT)-1) && (memcmp(value, NODE_FLIGHT, value_length) == 0)) {
-                //printf("\r(If statement 2)\n\n");//DEBUG
-
-
-
+    
                 //initial per data
                 uint8_t peripheral_v;
                 uint8_t wup_event = 0;
@@ -865,8 +861,6 @@
 
           // A L L M E M S
              if ((value_length == sizeof(NODE_ALLMEMS)-1) && (memcmp(value, NODE_ALLMEMS, value_length) == 0)){
-               //printf("\r(If statement 3)\n\n");//DEBUG
-
 
 
                 //initial per data
@@ -893,11 +887,8 @@
 
                 for (i=0; i<MAX_NUM_OF_NODES; i++) {
                     if (perDevs.is_disconnected[i]) {
-                      //printf("\r\n\n  if(perDevs.is_disconnected[i])\n\n");//DEBUG
                       if (memcmp(zeroAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
-                        //printf("\r\n\n  if(memcmp(zeroAddr, perDevs.devInfo[i].bdaddr, 6)==0)\n\n");//DEBUG
                         if (!found_zero_pos) {
-                          //printf("\r\n\n  if(!found_zero_pos)\n\n");//DEBUG
                           index = i;
                           perDevs.connDeviceIdx = i;
                           found_zero_pos = 1;
@@ -905,7 +896,6 @@
                         }
                       }
                       else if (memcmp(tempAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
-                        //printf("\r\n\n  else if(memcmp(tempAddr, perDevs.devInfo[i].bdaddr, 6)==0)\n\n");//DEBUG
                         index = i;
                         perDevs.connDeviceIdx = i;
                         alreadyIn = 0;
@@ -913,9 +903,7 @@
                         break;
                       }
                       else {
-                        //printf("\r\n\n  else - ROW 514\n\n");//DEBUG
                         if (!found_zero_pos) {
-                          //printf("\r\n\n  if(!found_zero_pos)\n\n");//DEBUG
                           index = i;
                           perDevs.connDeviceIdx = i;
                           alreadyIn = 0;
@@ -1128,8 +1116,7 @@
 
   //-----> index of current device
   uint8_t  idx = perDevs.connDeviceIdx;
-  //printf("\r\nconnDeviceIdx: %d\n", idx);//DEBUG
-
+ 
   //Prepend the DiscoveredCharacteristic into the list
   headCharacteristic[idx] = prependDCNode(headCharacteristic[idx], characteristicP);
 
@@ -1161,9 +1148,7 @@
 
         //HW
         perDevs.status = HARDWARE_SERV_CHARS_DISCOVERED;
-        //printf("\r\nstatus = HARDWARE_SERV_CHARS_DISCOVERED\n\n");
-
-
+      
         // Set the handle
         perDevs.environmental_char_handle[idx] = declHandle;
 
@@ -1184,8 +1169,6 @@
 
         //HW
         perDevs.status = HARDWARE_SERV_CHARS_DISCOVERED;
-        //printf("\r\nstatus = HARDWARE_SERV_CHARS_DISCOVERED\n\n");
-
         // Set the handle
         perDevs.agm_char_handle[idx] = declHandle;
 
@@ -1207,8 +1190,7 @@
 
         //SW
         perDevs.status = SOFTWARE_SERV_CHARS_DISCOVERED;
-        //printf("\r\nstatus = SOFTWARE_SERV_CHARS_DISCOVERED\n\n");
-
+  
         //Set the handle
         perDevs.sfusion_char_handle[idx] = declHandle;
         //Init value
@@ -1233,8 +1215,7 @@
 
         //HW
         perDevs.status = HARDWARE_SERV_CHARS_DISCOVERED;
-        //printf("\r\nstatus = HARDWARE_SERV_CHARS_DISCOVERED\n\n");
-
+      
         // Set the handle
         perDevs.led_char_handle[idx] = declHandle;
         // Init value
@@ -1254,8 +1235,7 @@
 
         //HW
         perDevs.status = HARDWARE_SERV_CHARS_DISCOVERED;
-        //printf("\r\nstatus = HARDWARE_SERV_CHARS_DISCOVERED\n\n");
-
+   
         // Set the handle
         perDevs.wup_char_handle[idx] = declHandle;
         // Init value
@@ -1276,8 +1256,7 @@
 
         //HW
         perDevs.status = HARDWARE_SERV_CHARS_DISCOVERED;
-        //printf("\r\nstatus = HARDWARE_SERV_CHARS_DISCOVERED\n\n");
-
+   
         // Set the handle
         perDevs.mic_char_handle[idx] = declHandle;
         // Init value
@@ -1297,8 +1276,7 @@
 
         //HW
         perDevs.status = HARDWARE_SERV_CHARS_DISCOVERED;
-        //printf("\r\nstatus = HARDWARE_SERV_CHARS_DISCOVERED\n\n");
-
+    
         // Set the handle
         perDevs.prx_char_handle[idx] = declHandle;
         // Init value
@@ -1320,8 +1298,7 @@
 
         //HW
         perDevs.status = HARDWARE_SERV_CHARS_DISCOVERED;
-        //printf("\r\nstatus = HARDWARE_SERV_CHARS_DISCOVERED\n\n");
-
+    
         // Set the handle
         perDevs.lux_char_handle[idx] = declHandle;
 
@@ -1342,8 +1319,7 @@
 
         //CFG
         perDevs.status = CONFIGURATION_SERV_CHARS_DISCOVERED;
-        //printf("\r\nstatus = CONFIGURATION_SERV_CHARS_DISCOVERED\n\n");
-
+   
 
         // Set the handle
         perDevs.cfg_char_handle[idx] = declHandle;
@@ -1472,7 +1448,6 @@
             perDevs.status = ALL_DATA_READ;
            
             if ((stackBusy == 0) && (writeDescriptorCompleted ==1)){
-                writeDescriptorCompleted=0;
                 notifyMaster(ATTR_HEAD_LEN+WUP_DATA_LEN, star_attr_value, slave_attr_handle);
             }else {
                 //disableAllNotifications();
@@ -1507,8 +1482,6 @@
         if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
             perDevs.status = ALL_DATA_READ;
             if ((stackBusy == 0) && (writeDescriptorCompleted ==1)){
-                
-                writeDescriptorCompleted = 0;
                 notifyMaster(ATTR_HEAD_LEN+MIC_DATA_LEN, star_attr_value, slave_attr_handle);
             }else {
                 //disableAllNotifications();
@@ -1536,7 +1509,6 @@
         if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
             perDevs.status = ALL_DATA_READ;
             if ((stackBusy == 0) && (writeDescriptorCompleted ==1)){
-                writeDescriptorCompleted=0;
                 notifyMaster(ATTR_HEAD_LEN+PRX_DATA_LEN, star_attr_value, slave_attr_handle);
             }else {
                 //disableAllNotifications();
@@ -1584,7 +1556,6 @@
         if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) && (perDevs.agm_on[index])) {
             perDevs.status = NOTIFICATIONS_DATA_READ;
             if ((stackBusy == 0) && (writeDescriptorCompleted ==1)){
-                writeDescriptorCompleted=0;
                 notifyMaster(ATTR_HEAD_LEN+data_len, star_attr_value, slave_attr_handle);
             }else {
                 //disableAllNotifications();
@@ -1620,7 +1591,6 @@
         if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) && (perDevs.sfusion_on[index])) {
             perDevs.status = NOTIFICATIONS_DATA_READ;
             if ((stackBusy == 0) && (writeDescriptorCompleted ==1)){
-                writeDescriptorCompleted=0;
                 notifyMaster(ATTR_HEAD_LEN+SFUSION_DATA_LEN, star_attr_value, slave_attr_handle);
             }else {
                 //disableAllNotifications();
@@ -1660,7 +1630,6 @@
 
         if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
             if ((stackBusy == 0) && (writeDescriptorCompleted ==1)){
-                writeDescriptorCompleted=0;
                 notifyMaster(ATTR_HEAD_LEN+PRESS_DATA_LEN+(3*TYPE_ID_LEN)+HUM_DATA_LEN+(2*TEMP_DATA_LEN),
                         star_attr_value, slave_attr_handle);
             }else {
@@ -1691,7 +1660,6 @@
                 perDevs.status = ALL_DATA_READ;
             }
             if ((stackBusy == 0) && (writeDescriptorCompleted ==1)){
-                writeDescriptorCompleted=0;
                 notifyMaster(ATTR_HEAD_LEN+LED_DATA_LEN, star_attr_value, slave_attr_handle);
             }else {
                 //disableAllNotifications();
@@ -1718,7 +1686,6 @@
 
         if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
             if ((stackBusy == 0) && (writeDescriptorCompleted ==1)){
-                writeDescriptorCompleted=0;
                 notifyMaster(ATTR_HEAD_LEN+LUX_DATA_LEN, star_attr_value, slave_attr_handle);
             }else {
                 //disableAllNotifications();
@@ -2448,10 +2415,8 @@
     if ((perDevs.status == READ_INIT)) {
         if (perDevs.connDevices > 0) {
             slaveDev.is_discoverable = true;
-            //printf("\r\nperDevs.connDevices > 0 (i=%d)\n", i);//DEBUG
             perDevs.status = ALL_DATA_READ;
             if (!perDevs.is_disconnected[i]) {
-                //printf("\r\n!perDevs.is_disconnected\n");//DEBUG
                 switch (perDevs.devInfo[i].dev_v) {
                     case NODE_ME1:
                     case NODE_AM1:
@@ -2483,19 +2448,16 @@
 
                                 value_len = 2;
                                 attr_value[0] = DISABLE;
-                                
-                                
-                                if (writeDescriptorCompleted == 1){
-                                    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
-                                } 
-                                
+        
+                                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
                         
@@ -2508,7 +2470,6 @@
                 }
             }
         } else {
-            //printf("\r\nconnDevices: %d - status = CONN_INIT - ELSE readingProces\n", perDevs.connDevices);//DEBUG
             perDevs.status = CONN_INIT;
         }
     }//if-READ_INIT
@@ -2540,8 +2501,6 @@
 
     //NOTIFY_ENV_TO_CLIENT
     if ((perDevs.status == NOTIFY_ENV_TO_CLIENT) && (readCompleted == 1) && (writeDescriptorCompleted ==1)) {
-        //printf("\r\nNOTIFY_ENV_TO_CLIENT\n");//DEBUG
-        writeDescriptorCompleted = 0;
         notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
                      slaveDev.notification_data.attribute_handle);
         eventQ.call(setNewStatus);
@@ -2572,7 +2531,6 @@
 
     //NOTIFY_LED_TO_CLIENT
     if ((perDevs.status == NOTIFY_LED_TO_CLIENT) && (readCompleted == 1) && (writeDescriptorCompleted ==1)) {
-        writeDescriptorCompleted = 0;
         notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
                      slaveDev.notification_data.attribute_handle);
         eventQ.call(setNewStatus);
@@ -2602,7 +2560,6 @@
 
     //NOTIFY_LUX_TO_CLIENT
     if ((perDevs.status == NOTIFY_LUX_TO_CLIENT) && (readCompleted == 1) && (writeDescriptorCompleted ==1)) {
-        writeDescriptorCompleted = 0;
         notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
                      slaveDev.notification_data.attribute_handle);
         eventQ.call(setNewStatus);
@@ -2623,8 +2580,6 @@
         readCharacteristicCallback(nullGattReadCallbackP(connHandle));
 
         perDevs.status = NOTIFY_MIC_TO_CLIENT;
-        
-        writeDescriptorCompleted=0;
         notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
                      slaveDev.notification_data.attribute_handle);
         eventQ.call(setNewStatus);
@@ -2644,7 +2599,6 @@
         readCharacteristicCallback(nullGattReadCallbackP(connHandle));
 
         perDevs.status = NOTIFY_PRX_TO_CLIENT;
-        writeDescriptorCompleted =0;
         notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
                      slaveDev.notification_data.attribute_handle);
         eventQ.call(setNewStatus);
@@ -2665,7 +2619,6 @@
         readCharacteristicCallback(nullGattReadCallbackP(connHandle));
 
         perDevs.status = NOTIFY_AGM_TO_CLIENT;
-        writeDescriptorCompleted=0;
         notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
                      slaveDev.notification_data.attribute_handle);
         eventQ.call(setNewStatus);
@@ -2685,7 +2638,6 @@
         readCharacteristicCallback(nullGattReadCallbackP(connHandle));
 
         perDevs.status = NOTIFY_SFUSION_TO_CLIENT;
-        writeDescriptorCompleted=0;
         notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
                      slaveDev.notification_data.attribute_handle);
         eventQ.call(setNewStatus);
@@ -2701,14 +2653,11 @@
 
     //ALL_DATA_READ
     if ((perDevs.status == ALL_DATA_READ) && (readCompleted == 1) && (writeDescriptorCompleted ==1)) {
-        //printf("\r\nstatus: ALL_DATA_READ (%d)\n", perDevs.status);//DEBUG
         if (i>0) {
             perDevs.readDeviceIdx--;
         }
         perDevs.status = READ_INIT;
-        //printf("\r\nstatus: READ_INIT (%d) - readingProcess\n", perDevs.status);//DEBUG
-
-
+      
         // All peripherals are read!
         if (i==0) {
 
@@ -2834,7 +2783,6 @@
             if ((perDevs.devInfo[i].dev_v == NODE_FL1) && (perDevs.prx_on[i]==0)) {
                 perDevs.status = READING_LUX;
             }else {
-                //printf("\r\nstatus = ALL_DATA_READ (setNewStatus)\n");
                 perDevs.status = ALL_DATA_READ;
             }
         }//if-MAIN
@@ -2858,7 +2806,6 @@
             }
             else if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) &&
                (perDevs.prx_event[i]) && (!perDevs.prx_char_read[i])) {
-                //printf("\r\n----->if\n");//DEBUG
                 perDevs.status = READING_PRX;
             }
 
@@ -2983,10 +2930,8 @@
 
     //
     case NOTIFY_LUX_TO_CLIENT:
-        //printf("NOTIFY_LUX_TO_CLIENT");//DEBUG
         if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) &&
             (perDevs.prx_event[i]) && (!perDevs.prx_char_read[i])) {
-            //printf("IF-LUX");//DEBUG
             perDevs.status = READING_PRX;
         }else {
             perDevs.status = ALL_DATA_READ;

--- a/source/BleSlaveService.cpp	Mon Feb 26 10:10:37 2018 +0000
+++ b/source/BleSlaveService.cpp	Mon Feb 26 12:29:03 2018 +0000
@@ -52,7 +52,6 @@
 /*----------------------------------------------------------------------------*/
 
 
-
 /* Set the device as a slave in discoverable mode */
 void setSlaveDiscoverable(void) {
     //printf("\r\nsetSlaveDiscoverable\n");//DEBUG
@@ -64,7 +63,8 @@
     if ((discoveryCompleted == 1) /*&& (readCompleted == 1) && (writeDescriptorCompleted == 1)*/){
 
 
-
+        /* !! Solve advertising after master disconnection */
+        
         /* Start advertisng */
         if ((!ble.gap().getState().advertising)){
             advEnds =0;
@@ -140,8 +140,7 @@
     attr_value[4] = command;
     attr_value[5] = data;
     value_len = 6;  /* FEATURE_MASK len (4bytes) + Command len (1byte) + Data len (1byte) */
-
-
+        
     /* The gatt write function above is w/out response, so we just wait for a short time before going on */
     writeCharacValueWithoutResp(conn_handle, attr_handle, value_len, attr_value);
     HAL_Delay(2000);//N ms
@@ -166,7 +165,6 @@
                 return;
             }
         }else {
-                writeDescriptorCompleted =1;
                 stackBusy = 0;
                 return;
         }
@@ -264,8 +262,6 @@
         if (disableErr != BLE_ERROR_NONE){
             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
 }
@@ -300,7 +296,7 @@
 
     /* Notify master for scanning */
 
-/* Stack busy, solve this */
+/* Check the scan enable/disable */
 //    notifyMaster(slaveDev.star_config_value_len, slaveDev.star_config_value,
 //                 slaveDev.star_config_char_handle);
 
@@ -504,7 +500,6 @@
 
             //MICLEVEL_TYPE_ID
             case MICLEVEL_TYPE_ID:
-            //perDevs.status = CHANGE_NOTIFICATION_STATUS;//47
             
                 if (readCompleted == 0){
                     notificationPending = 1;
@@ -542,7 +537,6 @@
 
             //PRX_TYPE_ID
             case PRX_TYPE_ID:
-                //perDevs.status = CHANGE_NOTIFICATION_STATUS;//47
             
                 if (readCompleted == 0){
                     notificationPending = 1;    
@@ -582,7 +576,6 @@
             case ACC_TYPE_ID:
             case GYR_TYPE_ID:
             case MAG_TYPE_ID:
-                //perDevs.status = CHANGE_NOTIFICATION_STATUS;//47
                 
                 if (readCompleted == 0){
                     notificationPending = 1;        
@@ -593,14 +586,14 @@
                     notifyP->conn_handle  = conn_handle;
                     notifyP->i            = i;
                     notifyP->feature_mask = FEATURE_MASK_ACC;
-                    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_ACC, SENDING_INTERVAL_100MS_MULTIPLE);
+                    Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_ACC, notification_freq*SENDING_INTERVAL_100MS_MULTIPLE);
                 }
                 perDevs.agm_on[i] = attr_value[0];
 
@@ -632,7 +625,6 @@
 
             //SFUSION_TYPE_ID
             case SFUSION_TYPE_ID:
-                //perDevs.status = CHANGE_NOTIFICATION_STATUS;//47
                 
                     if (readCompleted == 0){
                         notificationPending = 1;        
@@ -643,13 +635,13 @@
                         notifyP->conn_handle  = conn_handle;
                         notifyP->i            = i;
                         notifyP->feature_mask = FEATURE_MASK_SENSORFUSION;
-                        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_SENSORFUSION, SENDING_INTERVAL_100MS_MULTIPLE);
+                        Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_SENSORFUSION, notification_freq*SENDING_INTERVAL_100MS_MULTIPLE);
                     }
                 
                     perDevs.sfusion_on[i] = attr_value[0];
@@ -772,37 +764,37 @@
     
     /* WUP notification enable/disable */
     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;
-//                attr_value[0] = 0x01;
-//    
-//                setNotificationProperty(perDevs.connection_handle[j], j, FEATURE_MASK_WAKEUP_EVENTS, WAKEUP_NOTIFICATION_CMD, 1);
-//
-//                if (discoveryCompleted == 1){
-//                    
-//                    writeDescriptorCompleted=0;
-//                    ble_error_t wupErrOn = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);
-//
-//                    if (wupErrOn == BLE_ERROR_NONE){
-//                        perDevs.wup_event_enabled = 1;
-//                        perDevs.wup_event[j] = 1;
-//                        printf("\r\nWUP notification on node [%d] ON\n", j);
-//                    
-//                    } else {
-//                        printf("\r\nWrite WUP char descriptor failed! (%d)\n", wupErrOn);
-//
-//                    }//if-else
-//                    writeDescriptorCompleted=1;
-//                                           
-//                }//if-else-discovery-completed
-//            }//if
-//        }//for
-//
-//
-//        if (perDevs.wup_event_enabled == 1)
-//            //printf("\r\nAll WUP notifications turned ON\n");//DEBUG
-//        perDevs.status = ALL_DATA_READ;
+        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] = 0x01;
+    
+                setNotificationProperty(perDevs.connection_handle[j], j, FEATURE_MASK_WAKEUP_EVENTS, WAKEUP_NOTIFICATION_CMD, 1);
+
+                if (discoveryCompleted == 1){
+                    
+                    writeDescriptorCompleted=0;
+                    ble_error_t wupErrOn = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);
+
+                    if (wupErrOn == BLE_ERROR_NONE){
+                        perDevs.wup_event_enabled = 1;
+                        perDevs.wup_event[j] = 1;
+                        printf("\r\nWUP notification on node [%d] ON\n", j);
+                    
+                    } else {
+                        printf("\r\nWrite WUP char descriptor failed! (%d)\n", wupErrOn);
+
+                    }//if-else
+                    writeDescriptorCompleted=1;
+                                           
+                }//if-else-discovery-completed
+            }//if
+        }//for
+
+
+        if (perDevs.wup_event_enabled == 1)
+            //printf("\r\nAll WUP notifications turned ON\n");//DEBUG
+        perDevs.status = ALL_DATA_READ;
 
 
     } else{