BleSlaveService.cpp

00001 #include <BleSlaveService.h>
00002 
00003 
00004 /* ----- Private variables ----- */
00005 /* Struct of slave dev */
00006 SlaveDevice_t slaveDev;
00007 
00008 
00009 ChangeNotificationQueue notifyQ, *notifyP;
00010 
00011 
00012 /* Struct of periph dev */
00013 extern PeripheralDevices_t perDevs;
00014 
00015 /* Notification frequency */
00016 uint8_t notification_freq = NOTIFICATION_FREQ_WIFI_OFF;
00017 
00018 
00019 /* Star Service and Characteristic */
00020 uint8_t sUuid[16]    = {0x00,0x00,0x00,0x00,0x00,0x01,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
00021 uint8_t dataUuid[16] = {0x00,0x08,0x00,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
00022 uint8_t confUuid[16] = {0x00,0x04,0x00,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
00023 
00024 
00025 /*---- Create the two characteristic ----*/
00026 /* Config char */
00027 GattCharacteristic configChar(confUuid, slaveDev.star_config_value, STAR_CHAR_MAX_VALUE_LEN, STAR_CHAR_MAX_VALUE_LEN,
00028                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE,
00029                NULL, 0, true);
00030 
00031 
00032 /* Data char */
00033 GattCharacteristic dataChar(dataUuid, slaveDev.notification_data.attribute_value, STAR_CHAR_MAX_VALUE_LEN, STAR_CHAR_MAX_VALUE_LEN,
00034              GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE,
00035              NULL, 0, true);
00036 
00037 
00038 /* MANUFACTURER_SPECIFIC_DATA */
00039     uint8_t manuf_data[6] = {
00040                 0x01, /*SKD version */
00041                 0x81, /* NUCLEO-Board remote */
00042                 0x00, /* Led Prox+Lux */ //Unused
00043                 0x0C, /* Star Config Char + Star Data Char */ //Unused
00044                 0x00, /* SensorFusionShort */ //Unused
00045                 0x00 /* SensorFusionFloat */ //Unused
00046     };
00047 
00048 
00049 
00050 
00051 
00052 /*----------------------------------------------------------------------------*/
00053 
00054 
00055 /* Set the device as a slave in discoverable mode */
00056 void setSlaveDiscoverable(void) {
00057     //printf("\r\nsetSlaveDiscoverable\n");//DEBUG
00058     BLE& ble = BLE::Instance();
00059     ble_error_t e2;
00060 
00061 
00062     //if-MAIN - discovery/reading/writing ops completed
00063     if ((discoveryCompleted == 1) /*&& (readCompleted == 1) && (writeDescriptorCompleted == 1)*/){
00064 
00065 
00066         /* !! Solve advertising after master disconnection */
00067         
00068         /* Start advertisng */
00069         if ((!ble.gap().getState().advertising)){
00070             advEnds =0;
00071 
00072             printf("\r\nSet Discoverable Mode (%d)\n", perDevs.status);
00073             e2 = ble.gap().startAdvertising();
00074             if ( e2 == BLE_ERROR_NONE ){
00075                 /* Giving some time to connect */
00076                 eventQ.call_in(7000, stopAdv);//N ms
00077             }else{
00078                 printf("\r\nError while starting advertising (err: %d - stat: %d)\n", e2, perDevs.status);
00079                 advEnds=1;//terminated because not started
00080             }
00081 
00082         }//if-not-adv
00083     }//if-MAIN
00084 
00085 
00086     //Restart loop ops
00087     //perDevs.status = CONN_INIT;
00088     //perDevs.discovery_enabled = true;
00089 
00090 }
00091 /*----------------------------------------------------------------------------*/
00092 
00093 
00094 
00095 void stopAdv(){
00096 
00097     if ((slaveDev.is_connected == false) && (discoveryCompleted == 1)){
00098         if (BLE::Instance().gap().getState().advertising){
00099             ble_error_t e = BLE::Instance().gap().stopAdvertising();
00100             if ( e != BLE_ERROR_NONE ) {
00101                 printf("\r\nError stopping ADV\n");//DEBUG
00102             }
00103         }
00104     }//if
00105 
00106     advEnds = 1;
00107 }
00108 /*----------------------------------------------------------------------------*/
00109 
00110 
00111 
00112 /**
00113  * @brief  This function sets some notification properties (e.g. the
00114  *         notification period) by sending a command according to the
00115  *         following format:
00116  *         FEATURE_MASK (4bytes) + Command (1byte) + Data (1byte)
00117  *         In case of the notification period the command is 255 while the allowed data are:
00118  *         50 @5S, 10 @1S, 1 @100mS, 0 @50mS (default).
00119  * @param  connection handle
00120  * @param  peripheral device index
00121  * @param  feature mask
00122  * @param  command
00123  * @param  data
00124  * @retval None
00125  */
00126 void setNotificationProperty(uint16_t conn_handle, uint8_t i, uint32_t feature_mask,
00127                              uint8_t command, uint8_t data) {
00128     //printf("\r\nsetNotificationProperty\n");//DEBUG
00129 
00130     uint16_t attr_handle;
00131     uint8_t  value_len;
00132     uint8_t  attr_value[6];
00133 
00134     attr_handle = perDevs.cfg_char_handle[i] + 1;
00135     attr_value[0] = (uint8_t)((feature_mask) >> 24);
00136     attr_value[1] = (uint8_t)((feature_mask) >> 16);
00137     attr_value[2] = (uint8_t)((feature_mask) >> 8);
00138     attr_value[3] = (uint8_t)((feature_mask));
00139     attr_value[4] = command;
00140     attr_value[5] = data;
00141     value_len = 6;  /* FEATURE_MASK len (4bytes) + Command len (1byte) + Data len (1byte) */
00142         
00143     /* The gatt write function above is w/out response, so we just wait for a short time before going on */
00144     writeCharacValueWithoutResp(conn_handle, attr_handle, value_len, attr_value);
00145     HAL_Delay(1000);//N ms
00146 }
00147 /*----------------------------------------------------------------------------*/
00148 
00149 
00150 
00151 
00152 /* Forwards to master all notifications from peripherals */
00153 void notifyMaster(uint8_t data_length, uint8_t* attribute_value, uint16_t attribute_handle){
00154     //printf("\r\nnotifyMaster\n");//DEBUG
00155     ble_error_t notifyError;
00156 
00157         if (slaveDev.is_connected !=0)
00158             notifyError = BLE::Instance().gattServer().write(attribute_handle+1, attribute_value, data_length, false);
00159 
00160         if (notifyError != BLE_ERROR_NONE){
00161             printf("\r\n(notifyMaster) Error (%d) while updating characteristic\n", notifyError);//BLE_STACK_BUSY
00162             if (notifyError == BLE_STACK_BUSY){
00163                 stackBusy = 1;
00164                 return;
00165             }
00166         }else {
00167                 stackBusy = 0;
00168                 return;
00169         }
00170     
00171 }
00172 /*----------------------------------------------------------------------------*/
00173 
00174 
00175 /* Add all services using a vendor specific UUIDs */
00176 void addAllServices(void){
00177 
00178     ble_error_t error;
00179     notifyP = &notifyQ;
00180 
00181 
00182     GattCharacteristic *charTable[] = {&configChar, &dataChar};
00183 
00184     /*---- Create the hw service ----*/
00185     GattService starService(sUuid, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
00186 
00187     /*---- Add the hw service ----*/
00188     error = BLE::Instance().gattServer().addService(starService);
00189     slaveDev.star_hw_serv_handle     = starService.getHandle();
00190     slaveDev.star_config_char_handle = configChar.getValueHandle()-1;   //-1 for decl handle
00191     slaveDev.star_data_char_handle   = dataChar.getValueHandle()-1;     //-1 for decl handle
00192 
00193 
00194 
00195     if ( error != BLE_ERROR_NONE ){
00196         printf("\r\nError while creating starService(error: %d)\n", error);
00197 
00198     } else {
00199         printf("\r\nStar HW Gatt Service added (handle 0x%004x)\n", slaveDev.star_hw_serv_handle);
00200         printf("\rStar HW Config Char added (handle 0x%004x)\n", slaveDev.star_config_char_handle);
00201         printf("\rStar HW Data Char added (handle 0x%004x)", slaveDev.star_data_char_handle);
00202         printf("\r\n");
00203     }
00204 
00205 }
00206 /*----------------------------------------------------------------------------*/
00207 
00208 
00209 /* Disable all notifications */
00210 void disableAllNotifications(void){
00211     //printf("\r\ndisableAllNotifications\n");//DEBUG
00212 
00213     
00214     uint8_t j;
00215     uint16_t attr_handle;
00216     uint8_t  value_len;
00217     uint8_t  attr_value[2];
00218     char* feat = NULL;
00219 
00220     for (j=0; j<(perDevs.connDevices+perDevs.discDevices); j++) {
00221 
00222         if ((perDevs.mic_event[j]) && (perDevs.mic_event_enabled))  {
00223             feat = "MIC";
00224             perDevs.mic_event_enabled = 0;
00225             perDevs.mic_event[j] = 0;
00226             attr_handle = perDevs.mic_char_handle[j] + 2;
00227 
00228         } else if ((perDevs.prx_event[j]) && (perDevs.prx_event_enabled)) {
00229             feat = "PRX";
00230             perDevs.prx_event_enabled = 0;
00231             perDevs.prx_on[j] = 0;
00232             attr_handle = perDevs.prx_char_handle[j] + 2;
00233 
00234         } else if ((perDevs.agm_event[j]) && (perDevs.agm_event_enabled)) {
00235             feat = "AGM";
00236             perDevs.acc_event_enabled = 0;
00237             perDevs.gyr_event_enabled = 0;
00238             perDevs.mag_event_enabled = 0;
00239             perDevs.agm_event_enabled = 0;
00240             perDevs.agm_on[j] = 0;
00241             attr_handle = perDevs.agm_char_handle[j] + 2;
00242 
00243         } else if ((perDevs.sfusion_event[j]) && (perDevs.sfusion_event_enabled)) {
00244             feat = "SFUSION";
00245             perDevs.sfusion_event_enabled = 0;
00246             perDevs.sfusion_on[j] = 0;
00247             attr_handle = perDevs.sfusion_char_handle[j] + 2;
00248 
00249         } else {
00250             continue;
00251         }
00252 
00253         value_len = 2;
00254         attr_value[0] = DISABLE;
00255         
00256 
00257         writeDescriptorCompleted=0;
00258         ble_error_t disableErr = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);
00259         printf("\r\nSet OFF the %s notifications on node %d\n", feat, j);
00260 
00261         if (disableErr != BLE_ERROR_NONE){
00262             printf("\r\n(disableAllNotifications) Write charac descriptor failed (err: %d, stat: %d)\n", disableErr, perDevs.status);
00263             writeDescriptorCompleted=1;
00264         }//if-error
00265     }//for
00266 }
00267 /*----------------------------------------------------------------------------*/
00268 
00269 
00270 
00271 /* This function enables or disables the new peripheral scanning */
00272 void setNewNodesScanning(uint8_t enabled){
00273 
00274     STORE_LE_16(slaveDev.star_config_value,(HAL_GetTick()>>3)); /* Timestamp */
00275 
00276     switch (enabled) {
00277     case 0x01:
00278         perDevs.discovery_enabled = true;
00279 //        printf("\r\nScanning enabled (%d)\n", enabled);
00280     break;
00281 
00282     case 0x02:
00283         perDevs.discovery_enabled = false;
00284 //        printf("\r\nScanning disabled (%d)\n", enabled);
00285     break;
00286 
00287     default:
00288 //        printf("\r\nScanning set to unrecognized value (%d)\n", enabled);
00289     break;
00290     }
00291 
00292     slaveDev.star_config_value[2] = enabled;
00293     slaveDev.star_config_value_len = 3;
00294     //HAL_Delay(300);
00295 
00296     /* Notify master for scanning */
00297 
00298 /* Check the scan enable/disable */
00299     notifyMaster(slaveDev.star_config_value_len, slaveDev.star_config_value,
00300                  slaveDev.star_config_char_handle);
00301 
00302 }
00303 /*----------------------------------------------------------------------------*/
00304 
00305 
00306 //NOT USED FOR NOW !!
00307 /* This function forwards to the peripheral device the command coming from the Cloud */
00308 void Forward_Command_To_BlueNRG(uint8_t* data_ptr, uint8_t data_length){
00309 
00310     uint16_t handle;
00311     uint8_t  buffer[4] = {0, 0, 0, 0};
00312     uint8_t  num, even, odd;
00313     uint8_t  i;
00314     uint8_t cmd_type;
00315 
00316     cmd_type = (data_ptr[0] - '0') << 4 | (data_ptr[1] - '0');
00317     //printf("\r\ncommand %d\n", cmd_type);
00318 
00319   switch (cmd_type)
00320   {
00321   case 0:
00322     printf("\rScan cmd type (%d)\n", cmd_type);
00323     handle = slaveDev.star_config_char_handle + 1;
00324     break;
00325   case 1:
00326     printf("\rLED/MIC/PRX cmd type (%d)\n", cmd_type);
00327     handle = slaveDev.star_data_char_handle + 1;
00328     break;
00329   default:
00330     printf("\rUnknown cmd type (%d)\n", cmd_type);
00331     return;
00332     //break;
00333   }
00334 
00335   /**
00336   * For Scan cmd type:
00337   * |Value  |
00338   * |1 byte |
00339   * For LED/MIC/PRX cmd type:
00340   * |Node ID | Type ID | Value   |
00341   * |2 bytes | 1 byte  | x bytes |
00342   */
00343   for (i=2; i<data_length; i++) {
00344     switch (data_ptr[i])
00345     {
00346     case 'A':
00347     case 'a':
00348       num = 10;
00349       break;
00350     case 'B':
00351     case 'b':
00352       num = 11;
00353       break;
00354     case 'C':
00355     case 'c':
00356       num = 12;
00357       break;
00358     case 'D':
00359     case 'd':
00360       num = 13;
00361       break;
00362     case 'E':
00363     case 'e':
00364       num = 14;
00365       break;
00366     case 'F':
00367     case 'f':
00368       num = 15;
00369       break;
00370     default:
00371       num = data_ptr[i] - '0';
00372       break;
00373     }
00374     if (!(i%2)) {
00375       even = num << 4;
00376     }
00377     else {
00378       odd = num;
00379       buffer[(i-2)/2] = even | odd;
00380     }
00381   }
00382 
00383   createAttribute_Modified_CB_Prototype(handle, (data_length-2)/2, buffer);
00384 }
00385 /*----------------------------------------------------------------------------*/
00386 
00387 
00388 /* Function used to call AttributeModified_CB outside the callback call */
00389 void createAttribute_Modified_CB_Prototype(uint16_t handle, uint8_t data_length, uint8_t *att_data){
00390 
00391     GattWriteCallbackParams paramStruct, *pointerToParam;
00392     pointerToParam = &paramStruct;
00393 
00394     pointerToParam->handle = handle;
00395     pointerToParam->len = data_length;
00396     pointerToParam->data = att_data;
00397 
00398     AttributeModified_CB(pointerToParam);
00399 }
00400 /*----------------------------------------------------------------------------*/
00401 
00402 
00403 
00404 
00405 /* This function is called when a local attribute gets modified */
00406 void AttributeModified_CB(const GattWriteCallbackParams* param){
00407     //printf("\r\nAttributeModified_CB\n");//DEBUG
00408 
00409     /* Assign prototype vars */
00410     uint16_t handle     = param->handle;
00411     //uint8_t data_length = param->len; //not used
00412     uint8_t *att_data   = (uint8_t *)param->data;
00413 
00414 
00415     uint8_t  i;
00416     uint16_t conn_handle;
00417     uint16_t attr_handle;
00418     uint8_t  value_len;
00419     uint8_t  attr_value[6];
00420     char* mems;
00421 
00422     if (handle == slaveDev.star_config_char_handle + 1) {
00423 
00424         perDevs.status = ALL_DATA_READ;
00425         if (att_data[0]==0x01) {
00426             /* Disabling all previously enabled notifications */
00427             disableAllNotifications();
00428         }//if
00429         setNewNodesScanning(att_data[0]);
00430 
00431 
00432     } else if (handle == slaveDev.star_config_char_handle + 2) {
00433         if (att_data[0]==0x01) {
00434             //printf("\r\nEnable scanning notification\n");
00435             STORE_LE_16(slaveDev.star_config_value,(HAL_GetTick()>>3)); /* Timestamp */
00436             slaveDev.star_config_value[2] = perDevs.discovery_enabled;
00437             slaveDev.star_config_value_len = 3;
00438 
00439             /* send the status of the scanning when notifications are enabled */
00440             notifyMaster(slaveDev.star_config_value_len, slaveDev.star_config_value,
00441                          slaveDev.star_config_char_handle);
00442 
00443             /* enable here the data notifications since sometimes the relative command gets lost (FIXME) */
00444             //printf("\r\nEnable data notification (0)\n");
00445             slaveDev.star_data_char_notify = 1;
00446 
00447         } else if (att_data[0]==0x00) {
00448             //printf("\r\nDisable scanning notification\n");
00449         }
00450 
00451     } else if (handle == slaveDev.star_data_char_handle + 2) {
00452         if (att_data[0]==0x01) {
00453             //printf("\r\nEnable data notification (1)\n");
00454             slaveDev.star_data_char_notify = 1;
00455 
00456 
00457         } else if (att_data[0]==0x00) {
00458             //printf("\r\nDisable data notification\n");
00459             slaveDev.star_data_char_notify = 0;
00460 
00461         }
00462 
00463     } else if (handle == slaveDev.star_data_char_handle + 1) {
00464 
00465     /*
00466      * Forward the command to the proper peripheral node.
00467      * It can be:
00468      * - the ON/OFF of a LED
00469      * - the notification enabling/disabling for a MIC Level or a Proximity Sensor
00470      * The coming data are received according to the following format:
00471      * |Node ID | Type ID | value   |
00472      * |2 bytes | 1 byte  | x bytes |
00473      */
00474         
00475         i = Get_Device_Index_From_Addr(att_data);
00476         if ((i < MAX_NUM_OF_NODES) && (!perDevs.is_disconnected[i]) && (perDevs.cfg_char_handle[i] != 0)) {
00477             conn_handle = perDevs.connection_handle[i];
00478 
00479             switch (att_data[2]) {
00480 
00481 
00482             //LED_TYPE_ID
00483             case LED_TYPE_ID:
00484                 attr_handle = perDevs.cfg_char_handle[i] + 1;
00485                 value_len   = 5; /* 4 bytes for the feature mask + 1 byte for the command (0=ON, 1=OFF) */
00486                 attr_value[0] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS) >> 24);
00487                 attr_value[1] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS) >> 16);
00488                 attr_value[2] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS) >> 8);
00489                 attr_value[3] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS));
00490                 attr_value[4] = att_data[3];
00491 
00492                 writeCharacValueWithoutResp(conn_handle, attr_handle, value_len, attr_value);
00493 
00494                 perDevs.readDeviceIdx = i;
00495             break;
00496 
00497 
00498 
00499             //MICLEVEL_TYPE_ID
00500             case MICLEVEL_TYPE_ID:
00501             
00502                 if (readCompleted == 0){
00503                     notificationPending = 1;
00504                 
00505                     /* Fill the temp struct */
00506                     notifyP->att_data     = att_data;
00507                     notifyP->attr_value   = attr_value;
00508                     notifyP->conn_handle  = conn_handle;
00509                     notifyP->i            = i;
00510                     notifyP->feature_mask = FEATURE_MASK_MIC;
00511                     notifyP->frequency    = notification_freq;    
00512                 
00513                     /* Then, call the Change_Notification_Status from the readCallback with these params */
00514                 
00515                 }else if (readCompleted == 1) {
00516                     //notificationPending = 0;
00517                     Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_MIC, notification_freq);
00518                 }
00519                 perDevs.prx_on[i] = attr_value[0];
00520                 printf("\r\nMIC notifications [%d] %s (status %d, discovery %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), perDevs.status, perDevs.discovery_enabled);    
00521             
00522 #if SCAN_AFTER_NOTIFICATION
00523                 /* Restart peripheral discovery and notification*/
00524                 if (attr_value[0]!=1){
00525                     perDevs.discovery_enabled = 1;
00526                     setNewNodesScanning(0x01);
00527                     //HAL_Delay(300);
00528                 }
00529 #endif
00530             break;
00531 
00532 
00533 
00534 
00535 
00536             //PRX_TYPE_ID
00537             case PRX_TYPE_ID:
00538             
00539                 if (readCompleted == 0){
00540                     notificationPending = 1;    
00541                 
00542                     /* Fill the temp struct */
00543                     notifyP->att_data     = att_data;
00544                     notifyP->attr_value   = attr_value;
00545                     notifyP->conn_handle  = conn_handle;
00546                     notifyP->i            = i;
00547                     notifyP->feature_mask = FEATURE_MASK_PROX;
00548                     notifyP->frequency    = notification_freq*SENDING_INTERVAL_100MS_MULTIPLE;
00549                 
00550                     /* Then, call the Change_Notification_Status from the readCallback with these params */
00551                 
00552                 }else if (readCompleted == 1) {
00553                     //notificationPending = 0;
00554                     Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_PROX, notification_freq*SENDING_INTERVAL_100MS_MULTIPLE);
00555                 }    
00556                     perDevs.prx_on[i] = attr_value[0];
00557                     printf("\r\nPRX notifications [%d] %s (status %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), perDevs.status);
00558 
00559 #if SCAN_AFTER_NOTIFICATION
00560                 /* Restart peripheral discovery and notification*/
00561                 if (attr_value[0]!=1){
00562                     perDevs.discovery_enabled = 1;
00563                     setNewNodesScanning(0x01);
00564                     //HAL_Delay(300);
00565                 }
00566 #endif
00567             break;
00568 
00569 
00570 
00571             //ACC_TYPE_ID
00572             //GYR_TYPE_ID
00573             //MAG_TYPE_ID
00574             case ACC_TYPE_ID:
00575             case GYR_TYPE_ID:
00576             case MAG_TYPE_ID:
00577                 
00578                 if (readCompleted == 0){
00579                     notificationPending = 1;        
00580                 
00581                     /* Fill the temp struct */
00582                     notifyP->att_data     = att_data;
00583                     notifyP->attr_value   = attr_value;
00584                     notifyP->conn_handle  = conn_handle;
00585                     notifyP->i            = i;
00586                     notifyP->feature_mask = FEATURE_MASK_ACC;
00587                     notifyP->frequency    = SENDING_INTERVAL_100MS_MULTIPLE;
00588                 
00589                 
00590                     /* Then, call the Change_Notification_Status from the readCallback with these params */
00591                 
00592                 }else if (readCompleted == 1) {
00593                     //notificationPending = 0;
00594                     Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_ACC, SENDING_INTERVAL_100MS_MULTIPLE);
00595                 }
00596                 perDevs.agm_on[i] = attr_value[0];
00597 
00598                 if (att_data[2] == 0x09) {
00599                     perDevs.acc_event_enabled = attr_value[0];
00600                     mems = "ACC";
00601 
00602                 } else if (att_data[2] == 0x0A) {
00603                     perDevs.gyr_event_enabled = attr_value[0];
00604                     mems = "GYR";
00605 
00606                 } else if (att_data[2] == 0x0B) {
00607                     perDevs.mag_event_enabled = attr_value[0];
00608                     mems = "MAG";
00609                 }
00610 
00611                 printf("\r\nAGM notifications [%d] %s (%s) (status %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), mems, perDevs.status);
00612 #if SCAN_AFTER_NOTIFICATION
00613                 /* Restart peripheral discovery and notification*/
00614                 if (attr_value[0]!=1){
00615                     perDevs.discovery_enabled = 1;
00616                     setNewNodesScanning(0x01);
00617                     //HAL_Delay(300);
00618                 }
00619 #endif
00620             break;
00621 
00622 
00623 
00624             //SFUSION_TYPE_ID
00625             case SFUSION_TYPE_ID:
00626                 
00627                     if (readCompleted == 0){
00628                         notificationPending = 1;        
00629                 
00630                         /* Fill the temp struct */
00631                         notifyP->att_data     = att_data;
00632                         notifyP->attr_value   = attr_value;
00633                         notifyP->conn_handle  = conn_handle;
00634                         notifyP->i            = i;
00635                         notifyP->feature_mask = FEATURE_MASK_SENSORFUSION;
00636                         notifyP->frequency    = SENDING_INTERVAL_100MS_MULTIPLE;
00637             
00638                         /* Then, call the Change_Notification_Status from the readCallback with these params */
00639             
00640                     }else if (readCompleted == 1) {
00641                         Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_SENSORFUSION, SENDING_INTERVAL_100MS_MULTIPLE);
00642                     }
00643                 
00644                     perDevs.sfusion_on[i] = attr_value[0];
00645                     printf("\r\nSFUSION notifications [%d] %s (status %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), perDevs.status);
00646 
00647 #if SCAN_AFTER_NOTIFICATION
00648                 /* Restart peripheral discovery and notification*/
00649                 if (attr_value[0]!=1){
00650                     perDevs.discovery_enabled = 1;
00651                     setNewNodesScanning(0x01);
00652                     //HAL_Delay(300);
00653 
00654                 }
00655 #endif
00656             break;
00657             }
00658         }//if
00659     }//if-main
00660 
00661 
00662 }
00663 /*----------------------------------------------------------------------------*/
00664 
00665 
00666 
00667 
00668 /* Retrieve the device index from the peripheral device address */
00669 uint8_t Get_Device_Index_From_Addr(uint8_t *addr){
00670 
00671     uint8_t i;
00672 
00673     for (i=0; i<MAX_NUM_OF_NODES; i++) {
00674 
00675         if ((perDevs.devInfo[i].bdaddr[NODE_ID_B2] == addr[0]) && (perDevs.devInfo[i].bdaddr[NODE_ID_B1] == addr[1])) {
00676             return i;
00677         }
00678     }
00679     return MAX_NUM_OF_NODES;
00680 }
00681 /*----------------------------------------------------------------------------*/
00682 
00683 
00684 
00685 
00686 /**
00687  * @brief  This function is called to Enable/Disable MIC/PRX/AGM/SFusion notifications
00688  * @param  att_data : pointer to the modified attribute data
00689  * @param  connection handle
00690  * @param  peripheral device index
00691  * @param  feature mask
00692  * @param  notification frequency
00693  * @retval None
00694  */
00695  void Change_Notification_Status(uint8_t *att_data, uint8_t  *attr_value, uint16_t conn_handle, uint8_t i,
00696                                  uint32_t feature_mask, uint8_t frequency) {
00697     //printf("\r\nChange_Notification_Status (status: %d) (read: %s) (write: %s)\n", perDevs.status, (readCompleted == 1 ? "completed" : "pending"),(writeDescriptorCompleted == 1 ? "completed" : "pending"));//DEBUG
00698     
00699 
00700         uint8_t  value_len;
00701         uint16_t attr_handle;
00702 
00703 
00704         if (att_data[3] == 1) {
00705 
00706             /* Setting notification frequency (wait of 2s for writing op)*/
00707             setNotificationProperty(conn_handle, i, feature_mask, NOTIFICATION_FREQ_CMD, frequency);
00708 
00709             /* Stopping the scanning for new peripheral nodes */
00710             if (perDevs.discovery_enabled) {
00711                 //Disable scanning when notification is enabled
00712                 setNewNodesScanning(0x02);
00713             }
00714 
00715             /* Disabling all previously enabled notifications */
00716             disableAllNotifications();
00717 
00718         }//if
00719 
00720 
00721 
00722 
00723         /* Enabling/Disabling notifications */
00724         value_len = 2;
00725         attr_value[0] = att_data[3];
00726 
00727         if (feature_mask==FEATURE_MASK_MIC){
00728             attr_handle = perDevs.mic_char_handle[i] + 2;
00729             perDevs.mic_event_enabled = attr_value[0];
00730 
00731         } else if (feature_mask==FEATURE_MASK_PROX){
00732             attr_handle = perDevs.prx_char_handle[i] + 2;
00733             perDevs.prx_event_enabled = attr_value[0];
00734 
00735         } else if (feature_mask==FEATURE_MASK_ACC){
00736             attr_handle = perDevs.agm_char_handle[i] + 2;
00737             perDevs.agm_event_enabled = attr_value[0];
00738 
00739         } else if (feature_mask==FEATURE_MASK_SENSORFUSION){
00740             attr_handle = perDevs.sfusion_char_handle[i] + 2;
00741             perDevs.sfusion_event_enabled = attr_value[0];
00742         }
00743 
00744         
00745         
00746             ble_error_t err;
00747             
00748             writeDescriptorCompleted=0;
00749             err = writeCharacDescriptorWithError(conn_handle, attr_handle, value_len, attr_value);
00750 
00751             if (err != BLE_ERROR_NONE){
00752                 writeDescriptorCompleted=1;
00753                 printf("\r\nERROR Enabling/Disabling Notification (err: %d) (stat: %d)\n", err, perDevs.status);
00754                 
00755             }
00756 
00757 
00758     /* WUP notification enable/disable */
00759     if(attr_value[0]==0x00){
00760         for (uint8_t j=0; j<(perDevs.connDevices+perDevs.discDevices); j++) {
00761             if (!perDevs.wup_event[j] && perDevs.devInfo[j].dev_v == NODE_ME1 && perDevs.is_connected[j]) {
00762                 attr_handle = perDevs.wup_char_handle[j] + 2;
00763                 attr_value[0] = 0x01;
00764     
00765                 setNotificationProperty(perDevs.connection_handle[j], j, FEATURE_MASK_WAKEUP_EVENTS, WAKEUP_NOTIFICATION_CMD, 1);
00766               
00767                     writeDescriptorCompleted=0;
00768                     ble_error_t wupErrOn = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);
00769 
00770                     if (wupErrOn == BLE_ERROR_NONE){
00771                         perDevs.wup_event_enabled = 1;
00772                         perDevs.wup_event[j] = 1;
00773                         printf("\r\nWUP notification on node [%d] ON\n", j);
00774                          
00775                     } else {
00776                         printf("\r\nWrite WUP char descriptor failed! (%d)\n", wupErrOn);
00777                         writeDescriptorCompleted=1;
00778                     }//if-else
00779                     
00780             }//if
00781         }//for
00782 
00783 
00784         if (perDevs.wup_event_enabled == 1)
00785             //printf("\r\nAll WUP notifications turned ON\n");//DEBUG
00786         perDevs.status = ALL_DATA_READ;
00787 
00788 
00789     } else{
00790         for (uint8_t j=0; j<(perDevs.connDevices+perDevs.discDevices); j++) {
00791             if ((perDevs.wup_event[j]) && (perDevs.devInfo[j].dev_v == NODE_ME1) && (perDevs.is_connected[j])) {
00792                 attr_handle = perDevs.wup_char_handle[j] + 2;
00793                 attr_value[0] = 0x00;
00794 
00795 
00796                     writeDescriptorCompleted=0;
00797                     ble_error_t wupErrOff = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);
00798 
00799                     if (wupErrOff == BLE_ERROR_NONE){
00800                         perDevs.wup_event_enabled = 0;
00801                         perDevs.wup_event[j] = 0;
00802                         printf("\r\nWUP notification on node [%d] OFF\n", j);
00803 
00804                     } else {         
00805                         printf("\r\nWrite WUP char descriptor failed! (%d)\n", wupErrOff);
00806                         writeDescriptorCompleted=1;
00807                     }
00808 
00809             }//if
00810         }//for
00811         
00812         if (perDevs.wup_event_enabled == 0)
00813             //printf("\r\nAll WUP notifications turned OFF\n");//DEBUG
00814         perDevs.status = NOTIFICATIONS_DATA_READ;
00815 
00816     }//if-else-wup
00817 
00818 
00819 
00820         attr_value[0] = att_data[3];
00821         perDevs.readDeviceIdx = i;
00822         
00823         /* No notification is pending anymore */
00824         notificationPending = 0;
00825         
00826         //Return to connectionProcess
00827         //perDevs.status = CONN_INIT;
00828     
00829 }
00830 /*----------------------------------------------------------------------------*/