Diff: source/BleSlaveService.cpp

Revision:

2:1525f4848cf2

Parent:

1:110b5e896bc9

Child:

3:3f35e80ed848

--- 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{