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/BleSlaveService.cpp
- Revision:
- 0:1902469bdd2d
- Child:
- 1:110b5e896bc9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/source/BleSlaveService.cpp Tue Feb 20 11:21:41 2018 +0000
@@ -0,0 +1,885 @@
+#include <BleSlaveService.h>
+
+
+/* ----- Private variables ----- */
+/* Struct of slave dev */
+SlaveDevice_t slaveDev;
+
+
+ChangeNotificationQueue notifyQ, *notifyP;
+
+
+/* Struct of periph dev */
+extern PeripheralDevices_t perDevs;
+
+/* Notification frequency */
+uint8_t notification_freq = NOTIFICATION_FREQ_WIFI_OFF;
+
+
+/* Star Service and Characteristic */
+uint8_t sUuid[16] = {0x00,0x00,0x00,0x00,0x00,0x01,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
+uint8_t dataUuid[16] = {0x00,0x08,0x00,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
+uint8_t confUuid[16] = {0x00,0x04,0x00,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
+
+
+/*---- Create the two characteristic ----*/
+/* Config char */
+GattCharacteristic configChar(confUuid, slaveDev.star_config_value, STAR_CHAR_MAX_VALUE_LEN, STAR_CHAR_MAX_VALUE_LEN,
+ GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE,
+ NULL, 0, true);
+
+
+/* Data char */
+GattCharacteristic dataChar(dataUuid, slaveDev.notification_data.attribute_value, STAR_CHAR_MAX_VALUE_LEN, STAR_CHAR_MAX_VALUE_LEN,
+ GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE,
+ NULL, 0, true);
+
+
+/* MANUFACTURER_SPECIFIC_DATA */
+ uint8_t manuf_data[6] = {
+ 0x01, /*SKD version */
+ 0x81, /* NUCLEO-Board remote */
+ 0x00, /* Led Prox+Lux */ //Unused
+ 0x0C, /* Star Config Char + Star Data Char */ //Unused
+ 0x00, /* SensorFusionShort */ //Unused
+ 0x00 /* SensorFusionFloat */ //Unused
+ };
+
+
+
+
+
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Set the device as a slave in discoverable mode */
+void setSlaveDiscoverable(void) {
+ //printf("\r\nsetSlaveDiscoverable\n");//DEBUG
+ BLE& ble = BLE::Instance();
+ ble_error_t e2;
+
+
+ //if-MAIN - discovery/reading/writing ops completed
+ if ((discoveryCompleted == 1) /*&& (readCompleted == 1) && (writeDescriptorCompleted == 1)*/){
+
+
+
+ /* Start advertisng */
+ if ((!ble.gap().getState().advertising)){
+ advEnds =0;
+
+ printf("\r\nSet Discoverable Mode (%d)\n", perDevs.status);
+ e2 = ble.gap().startAdvertising();
+ if ( e2 == BLE_ERROR_NONE ){
+ /* Giving some time to connect */
+ eventQ.call_in(7000, stopAdv);//N ms
+ }else{
+ printf("\r\nError while starting advertising (err: %d - stat: %d)\n", e2, perDevs.status);
+ advEnds=1;//terminated because not started
+ }
+
+ }//if-not-adv
+ }//if-MAIN
+
+
+ //Restart loop ops
+ perDevs.status = CONN_INIT;
+ perDevs.discovery_enabled = true;
+ //printf("\rstatus = CONN_INIT (%d)\n", perDevs.status);//DEBUG
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+void stopAdv(){
+
+ if ((slaveDev.is_connected == false) && (discoveryCompleted == 1)){
+ if (BLE::Instance().gap().getState().advertising){
+ ble_error_t e = BLE::Instance().gap().stopAdvertising();
+ if ( e != BLE_ERROR_NONE ) {
+ printf("\r\nError stopping ADV\n");//DEBUG
+ }
+ }
+ }//if
+
+ advEnds = 1;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/**
+ * @brief This function sets some notification properties (e.g. the
+ * notification period) by sending a command according to the
+ * following format:
+ * FEATURE_MASK (4bytes) + Command (1byte) + Data (1byte)
+ * In case of the notification period the command is 255 while the allowed data are:
+ * 50 @5S, 10 @1S, 1 @100mS, 0 @50mS (default).
+ * @param connection handle
+ * @param peripheral device index
+ * @param feature mask
+ * @param command
+ * @param data
+ * @retval None
+ */
+void setNotificationProperty(uint16_t conn_handle, uint8_t i, uint32_t feature_mask,
+ uint8_t command, uint8_t data) {
+ //printf("\r\nsetNotificationProperty\n");//DEBUG
+
+ uint16_t attr_handle;
+ uint8_t value_len;
+ uint8_t attr_value[6];
+
+ attr_handle = perDevs.cfg_char_handle[i] + 1;
+ attr_value[0] = (uint8_t)((feature_mask) >> 24);
+ attr_value[1] = (uint8_t)((feature_mask) >> 16);
+ attr_value[2] = (uint8_t)((feature_mask) >> 8);
+ attr_value[3] = (uint8_t)((feature_mask));
+ 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
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* Forwards to master all notifications from peripherals */
+void notifyMaster(uint8_t data_length, uint8_t* attribute_value, uint16_t attribute_handle){
+ //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;
+ }
+
+ } else {
+ //HAL_Delay(1000);//giving some time to slowdown the rate
+ //dOING SOMETHING !
+ return;
+ }
+}
+/*----------------------------------------------------------------------------*/
+
+
+/* Add all services using a vendor specific UUIDs */
+void addAllServices(void){
+
+ ble_error_t error;
+ notifyP = ¬ifyQ;
+
+
+ GattCharacteristic *charTable[] = {&configChar, &dataChar};
+
+ /*---- Create the hw service ----*/
+ GattService starService(sUuid, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
+
+ /*---- Add the hw service ----*/
+ error = BLE::Instance().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
+
+
+
+ if ( error != BLE_ERROR_NONE ){
+ printf("\r\nError while creating starService(error: %d)\n", error);
+
+ } else {
+ printf("\r\nStar HW Gatt Service added (handle 0x%004x)\n", slaveDev.star_hw_serv_handle);
+ printf("\rStar HW Config Char added (handle 0x%004x)\n", slaveDev.star_config_char_handle);
+ printf("\rStar HW Data Char added (handle 0x%004x)", slaveDev.star_data_char_handle);
+ printf("\r\n");
+ }
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+/* Disable all notifications */
+void disableAllNotifications(void){
+ //printf("\r\ndisableAllNotifications\n");//DEBUG
+
+
+ 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.mic_event[j]) && (perDevs.mic_event_enabled)) {
+ feat = "MIC";
+ perDevs.mic_event_enabled = 0;
+ perDevs.mic_event[j] = 0;
+ attr_handle = perDevs.mic_char_handle[j] + 2;
+
+ } else 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 if ((perDevs.agm_event[j]) && (perDevs.agm_event_enabled)) {
+ feat = "AGM";
+ perDevs.acc_event_enabled = 0;
+ perDevs.gyr_event_enabled = 0;
+ perDevs.mag_event_enabled = 0;
+ perDevs.agm_event_enabled = 0;
+ perDevs.agm_on[j] = 0;
+ attr_handle = perDevs.agm_char_handle[j] + 2;
+
+ } else if ((perDevs.sfusion_event[j]) && (perDevs.sfusion_event_enabled)) {
+ feat = "SFUSION";
+ perDevs.sfusion_event_enabled = 0;
+ perDevs.sfusion_on[j] = 0;
+ attr_handle = perDevs.sfusion_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(disableAllNotifications) Write charac descriptor failed (%d)\n", disableErr);
+ writeDescriptorCompleted=1;
+ //HAL_Delay(500);
+ //disableAllNotifications();
+ }//if-error
+ }//for
+
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* This function enables or disables the new peripheral scanning */
+void setNewNodesScanning(uint8_t enabled){
+
+ STORE_LE_16(slaveDev.star_config_value,(HAL_GetTick()>>3)); /* Timestamp */
+
+ switch (enabled) {
+ case 0x01:
+ perDevs.discovery_enabled = true;
+ //printf("\r\nScanning enabled (%d)\n", enabled);
+ break;
+
+ case 0x02:
+ perDevs.discovery_enabled = false;
+ //printf("\r\nScanning disabled (%d)\n", enabled);
+ break;
+
+ default:
+ //printf("\r\nScanning set to unrecognized value (%d)\n", enabled);
+ break;
+ }
+
+ slaveDev.star_config_value[2] = enabled;
+ slaveDev.star_config_value_len = 3;
+ //HAL_Delay(300);
+
+ /* Notify master for scanning */
+
+/* Stack busy, solve this */
+// notifyMaster(slaveDev.star_config_value_len, slaveDev.star_config_value,
+// slaveDev.star_config_char_handle);
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+//NOT USED FOR NOW !!
+/* This function forwards to the peripheral device the command coming from the Cloud */
+void Forward_Command_To_BlueNRG(uint8_t* data_ptr, uint8_t data_length){
+
+ uint16_t handle;
+ uint8_t buffer[4] = {0, 0, 0, 0};
+ uint8_t num, even, odd;
+ uint8_t i;
+ uint8_t cmd_type;
+
+ cmd_type = (data_ptr[0] - '0') << 4 | (data_ptr[1] - '0');
+ //printf("\r\ncommand %d\n", cmd_type);
+
+ switch (cmd_type)
+ {
+ case 0:
+ printf("\rScan cmd type (%d)\n", cmd_type);
+ handle = slaveDev.star_config_char_handle + 1;
+ break;
+ case 1:
+ printf("\rLED/MIC/PRX cmd type (%d)\n", cmd_type);
+ handle = slaveDev.star_data_char_handle + 1;
+ break;
+ default:
+ printf("\rUnknown cmd type (%d)\n", cmd_type);
+ return;
+ //break;
+ }
+
+ /**
+ * For Scan cmd type:
+ * |Value |
+ * |1 byte |
+ * For LED/MIC/PRX cmd type:
+ * |Node ID | Type ID | Value |
+ * |2 bytes | 1 byte | x bytes |
+ */
+ for (i=2; i<data_length; i++) {
+ switch (data_ptr[i])
+ {
+ case 'A':
+ case 'a':
+ num = 10;
+ break;
+ case 'B':
+ case 'b':
+ num = 11;
+ break;
+ case 'C':
+ case 'c':
+ num = 12;
+ break;
+ case 'D':
+ case 'd':
+ num = 13;
+ break;
+ case 'E':
+ case 'e':
+ num = 14;
+ break;
+ case 'F':
+ case 'f':
+ num = 15;
+ break;
+ default:
+ num = data_ptr[i] - '0';
+ break;
+ }
+ if (!(i%2)) {
+ even = num << 4;
+ }
+ else {
+ odd = num;
+ buffer[(i-2)/2] = even | odd;
+ }
+ }
+
+ createAttribute_Modified_CB_Prototype(handle, (data_length-2)/2, buffer);
+}
+/*----------------------------------------------------------------------------*/
+
+
+/* Function used to call AttributeModified_CB outside the callback call */
+void createAttribute_Modified_CB_Prototype(uint16_t handle, uint8_t data_length, uint8_t *att_data){
+
+ GattWriteCallbackParams paramStruct, *pointerToParam;
+ pointerToParam = ¶mStruct;
+
+ pointerToParam->handle = handle;
+ pointerToParam->len = data_length;
+ pointerToParam->data = att_data;
+
+ AttributeModified_CB(pointerToParam);
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* This function is called when a local attribute gets modified */
+void AttributeModified_CB(const GattWriteCallbackParams* param){
+// printf("\r\nAttributeModified_CB (%d)\n", perDevs.status);//DEBUG
+
+
+ /* Assign prototype vars */
+ uint16_t handle = param->handle;
+ //uint8_t data_length = param->len; //not used
+ uint8_t *att_data = (uint8_t *)param->data;
+
+
+ uint8_t i;
+ uint16_t conn_handle;
+ uint16_t attr_handle;
+ uint8_t value_len;
+ uint8_t attr_value[6];
+ char* mems;
+
+ if (handle == slaveDev.star_config_char_handle + 1) {
+
+ perDevs.status = ALL_DATA_READ;
+ if (att_data[0]==0x01) {
+ /* Disabling all previously enabled notifications */
+ disableAllNotifications();
+ }//if
+ setNewNodesScanning(att_data[0]);
+
+
+ } else if (handle == slaveDev.star_config_char_handle + 2) {
+ if (att_data[0]==0x01) {
+ //printf("\r\nEnable scanning notification\n");
+ STORE_LE_16(slaveDev.star_config_value,(HAL_GetTick()>>3)); /* Timestamp */
+ slaveDev.star_config_value[2] = perDevs.discovery_enabled;
+ slaveDev.star_config_value_len = 3;
+
+ /* send the status of the scanning when notifications are enabled */
+ notifyMaster(slaveDev.star_config_value_len, slaveDev.star_config_value,
+ slaveDev.star_config_char_handle);
+
+ /* enable here the data notifications since sometimes the relative command gets lost (FIXME) */
+ //printf("\r\nEnable data notification (0)\n");
+ slaveDev.star_data_char_notify = 1;
+
+ } else if (att_data[0]==0x00) {
+ //printf("\r\nDisable scanning notification\n");
+ }
+
+ } else if (handle == slaveDev.star_data_char_handle + 2) {
+ if (att_data[0]==0x01) {
+ //printf("\r\nEnable data notification (1)\n");
+ slaveDev.star_data_char_notify = 1;
+
+
+ } else if (att_data[0]==0x00) {
+ //printf("\r\nDisable data notification\n");
+ slaveDev.star_data_char_notify = 0;
+
+ }
+
+ } else if (handle == slaveDev.star_data_char_handle + 1) {
+
+ /*
+ * Forward the command to the proper peripheral node.
+ * It can be:
+ * - the ON/OFF of a LED
+ * - the notification enabling/disabling for a MIC Level or a Proximity Sensor
+ * The coming data are received according to the following format:
+ * |Node ID | Type ID | value |
+ * |2 bytes | 1 byte | x bytes |
+ */
+
+ i = Get_Device_Index_From_Addr(att_data);
+ if ((i < MAX_NUM_OF_NODES) && (!perDevs.is_disconnected[i]) && (perDevs.cfg_char_handle[i] != 0)) {
+ conn_handle = perDevs.connection_handle[i];
+
+ switch (att_data[2]) {
+
+
+ //LED_TYPE_ID
+ case LED_TYPE_ID:
+ attr_handle = perDevs.cfg_char_handle[i] + 1;
+ value_len = 5; /* 4 bytes for the feature mask + 1 byte for the command (0=ON, 1=OFF) */
+ attr_value[0] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS) >> 24);
+ attr_value[1] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS) >> 16);
+ attr_value[2] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS) >> 8);
+ attr_value[3] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS));
+ attr_value[4] = att_data[3];
+
+ writeCharacValueWithoutResp(conn_handle, attr_handle, value_len, attr_value);
+
+ perDevs.readDeviceIdx = i;
+ break;
+
+
+
+ //MICLEVEL_TYPE_ID
+ case MICLEVEL_TYPE_ID:
+ //perDevs.status = CHANGE_NOTIFICATION_STATUS;//47
+
+ if (readCompleted == 0){
+ notificationPending = 1;
+
+ /* Fill the temp struct */
+ notifyP->att_data = att_data;
+ notifyP->attr_value = attr_value;
+ notifyP->conn_handle = conn_handle;
+ notifyP->i = i;
+ notifyP->feature_mask = FEATURE_MASK_MIC;
+ notifyP->frequency = notification_freq;
+
+ /* 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_MIC, notification_freq);
+ }
+ perDevs.prx_on[i] = attr_value[0];
+ printf("\r\nMIC notifications [%d] %s (status %d, discovery %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), perDevs.status, perDevs.discovery_enabled);
+
+#if SCAN_AFTER_NOTIFICATION
+ /* Restart peripheral discovery and notification*/
+ if (attr_value[0]!=1){
+ perDevs.discovery_enabled = 1;
+ setNewNodesScanning(0x01);
+ //HAL_Delay(300);
+ }
+#endif
+ break;
+
+
+
+
+
+ //PRX_TYPE_ID
+ case PRX_TYPE_ID:
+ //perDevs.status = CHANGE_NOTIFICATION_STATUS;//47
+
+ if (readCompleted == 0){
+ notificationPending = 1;
+
+ /* Fill the temp struct */
+ notifyP->att_data = att_data;
+ notifyP->attr_value = attr_value;
+ notifyP->conn_handle = conn_handle;
+ notifyP->i = i;
+ notifyP->feature_mask = FEATURE_MASK_PROX;
+ notifyP->frequency = 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);
+ }
+ 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);
+
+#if SCAN_AFTER_NOTIFICATION
+ /* Restart peripheral discovery and notification*/
+ if (attr_value[0]!=1){
+ perDevs.discovery_enabled = 1;
+ setNewNodesScanning(0x01);
+ //HAL_Delay(300);
+ }
+#endif
+ break;
+
+
+
+ //ACC_TYPE_ID
+ //GYR_TYPE_ID
+ //MAG_TYPE_ID
+ case ACC_TYPE_ID:
+ case GYR_TYPE_ID:
+ case MAG_TYPE_ID:
+ //perDevs.status = CHANGE_NOTIFICATION_STATUS;//47
+
+ if (readCompleted == 0){
+ notificationPending = 1;
+
+ /* Fill the temp struct */
+ notifyP->att_data = att_data;
+ notifyP->attr_value = attr_value;
+ notifyP->conn_handle = conn_handle;
+ notifyP->i = i;
+ notifyP->feature_mask = FEATURE_MASK_ACC;
+ notifyP->frequency = 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);
+ }
+ perDevs.agm_on[i] = attr_value[0];
+
+ if (att_data[2] == 0x09) {
+ perDevs.acc_event_enabled = attr_value[0];
+ mems = "ACC";
+
+ } else if (att_data[2] == 0x0A) {
+ perDevs.gyr_event_enabled = attr_value[0];
+ mems = "GYR";
+
+ } else if (att_data[2] == 0x0B) {
+ perDevs.mag_event_enabled = attr_value[0];
+ mems = "MAG";
+ }
+
+ printf("\r\nAGM notifications [%d] %s (%s) (status %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), mems, perDevs.status);
+#if SCAN_AFTER_NOTIFICATION
+ /* Restart peripheral discovery and notification*/
+ if (attr_value[0]!=1){
+ perDevs.discovery_enabled = 1;
+ setNewNodesScanning(0x01);
+ //HAL_Delay(300);
+ }
+#endif
+ break;
+
+
+
+ //SFUSION_TYPE_ID
+ case SFUSION_TYPE_ID:
+ //perDevs.status = CHANGE_NOTIFICATION_STATUS;//47
+
+ if (readCompleted == 0){
+ notificationPending = 1;
+
+ /* Fill the temp struct */
+ notifyP->att_data = att_data;
+ notifyP->attr_value = attr_value;
+ notifyP->conn_handle = conn_handle;
+ notifyP->i = i;
+ notifyP->feature_mask = FEATURE_MASK_SENSORFUSION;
+ notifyP->frequency = 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);
+ }
+
+ perDevs.sfusion_on[i] = attr_value[0];
+ printf("\r\nSFUSION notifications [%d] %s (status %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), perDevs.status);
+
+#if SCAN_AFTER_NOTIFICATION
+ /* Restart peripheral discovery and notification*/
+ if (attr_value[0]!=1){
+ perDevs.discovery_enabled = 1;
+ setNewNodesScanning(0x01);
+ //HAL_Delay(300);
+
+ }
+#endif
+ break;
+ }
+ }//if
+ }//if-main
+
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* Retrieve the device index from the peripheral device address */
+uint8_t Get_Device_Index_From_Addr(uint8_t *addr){
+
+ uint8_t i;
+
+ for (i=0; i<MAX_NUM_OF_NODES; i++) {
+
+ if ((perDevs.devInfo[i].bdaddr[NODE_ID_B2] == addr[0]) && (perDevs.devInfo[i].bdaddr[NODE_ID_B1] == addr[1])) {
+ return i;
+ }
+ }
+ return MAX_NUM_OF_NODES;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/**
+ * @brief This function is called to Enable/Disable MIC/PRX/AGM/SFusion notifications
+ * @param att_data : pointer to the modified attribute data
+ * @param connection handle
+ * @param peripheral device index
+ * @param feature mask
+ * @param notification frequency
+ * @retval None
+ */
+ 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
+
+
+
+ uint8_t value_len;
+ uint16_t attr_handle;
+
+
+ if (att_data[3] == 1) {
+
+ /* Setting notification frequency (wait of 2s for writing op)*/
+ setNotificationProperty(conn_handle, i, feature_mask, NOTIFICATION_FREQ_CMD, frequency);
+
+ /* Stopping the scanning for new peripheral nodes */
+ if (perDevs.discovery_enabled) {
+ //Disable scanning when notification is enabled
+ setNewNodesScanning(0x02);
+ }
+
+ /* Disabling all previously enabled notifications */
+ disableAllNotifications();
+
+ }//if
+
+
+
+
+ /* Enabling/Disabling notifications */
+ value_len = 2;
+ attr_value[0] = att_data[3];
+
+ if (feature_mask==FEATURE_MASK_MIC){
+ attr_handle = perDevs.mic_char_handle[i] + 2;
+ perDevs.mic_event_enabled = attr_value[0];
+
+ } else if (feature_mask==FEATURE_MASK_PROX){
+ attr_handle = perDevs.prx_char_handle[i] + 2;
+ perDevs.prx_event_enabled = attr_value[0];
+
+ } else if (feature_mask==FEATURE_MASK_ACC){
+ attr_handle = perDevs.agm_char_handle[i] + 2;
+ perDevs.agm_event_enabled = attr_value[0];
+
+ } else if (feature_mask==FEATURE_MASK_SENSORFUSION){
+ attr_handle = perDevs.sfusion_char_handle[i] + 2;
+ perDevs.sfusion_event_enabled = attr_value[0];
+ }
+
+
+// if ((discoveryCompleted == 1)){
+
+ ble_error_t err;
+
+ writeDescriptorCompleted=0;
+ 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);
+// 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){
+// 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{
+// 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;
+
+ notificationPending = 0;
+
+ //Return to connectionProcess
+ perDevs.status = CONN_INIT;
+
+
+
+
+}
+/*----------------------------------------------------------------------------*/