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/BleMasterService.cpp
- Revision:
- 0:1902469bdd2d
- Child:
- 1:110b5e896bc9
diff -r 000000000000 -r 1902469bdd2d source/BleMasterService.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/source/BleMasterService.cpp Tue Feb 20 11:21:41 2018 +0000
@@ -0,0 +1,3081 @@
+#include <BleMasterService.h>
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
+#include "../inc/BleMasterService.h"
+#include "../inc/BleSlaveService.h"
+
+EventQueue eventQ(/* event count */ 128 * EVENTS_EVENT_SIZE);
+
+/* This var defines the central role
+ * 0 if master
+ * 1 if slave */
+uint8_t role = 1;
+
+
+
+/* Flag to indicate that the characteristic read is completed successful
+ * 1 successful
+ * 0 or anything else failed */
+uint8_t readCompleted;
+
+
+
+
+/* Flag to indicate if the device discovery is completed successful
+ * 1 successful
+ * 0 or anything else failed */
+uint8_t discoveryCompleted;
+
+
+/* Flag to indicate if the ch descriptor write is completed successful
+ * 1 successful
+ * 0 or anything else failed */
+uint8_t writeDescriptorCompleted = 1;
+
+
+/* Flag to indicate if the advertising is completed
+ * 1 completed
+ * 0 or anything else not completed */
+uint8_t advEnds = 1;
+
+
+/* Flag to indicate if a change of notification is pending
+ * 1 pending
+ * 0 or anything else not pending */
+uint8_t notificationPending = 0;
+
+
+/* Header pointer of the DiscoveredCharacteristic list */
+DiscoveredCharacteristicNode * headCharacteristic[MAX_NUM_OF_NODES];
+DiscoveredCharacteristicNode * tmp[MAX_NUM_OF_NODES];
+
+/* Primary Service UUID expected from Sensor demo peripherals */
+UUID::ShortUUIDBytes_t GENERIC_ACCESS_PROFILE_UUID = 0x1800;
+UUID::ShortUUIDBytes_t GENERIC_ATTRIBUTE_PROFILE_UUID = 0x1801;
+
+/* Services UUID */
+UUID::LongUUIDBytes_t HARDWARE_SERVICE_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0xb4,0x9a,0xe1,0x11,0x01,0x00,0x00,0x00,0x00,0x00};
+UUID::LongUUIDBytes_t CONFIG_SERVICE_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0xb4,0x9a,0xe1,0x11,0x0F,0x00,0x00,0x00,0x00,0x00};
+UUID::LongUUIDBytes_t SOFTWARE_SERVICE_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0xb4,0x9a,0xe1,0x11,0x02,0x00,0x00,0x00,0x00,0x00};
+
+/* Characteristics UUID */
+UUID::LongUUIDBytes_t ENVIRONMENTAL_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x1d,0x00};
+UUID::LongUUIDBytes_t ENVIRONMENTAL_ST_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x14,0x00};
+UUID::LongUUIDBytes_t ACCGYROMAG_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0xE0,0x00};
+UUID::LongUUIDBytes_t SFUSION_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x01,0x00,0x00};
+UUID::LongUUIDBytes_t LED_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x00,0x20};
+UUID::LongUUIDBytes_t WAKEUP_EVENT_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x04,0x00,0x00};
+UUID::LongUUIDBytes_t MIC_EVENT_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x00,0x04};
+UUID::LongUUIDBytes_t PROXIMITY_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x00,0x02};
+UUID::LongUUIDBytes_t LUX_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x00,0x01};
+UUID::LongUUIDBytes_t CONFIG_CHAR_UUID = {0x1b,0xc5,0xd5,0xa5,0x02,0x00,0x36,0xac,0xe1,0x11,0x0F,0x00,0x02,0x00,0x00,0x00};
+
+/*----- -----*/
+uint8_t wifi_data[256];
+uint8_t new_data = 0;
+uint8_t *data;
+uint8_t wifi_present;
+
+char print_msg_buff[512];
+//UART_HandleTypeDef UartMsgHandle;
+
+//uint16_t connection_handle = 0;
+
+uint8_t attribute_value[20];
+uint8_t star_attr_value[256];
+
+
+
+PeripheralDevices_t perDevs;
+extern SlaveDevice_t slaveDev;
+extern ChangeNotificationQueue notifyQ, *notifyP;
+
+/* Private variable set in discoveryTerminationCallback and used
+ * in connectionProcess for enabling notification */
+uint8_t customDev_v;
+
+
+
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Initialize the struct where all peripheral devices are stored */
+void initProcess(void) {
+ //printf("\rinitProcess\n");//DEBUG
+
+ //Init the lists of characteristics
+ for(int i=0; i<MAX_NUM_OF_NODES; i++){
+ headCharacteristic[i] = NULL;
+ tmp[i] = NULL;
+ }
+
+
+
+ /* The first char read */
+ readCompleted = 1;
+
+ /* No discovery started */
+ discoveryCompleted = 1;
+
+
+ perDevs.discovery_enabled = true;
+ perDevs.device_found = false;
+ perDevs.connDevices = 0;
+ perDevs.discDevices = 0;
+ perDevs.connDeviceIdx = 0;
+ perDevs.readDeviceIdx = 0;
+
+ for(unsigned i=0;i<MAX_NUM_OF_NODES;i++){
+ perDevs.devInfo[i].dev_v = 0;
+ perDevs.connection_handle[i] = 0;
+ perDevs.is_connected[i] = false;
+ perDevs.is_disconnected[i] = true;
+ perDevs.is_unconnectable[i] = false;
+ perDevs.gen_access_profile_handle[i].start_h = 0;
+ perDevs.gen_access_profile_handle[i].end_h = 0;
+ perDevs.gen_attribute_profile_handle[i].start_h = 0;
+ perDevs.gen_attribute_profile_handle[i].end_h = 0;
+ perDevs.hardware_service_handle[i].start_h = 0;
+ perDevs.hardware_service_handle[i].end_h = 0;
+ perDevs.software_service_handle[i].start_h = 0;
+ perDevs.software_service_handle[i].end_h = 0;
+ perDevs.led_char_read[i] = 0;
+ perDevs.wup_char_read[i] = 0;
+ perDevs.mic_char_read[i] = 0;
+ perDevs.prx_char_read[i] = 0;
+ perDevs.agm_char_read[i] = 0;
+ perDevs.sfusion_char_read[i] = 0;
+ perDevs.wup_event[i] = 0;
+ perDevs.mic_event[i] = 0;
+ perDevs.prx_on[i] = 0;
+ perDevs.agm_on[i] = 0;
+ perDevs.sfusion_on[i] = 0;
+ }
+
+ perDevs.mic_event_enabled = 0;
+ perDevs.prx_event_enabled = 0;
+ perDevs.agm_event_enabled = 0;
+ perDevs.sfusion_event_enabled = 0;
+ perDevs.acc_event_enabled = 0;
+ perDevs.gyr_event_enabled = 0;
+ perDevs.mag_event_enabled = 0;
+ perDevs.status = CONN_INIT;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Called every 10 seconds to return the current status */
+void checkStatus(){
+ if ((perDevs.status != 10) && (perDevs.status != 11) && (perDevs.status != 12) && (perDevs.status != 13) && (perDevs.status != 14)
+ && (perDevs.status != 15) && (perDevs.status != 16) && (perDevs.status != 17) && (perDevs.status != 18)
+ && (perDevs.status != 19) && (perDevs.status != 20) && (perDevs.status != 21)){
+ printf("\r\nCurrent Status (status: %d) (read: %s) (write: %s)\n", perDevs.status, (readCompleted == 1 ? "completed" : "pending"),(writeDescriptorCompleted == 1 ? "completed" : "pending"));
+ }
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Print out device MAC address to the console*/
+void printMacAddress()
+{
+ Gap::AddressType_t addr_type;
+ Gap::Address_t address;
+ BLE::Instance().gap().getAddress(&addr_type, address);
+ printf("\rBLE CENTRAL MAC ADDRESS: ");
+ for (int i = 5; i >= 1; i--){
+ printf("%02x:", address[i]);
+ }
+ printf("%02x\n\n", address[0]);
+}
+/*----------------------------------------------------------------------------*/
+
+
+void connectionProcess(void){
+ //printf("\r\nconnectionProcess(%d)\n", perDevs.status);//DEBUG
+
+
+
+
+ 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();
+ }
+ else {
+ perDevs.status = DEVICE_CONNECTED;
+ //printf("\r\nstatus: DEVICE_CONNECTED\n");//DEBUG
+ }
+
+ }//if
+
+
+
+
+
+
+ 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
+
+
+
+
+
+
+ // 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
+
+
+
+
+
+
+ if (perDevs.status == DISCOVERABLE_MODE_SET) {
+ /* Search for all services */
+ perDevs.status = START_SERVICE_DISCOVERY;
+ eventQ.call(discoverServices);
+ }//if
+
+
+
+
+
+
+ if ( ((perDevs.status == ENABLE_ME1_LED_NOTIFICATIONS) ||
+ (perDevs.status == ENABLE_ME1_WUP_NOTIFICATIONS)) && (writeDescriptorCompleted == 1) ) {
+
+ writeDescriptorCompleted =0;
+ /* Enabling notifications on peripheral node */
+ uint8_t i = perDevs.connDeviceIdx;
+ uint16_t connection_handle = perDevs.connection_handle[i];
+ enableNotifications(connection_handle, i, customDev_v, 1);
+ }//if
+
+
+
+
+
+
+
+ if (perDevs.status == NOTIFICATIONS_ENABLED) {
+ if (!perDevs.is_disconnected[perDevs.connDeviceIdx]) {
+ perDevs.is_connected[perDevs.connDeviceIdx] = true;
+ perDevs.connDeviceIdx++;
+ perDevs.connDevices++;
+ //printf("\r\nConnected Devices %d\n", perDevs.connDevices);//DEBUG
+ }
+
+ perDevs.readDeviceIdx = perDevs.connDevices-1;
+ perDevs.status = READ_INIT;
+ }//if
+
+
+
+
+
+ /* Start reading process */
+ eventQ.call(readingProcess);
+ //eventQ.call(connectionProcess);//DEBUG_ONLY /* Call in loop coonnectionProcess */
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Method called in advertingCallback when a device is found */
+void saveDeviceFound (uint8_t adv_type, BLEProtocol::AddressBytes_t addr,
+ uint8_t data_length, const uint8_t* data_RSSI, uint8_t pos, uint8_t dev_v,
+ uint8_t wup_event, uint8_t mic_event, uint8_t prx_event,
+ uint8_t agm_event, uint8_t sfusion_event){
+
+ perDevs.devInfo[pos].dev_v = dev_v;
+ perDevs.wup_event[pos] = wup_event;
+ perDevs.mic_event[pos] = mic_event;
+ perDevs.prx_event[pos] = prx_event;
+ perDevs.agm_event[pos] = agm_event;
+ perDevs.sfusion_event[pos] = sfusion_event;
+ memcpy(perDevs.devInfo[pos].bdaddr, addr, 6);
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* Function called to connect a peripheral */
+void connectPeripheral(void){
+
+ ble_error_t e0, e1;
+ BLE& ble = BLE::Instance();
+
+
+/* if 1 to enable */
+#if 1
+ e0 = ble.gap().stopScan();
+ if (e0 != BLE_ERROR_NONE){
+ printf("\r\nError while stopping scan\n");
+ }
+#endif
+
+ uint8_t index;
+ index = perDevs.connDeviceIdx;
+
+ printf("\r\nClient create connection with peripheral %d at pos %d\n",
+ perDevs.connDevices+1, index+1);
+
+ //Connect as master (0)
+ role = 0;
+
+
+ e1 = ble.gap().connect(perDevs.devInfo[index].bdaddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
+
+
+ if (e1 != BLE_ERROR_NONE){
+ printf("Error while starting connection with peripheral %d (%02x%02x).\n",
+ index+1, perDevs.devInfo[index].bdaddr[NODE_ID_B2], perDevs.devInfo[index].bdaddr[NODE_ID_B1]);
+ perDevs.is_unconnectable[index] = true;
+ }
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* Function called on disconnection event */
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params) {
+ //printf("\r\ndisconnectionCallback (%d)\n", perDevs.status);//DEBUG
+ BLE& ble = BLE::Instance();
+
+ uint8_t i;
+
+ // current handle
+ uint16_t handle = params->handle;
+
+
+
+
+
+ for (unsigned i=0; i<MAX_NUM_OF_NODES; i++) {
+
+ //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;
+ tmp[i] = NULL;
+
+ if (perDevs.is_disconnected[i] == true){
+ perDevs.is_unconnectable[i] = true;
+
+ printf("\r\nPeripheral 0x%02x%02x disconnected (%d Ch in DCList)\n",
+ perDevs.devInfo[i].bdaddr[NODE_ID_B2], perDevs.devInfo[i].bdaddr[NODE_ID_B1], countElements(headCharacteristic[i]));
+
+ }else {
+ perDevs.is_disconnected[i] = true;
+ perDevs.is_unconnectable[i] = false;
+
+
+ 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++;
+ perDevs.is_connected[i] = false;
+ }
+
+ /* Notify device disconnection to client */
+ perDevs.status = READING_DISCONNECTION;
+
+ readCharacteristicCallback(nullGattReadCallbackP(perDevs.connection_handle[i]));
+
+
+ notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
+ slaveDev.notification_data.attribute_handle);
+ perDevs.status = CONN_INIT;
+
+
+ printf("\r\nPeripheral 0x%02x%02x disconnected (%d still connected) (%d Ch in DCList)\n",
+ perDevs.devInfo[i].bdaddr[NODE_ID_B2], perDevs.devInfo[i].bdaddr[NODE_ID_B1],
+ perDevs.connDevices, countElements(headCharacteristic[i]));
+ }//if-else
+ break;
+
+ }//if-MAIN
+ }//for
+
+
+
+ /* SLAVE ROLE */
+ if (handle == slaveDev.conn_handle) {
+ printf("\r\nMaster disconnected (adv: %d - status %d)\n", ble.gap().getState().advertising, perDevs.status);//DEBUG
+
+
+ slaveDev.is_discoverable = true;
+ slaveDev.is_connected = false;
+ slaveDev.conn_handle = 0;
+ slaveDev.star_data_char_notify = 0;
+
+ for (i=0; i<MAX_NUM_OF_NODES; i++) {
+ perDevs.led_char_read[i] = 0;
+ perDevs.wup_char_read[i] = 0;
+ perDevs.mic_char_read[i] = 0;
+ perDevs.prx_char_read[i] = 0;
+ perDevs.agm_char_read[i] = 0;
+ perDevs.sfusion_char_read[i] = 0;
+ perDevs.prx_on[i] = 0;
+ perDevs.agm_on[i] = 0;
+ perDevs.sfusion_on[i] = 0;
+ }//for
+
+ perDevs.discovery_enabled = true;
+ perDevs.status = DISABLE_NOTIFICATIONS;
+
+
+ //setSlaveDiscoverable();//DEBUG_only
+
+ }//SLAVE-ROLE
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* Function called on connection */
+void connectionCallback(const Gap::ConnectionCallbackParams_t *params) {
+ //printf("\r\n----> connectionCallback\n\n");//DEBUG
+
+ BLE& ble = BLE::Instance();
+
+ static const size_t ADDR_LEN = 6;
+ //the current address
+ BLEProtocol::AddressBytes_t tempAddr;
+ //fill the address
+ for(unsigned i=0;i<ADDR_LEN;i++) {
+ tempAddr[i]=params->peerAddr[i];
+ }
+
+ // connection handle
+ uint16_t handle = params ->handle;
+
+ //ROLE uint8_t role
+ // 0 = master
+ // 1 = slave
+
+ uint8_t index = perDevs.connDeviceIdx;
+
+
+
+
+ /* MASTER ROLE */
+ if (role == 0) {
+
+ if (perDevs.is_unconnectable[index] == false) {
+ perDevs.connection_handle[index] = handle;
+ perDevs.is_disconnected[index] = false;
+
+ printf("\r\n\nConnected to peripheral: [%02x %02x %02x %02x %02x %02x] (%d/%d - 0x%04x) (role: %s)\n",
+ tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2],
+ tempAddr[1], tempAddr[0], index+1, perDevs.connDevices+1, handle, (role == 1 ? "slave" : "master"));
+
+ perDevs.status = DEVICE_CONNECTED;
+
+ } else {
+ perDevs.is_unconnectable[index] = false;
+ perDevs.device_found = false;
+ perDevs.status = DEVICE_NOT_FOUND;
+ }
+
+ }//if-MASTER
+
+
+
+
+ /* SLAVE ROLE */
+ if (role == 1) {
+ //When connected the adv seams disabled
+ slaveDev.conn_handle = handle;
+ slaveDev.is_connected = true;
+ slaveDev.is_discoverable = false;
+
+
+ printf("\r\n\nConnected to master: [%02x %02x %02x %02x %02x %02x] (role: %s) (adv: %d)\n",
+ tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2],
+ tempAddr[1], tempAddr[0], (role == 1 ? "slave" : "master"), ble.gap().getState().advertising);
+
+ }//if-SLAVE
+
+
+ role = 1;
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* Service discovery */
+void discoverServices(void){
+
+ BLE& ble = BLE::Instance();
+ uint8_t index = perDevs.connDeviceIdx;
+ uint16_t conn_handle = perDevs.connection_handle[index];
+ ble_error_t e0;
+
+ printf("\r\nDiscovering services for peripheral %d (0x%04x)\n", index+1, conn_handle);
+
+ if (perDevs.is_disconnected[index]){
+ printf("\r\nPeripheral %d (0x%04x) disconnected\n", index+1, conn_handle);
+ eventQ.call(setNewStatus);
+
+ }else {
+ ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
+ e0 = ble.gattClient().launchServiceDiscovery(conn_handle, serviceDiscoveryCallback,
+ characteristicDiscoveryCallback);
+ if (e0 != BLE_ERROR_NONE){
+ printf("\r\nError while discovering primary services (error: %d)\n", e0);
+ eventQ.call(setNewStatus);
+ }
+ }//if-else
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+void onStopScan(Gap::TimeoutSource_t t){
+ //printf("\r\nScanning ends\n");//DEBUG
+
+
+ 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);
+
+ //HAL_Delay(100);
+
+ discoveryCompleted = 1;
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+void startDiscovery(void) {
+ //printf("\r\nstartDiscovery - status: %d\n", perDevs.status);//DEBUG
+
+
+
+
+ BLE &ble = BLE::Instance();
+ ble_error_t e1;
+
+
+//if-MAIN
+ if ((perDevs.status == CONN_INIT) && (discoveryCompleted == 1) && (advEnds == 1)){
+
+ discoveryCompleted = 0;
+
+ /* if 1 to enable
+ * if 0 default because of the adv timer setup */
+
+#if 0
+ if ((slaveDev.is_connected == false) && (slaveDev.is_discoverable == true)){
+ //printf("\r\nslaveDev.is_discoverable == true\n");//DEBUG
+
+ ble_error_t e0;
+ e0= ble.gap().stopAdvertising();
+ if ( e0 == BLE_ERROR_NONE ) {
+ }else {
+ printf("\r\nERROR stopping advertising\n");//DEBUG
+ }
+ slaveDev.is_discoverable = false;
+ }//stopAdv
+#endif
+
+
+
+ /* MASTER ROLE */
+ perDevs.status = START_DEVICE_DISCOVERY;
+ printf("\r\nStart Device Discovery (%d)\n", perDevs.status);
+
+ e1 = ble.gap().startScan(advertisementCallback);
+
+ 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
+ //doing something
+ return;
+ }
+
+ }//if-MAIN
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* Function called as response of advertising */
+void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) { //defined in Gap.h
+ //printf("\r----> advertisementCallback\n");//DEBUG
+
+ static const size_t ADDR_LEN = 6;
+ //the current address
+ BLEProtocol::AddressBytes_t tempAddr;
+ //fill the address
+ for(unsigned i=0;i<ADDR_LEN;i++) {
+ tempAddr[i]=params->peerAddr[i];
+ }
+
+
+ uint8_t alreadyIn = 1;
+ uint8_t index;
+ //uint8_t i;
+ uint8_t found_zero_pos = 0;
+ BLEProtocol::AddressBytes_t zeroAddr;
+ memset(zeroAddr, 0, 6);
+
+
+
+if (perDevs.status != DEVICE_FOUND) {
+
+
+ for (uint8_t i = 0; i < params->advertisingDataLen; ++i) {
+
+ const uint8_t record_length = params->advertisingData[i];
+ if (record_length == 0) {
+ continue;
+ }
+ const uint8_t type = params->advertisingData[i + 1];
+ const uint8_t* value = params->advertisingData + i + 2;
+ const uint8_t value_length = record_length - 1;
+
+
+
+
+
+
+
+ /* Switch because per NAMES */
+ if(type == GapAdvertisingData::COMPLETE_LOCAL_NAME) {
+
+
+
+ /* Switch the peripheral nodes */
+
+ // 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;
+ uint8_t wup_event = 0;
+ uint8_t mic_event = 0;
+ uint8_t prx_event = 0;
+ uint8_t agm_event = 0;
+ uint8_t sfusion_event = 0;
+
+ uint8_t peripheral_name_len = params->advertisingData[3];
+ const uint8_t * manuf_data = params->advertisingData+4+peripheral_name_len;
+ uint32_t features = 0;
+
+
+ features = (manuf_data[4]<<24) | (manuf_data[5]<<16) | (manuf_data[6]<<8) | manuf_data[7];
+ wup_event = (features & FEATURE_MASK_WAKEUP_EVENTS) >> WUP_POS;
+ mic_event = (features & FEATURE_MASK_MIC) >> MIC_POS;
+ prx_event = (features & FEATURE_MASK_PROX) >> PRX_POS;
+ agm_event = (features & FEATURE_MASK_ACC) >> ACC_POS;
+ sfusion_event = (features & FEATURE_MASK_SENSORFUSION) >> SFUS_POS;
+ peripheral_v = NODE_ME1;//MOTENV
+
+
+ for (i=0; i<MAX_NUM_OF_NODES; i++) {
+ if (perDevs.is_disconnected[i]) {
+ if (memcmp(zeroAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
+ if (!found_zero_pos) {
+ index = i;
+ perDevs.connDeviceIdx = i;
+ found_zero_pos = 1;
+ alreadyIn = 0;
+ }
+ }
+ else if (memcmp(tempAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
+ index = i;
+ perDevs.connDeviceIdx = i;
+ alreadyIn = 0;
+ perDevs.discDevices--;
+ break;
+ }
+ else {
+ if (!found_zero_pos) {
+ index = i;
+ perDevs.connDeviceIdx = i;
+ alreadyIn = 0;
+ }
+ }
+ }
+ }//for
+
+
+ if ((!alreadyIn) && (perDevs.connDevices < MAX_NUM_OF_NODES)) {
+
+
+ /* Save the found peripheral device in the struct containing all the found peripheral devices */
+ saveDeviceFound(params->type, tempAddr, params->advertisingDataLen, params->advertisingData, index,
+ peripheral_v, wup_event, mic_event, prx_event, agm_event, sfusion_event);
+
+ perDevs.status = DEVICE_FOUND;
+
+ printf("\r\nPeripheral %d inserted (%02x%02x%02x%02x%02x%02x) \n", perDevs.connDevices+1,
+ tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2], tempAddr[1], tempAddr[0]);
+
+ }//if-alreadyIn
+
+
+ }//IF-MOTENV
+
+
+
+
+ // 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;
+ uint8_t mic_event = 0;
+ uint8_t prx_event = 0;
+ uint8_t agm_event = 0;
+ uint8_t sfusion_event = 0;
+
+ uint8_t peripheral_name_len = params->advertisingData[3];
+ const uint8_t * manuf_data = params->advertisingData+4+peripheral_name_len;
+ uint32_t features = 0;
+
+
+ features = (manuf_data[4]<<24) | (manuf_data[5]<<16) | (manuf_data[6]<<8) | manuf_data[7];
+ wup_event = (features & FEATURE_MASK_WAKEUP_EVENTS) >> WUP_POS;
+ mic_event = (features & FEATURE_MASK_MIC) >> MIC_POS;
+ prx_event = (features & FEATURE_MASK_PROX) >> PRX_POS;
+ agm_event = (features & FEATURE_MASK_ACC) >> ACC_POS;
+ sfusion_event = (features & FEATURE_MASK_SENSORFUSION) >> SFUS_POS;
+ peripheral_v = NODE_FL1;//FLIGHT
+
+
+
+ for (i=0; i<MAX_NUM_OF_NODES; i++) {
+ if (perDevs.is_disconnected[i]) {
+ if (memcmp(zeroAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
+ if (!found_zero_pos) {
+ index = i;
+ perDevs.connDeviceIdx = i;
+ found_zero_pos = 1;
+ alreadyIn = 0;
+ }
+ }
+ else if (memcmp(tempAddr, perDevs.devInfo[i].bdaddr, 6)==0) {
+ index = i;
+ perDevs.connDeviceIdx = i;
+ alreadyIn = 0;
+ perDevs.discDevices--;
+ break;
+ }
+ else {
+ if (!found_zero_pos) {
+ index = i;
+ perDevs.connDeviceIdx = i;
+ alreadyIn = 0;
+ }
+ }
+ }
+ }//for
+
+
+ if ((!alreadyIn) && (perDevs.connDevices < MAX_NUM_OF_NODES)) {
+
+
+ /* Save the found peripheral device in the struct containing all the found peripheral devices */
+ saveDeviceFound(params->type, tempAddr, params->advertisingDataLen, params->advertisingData, index,
+ peripheral_v, wup_event, mic_event, prx_event, agm_event, sfusion_event);
+
+ perDevs.status = DEVICE_FOUND;
+
+ printf("\r\nPeripheral %d inserted (%02x%02x%02x%02x%02x%02x) \n", perDevs.connDevices+1,
+ tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2], tempAddr[1], tempAddr[0]);
+ }//if-alreadyIn
+
+ }//IF-FLIGHT
+
+
+
+
+
+
+
+ // 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
+ uint8_t peripheral_v;
+ uint8_t wup_event = 0;
+ uint8_t mic_event = 0;
+ uint8_t prx_event = 0;
+ uint8_t agm_event = 0;
+ uint8_t sfusion_event = 0;
+
+ uint8_t peripheral_name_len = params->advertisingData[3];
+ const uint8_t * manuf_data = params->advertisingData+4+peripheral_name_len;
+ uint32_t features = 0;
+
+
+ features = (manuf_data[4]<<24) | (manuf_data[5]<<16) | (manuf_data[6]<<8) | manuf_data[7];
+ wup_event = (features & FEATURE_MASK_WAKEUP_EVENTS) >> WUP_POS;
+ mic_event = (features & FEATURE_MASK_MIC) >> MIC_POS;
+ prx_event = (features & FEATURE_MASK_PROX) >> PRX_POS;
+ agm_event = (features & FEATURE_MASK_ACC) >> ACC_POS;
+ sfusion_event = (features & FEATURE_MASK_SENSORFUSION) >> SFUS_POS;
+ peripheral_v = NODE_AM1;//ALLMEMS
+
+
+ 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;
+ alreadyIn = 0;
+ }
+ }
+ 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;
+ perDevs.discDevices--;
+ 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;
+ }
+ }
+ }
+ }//for
+
+
+ if ((!alreadyIn) && (perDevs.connDevices < MAX_NUM_OF_NODES)) {
+
+
+ /* Save the found peripheral device in the struct containing all the found peripheral devices */
+ saveDeviceFound(params->type, tempAddr, params->advertisingDataLen, params->advertisingData, index,
+ peripheral_v, wup_event, mic_event, prx_event, agm_event, sfusion_event);
+
+ perDevs.status = DEVICE_FOUND;
+
+
+ printf("\r\nPeripheral %d inserted (%02x%02x%02x%02x%02x%02x) \n", perDevs.connDevices+1,
+ tempAddr[5], tempAddr[4], tempAddr[3], tempAddr[2], tempAddr[1], tempAddr[0]);
+
+ }//if-alreadyIn
+
+
+ }//IF-ALLMEMS
+
+
+ }//IF-typeCOMPLETE_LOCAL_NAME
+
+
+ i += record_length;
+ }//for-MAIN
+
+}//if status != DEVICE_FOUND
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* Function called after the discovery of a service */
+void serviceDiscoveryCallback(const DiscoveredService *service) {
+ //printf("\r\n\n----> serviceDiscoveryCallback\n\n");//DEBUG
+ printf("\r\n");
+
+
+ //printf("\r\nstatus: SERVICE_DISCOVERED\n\n");
+ perDevs.status = SERVICE_DISCOVERED;
+
+ //length of the long uuid
+ unsigned uuidLength = UUID::LENGTH_OF_LONG_UUID; //16
+ //pointer to the current service uuid
+ const uint8_t * pointerToUUID = service->getUUID().getBaseUUID();
+ //temp long array
+ UUID::LongUUIDBytes_t tempUUID;
+ //temp short array
+ UUID::ShortUUIDBytes_t shortTempUUID;
+
+
+ /* Switch the known services */
+ //create the temp array
+ for (unsigned i=0;i<uuidLength;i++){
+ tempUUID[i]=pointerToUUID[i];
+ }
+
+
+
+ /* Switch the known services */
+ // HARDWARE_SERVICE_UUID
+ if ((memcmp(tempUUID, HARDWARE_SERVICE_UUID, uuidLength)) == 0){
+ printf("\r\nHARDWARE service: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", tempUUID[i]);
+ }
+
+ //handles
+ perDevs.hardware_service_handle[perDevs.connDeviceIdx].start_h = service->getStartHandle();
+ perDevs.hardware_service_handle[perDevs.connDeviceIdx].end_h = service->getEndHandle();
+
+ //status HW
+ perDevs.status = START_HARDWARE_SERV_CHARS_DISCOVERY;
+ //prinft("\r\nstatus = START_HARDWARE_SERV_CHARS_DISCOVERY\n");
+
+
+
+
+
+
+ // CONFIG_SERVICE_UUID
+ }else if ((memcmp(tempUUID, CONFIG_SERVICE_UUID, uuidLength)) == 0){
+ printf("\r\nCFG service: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", tempUUID[i]);
+ }
+
+ //handles
+ perDevs.configuration_service_handle[perDevs.connDeviceIdx].start_h = service->getStartHandle();
+ perDevs.configuration_service_handle[perDevs.connDeviceIdx].end_h = service->getEndHandle();
+
+ //status CFG
+ perDevs.status = START_CONFIGURATION_SERV_CHARS_DISCOVERY;
+ //prinft("\r\nstatus = START_CONFIGURATION_SERV_CHARS_DISCOVERY\n");
+
+
+
+
+
+ // SOFTWARE_SERVICE_UUID
+ }else if ((memcmp(tempUUID, SOFTWARE_SERVICE_UUID, uuidLength)) == 0){
+ printf("\r\nSOFTWARE service: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", tempUUID[i]);
+ }
+
+ //handles
+ perDevs.software_service_handle[perDevs.connDeviceIdx].start_h = service->getStartHandle();
+ perDevs.software_service_handle[perDevs.connDeviceIdx].end_h = service->getEndHandle();
+
+ //status SW
+ perDevs.status = START_SOFTWARE_SERV_CHARS_DISCOVERY;
+ //prinft("\r\nstatus = START_SOFTWARE_SERV_CHARS_DISCOVERY\n");
+
+
+
+
+
+ // Short UUID services
+ } else if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
+ //get the short uuid
+ shortTempUUID = service->getUUID().getShortUUID();
+// printf("\r\n\n\nS UUID [ %x ", service->getUUID().getShortUUID());//DEBUG
+
+
+
+
+
+
+
+ // GENERIC_ACCESS_PROFILE_UUID
+ if (shortTempUUID == GENERIC_ACCESS_PROFILE_UUID) {
+ printf("\r\nGeneric Access Profile (GAP): [ %x ", shortTempUUID);
+
+ //handles
+ perDevs.gen_access_profile_handle[perDevs.connDeviceIdx].start_h = service->getStartHandle();
+ perDevs.gen_access_profile_handle[perDevs.connDeviceIdx].end_h = service->getEndHandle();
+
+
+
+
+
+
+
+ // GENERIC_ATTRIBUTE_PROFILE_UUID
+ } else if (shortTempUUID == GENERIC_ATTRIBUTE_PROFILE_UUID) {
+ printf("\r\nGeneric Attribute Profile (GATT): [ %x ", shortTempUUID);
+
+ //handles
+ perDevs.gen_attribute_profile_handle[perDevs.connDeviceIdx].start_h = service->getStartHandle();
+ perDevs.gen_attribute_profile_handle[perDevs.connDeviceIdx].end_h = service->getEndHandle();
+
+
+
+
+
+
+ // UNKNOWN short service
+ } else {
+ printf("\r\nUNKNOWN service: [ %x", shortTempUUID);
+ }
+
+
+
+
+
+
+ // UNKNOWN long service
+ } else {
+ printf("\r\nUNKNOWN service: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", tempUUID[i]);
+ }
+ }
+
+
+
+
+ printf("]");
+ printf("\r\n\n");
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+
+/* Function called after the discovery of a characteristic */
+void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP) {
+ //printf("\r----> characteristicDiscoveryCallback\n\n");//DEBUG
+
+ //current characteristic handles
+ uint16_t declHandle = characteristicP->getDeclHandle(); //identify chars
+ uint16_t valueHandle = characteristicP->getValueHandle();
+
+
+ //-----> 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);
+
+
+
+ //length of the long uuid
+ unsigned uuidLength = UUID::LENGTH_OF_LONG_UUID; //16
+ //pointer to the current service uuid
+ const uint8_t * pointerToUUID = characteristicP->getUUID().getBaseUUID();
+ //temp array
+ UUID::LongUUIDBytes_t tempUUID;
+
+ //create the temp uuid array
+ for (unsigned i=0;i<uuidLength;i++){
+ tempUUID[i]=pointerToUUID[i];
+ }
+
+
+
+
+
+
+
+ /* switch the known characteristic */
+ // ENVIRONMENTAL_CHAR_UUID
+ // ENVIRONMENTAL_ST_CHAR_UUID
+ if (((memcmp(tempUUID, ENVIRONMENTAL_CHAR_UUID, uuidLength)) == 0) ||
+ ((memcmp(tempUUID, ENVIRONMENTAL_ST_CHAR_UUID, uuidLength)) == 0)){
+
+ //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;
+
+ printf("\r\n - Enviromental Ch: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", pointerToUUID[i]);
+ }
+
+
+
+
+
+
+
+ // ACCGYROMAG_CHAR_UUID
+ }else if ((memcmp(tempUUID, ACCGYROMAG_CHAR_UUID, uuidLength)) == 0){
+
+ //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;
+
+
+ printf("\r\n - Acc-Gyro-Mag Ch: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", pointerToUUID[i]);
+ }
+
+
+
+
+
+
+
+ // SFUSION_CHAR_UUID
+ }else if ((memcmp(tempUUID, SFUSION_CHAR_UUID, uuidLength)) == 0){
+
+ //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
+ perDevs.sfusion_char_read[idx] = 0;
+ perDevs.sfusion_on[idx] = 0;
+
+
+
+ printf("\r\n - SFusion Ch: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", pointerToUUID[i]);
+ }
+
+
+
+
+
+
+ // LED_CHAR_UUID
+ }else if ((memcmp(tempUUID, LED_CHAR_UUID, uuidLength)) == 0){
+
+ //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
+ perDevs.led_char_read[idx] = 0;
+ printf("\r\n - Led Ch: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", pointerToUUID[i]);
+ }
+
+
+
+
+
+ // WAKEUP_EVENT_CHAR_UUID
+ }else if ((memcmp(tempUUID, WAKEUP_EVENT_CHAR_UUID, uuidLength)) == 0){
+
+ //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
+ perDevs.wup_char_read[idx] = 0;
+ printf("\r\n - Wakeup Event Ch: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", pointerToUUID[i]);
+ }
+
+
+
+
+
+
+ // MIC_EVENT_CHAR_UUID
+ }else if ((memcmp(tempUUID, MIC_EVENT_CHAR_UUID, uuidLength)) == 0){
+
+ //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
+ perDevs.mic_char_read[idx] = 0;
+ printf("\r\n - Mic Event Ch: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", pointerToUUID[i]);
+ }
+
+
+
+
+
+ // PROXIMITY_CHAR_UUID
+ }else if ((memcmp(tempUUID, PROXIMITY_CHAR_UUID, uuidLength)) == 0){
+
+ //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
+ perDevs.prx_char_read[idx] = 0;
+ perDevs.prx_on[idx] = 0;
+ printf("\r\n - Proximity Ch: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", pointerToUUID[i]);
+ }
+
+
+
+
+
+
+ // LUX_CHAR_UUID
+ }else if ((memcmp(tempUUID, LUX_CHAR_UUID, uuidLength)) == 0){
+
+ //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;
+
+
+ printf("\r\n - Lux Ch: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", pointerToUUID[i]);
+ }
+
+
+
+
+
+
+ // CONFIG_CHAR_UUID
+ }else if ((memcmp(tempUUID, CONFIG_CHAR_UUID, uuidLength)) == 0){
+
+ //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;
+
+ printf("\r\n - Config Ch: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", pointerToUUID[i]);
+ }
+
+
+
+
+
+ // UNKNOWN
+ }else {
+
+ printf("\r\n - Unknown Ch: [ ");
+
+ for (unsigned i = 0; i < uuidLength; i++) {
+ printf("%02x ", pointerToUUID[i]);
+ }
+ }
+
+
+
+
+ printf("] ");
+ printf("(declH 0x%04x), r: %x\n", declHandle, (uint8_t)characteristicP->getProperties().read());
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Function called when discovery is terminated */
+void discoveryTerminationCallback(Gap::Handle_t connectionHandle) {
+ //printf("\r----> discoveryTerminationCallback\n\n");//DEBUG
+
+ uint8_t * pointerToCustomDev_v;
+ pointerToCustomDev_v = &customDev_v;
+
+ uint8_t idx = perDevs.connDeviceIdx; //idx
+
+ printf("\r\n\n%d characteristics discovered and added into the DCList \n", countElements(headCharacteristic[idx]));
+ printf("\r\nTerminated SERVICE DISCOVERY for Handle 0x%04x\n\n", connectionHandle);
+
+
+
+
+////Enable notifications after the discovery of the configuration characteristics
+////------------
+ if (!perDevs.is_disconnected[perDevs.connDeviceIdx]) {
+ *pointerToCustomDev_v = perDevs.devInfo[perDevs.connDeviceIdx].dev_v;
+
+ if (perDevs.devInfo[perDevs.connDeviceIdx].dev_v == NODE_ME1) {
+ perDevs.status = ENABLE_ME1_LED_NOTIFICATIONS;
+
+ }else if (perDevs.devInfo[perDevs.connDeviceIdx].dev_v == NODE_AM1) {
+ perDevs.status = NOTIFICATIONS_ENABLED;
+
+ }else if (perDevs.devInfo[perDevs.connDeviceIdx].dev_v == NODE_FL1) {
+ perDevs.status = NOTIFICATIONS_ENABLED;
+ }
+ } else {
+ perDevs.status = NOTIFICATIONS_ENABLED;
+ }
+
+
+////------------
+
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+/* Method called when there is a notification from the sever */
+void onNotificationCallback(const GattHVXCallbackParams* event){
+ //printf("\r\nonNotificationCallback\n");//DEBUG
+
+ /* GATT Notification params */
+ uint16_t conn_handle = event->connHandle;
+ uint16_t attr_handle = event->handle;
+ uint8_t attr_len = event->len;
+ uint8_t *attr_value = (uint8_t *)event->data;
+
+
+ int32_t tmp = 0;
+ uint8_t tmp_data[256];
+ uint8_t notify = 0;
+ tBDAddr devAddr;
+ uint8_t index;
+ uint16_t slave_attr_handle;
+ static uint8_t wupevt_value = 0;
+
+ /* Retrieving the peripheral device index and address from the connection handle */
+ getDeviceFromConnHandle(conn_handle, &index, devAddr);
+ slave_attr_handle = slaveDev.star_data_char_handle;
+
+
+
+ /* Build buffer in JSON format */
+ if (attr_handle == perDevs.wup_char_handle[index]+1) {
+
+ /**
+ * The motion is detected when a WakeUp event notification is received.
+ * The received data is composed by 2 bytes for the tstamp + 2 bytes
+ * for the value. Since we transmit only information about the detected
+ * motion, we don't care of the last 2 bytes.
+ */
+ if (attr_len != TSTAMP_LEN+2)
+ printf("\r\n(NC)WUP Warning: received data length is %d (while expecting %d)\n", attr_len, (TSTAMP_LEN+2));
+
+ wupevt_value = !wupevt_value;
+ tmp = wupevt_value;
+ sprintf((char *)wifi_data, "\"WUpEvt_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+ notify = 1;
+
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | WakeUp Type ID |
+ * 2 bytes | 2 bytes | 1 byte |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, WUP_TYPE_ID, NULL, WUP_DATA_LEN);
+
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ perDevs.status = ALL_DATA_READ;
+ notifyMaster(ATTR_HEAD_LEN+WUP_DATA_LEN, star_attr_value, slave_attr_handle);
+ }
+
+
+ } else if (attr_handle == perDevs.mic_char_handle[index]+1) {
+
+ /* The level in dB (the mic level is detected when a mic event notification is received) */
+ /* The Sensor Tile board has only Mic1 so it sends 3 bytes */
+ if ((attr_len != TSTAMP_LEN+(2*MIC_DATA_LEN)) && (attr_len != TSTAMP_LEN+MIC_DATA_LEN))
+ printf("\r\n(NC)MIC Warning: received data length is %d (while expecting %d or %d)\n", attr_len, (TSTAMP_LEN+MIC_DATA_LEN), (TSTAMP_LEN+(2*MIC_DATA_LEN)));
+
+ tmp = attr_value[2];
+ sprintf((char *)wifi_data, "\"Mic1_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+ /* We do not send the audio level from Mic2 anyway */
+ //tmp = attr_value[3];
+ //sprintf((char *)tmp_data, ",\"Mic2_0x%02x%02x\":%d", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+ //strcat((char *)wifi_data, (char *)tmp_data);
+ notify = 1;
+
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | MIC Event Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 1 byte |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, MICLEVEL_TYPE_ID, attr_value+TSTAMP_LEN, MIC_DATA_LEN);
+
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ perDevs.status = ALL_DATA_READ;
+ notifyMaster(ATTR_HEAD_LEN+MIC_DATA_LEN, star_attr_value, slave_attr_handle);
+ }
+
+
+ } else if (attr_handle == perDevs.prx_char_handle[index]+1) {
+ /* the distance value (in mm) */
+ if (attr_len != TSTAMP_LEN+PRX_DATA_LEN)
+ printf("\r\n(NC)PRX Warning: received data length is %d (while expecting %d)\n", attr_len, (TSTAMP_LEN+PRX_DATA_LEN));
+
+ tmp = (attr_value[3]<<8) | attr_value[2];
+ sprintf((char *)wifi_data, "\"PRX_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+ notify = 1;
+
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | Proximity Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 2 bytes |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, PRX_TYPE_ID, attr_value+TSTAMP_LEN, PRX_DATA_LEN); /* Timestamp, Node ID, Proximity Type ID, data, len */
+
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ perDevs.status = ALL_DATA_READ;
+ notifyMaster(ATTR_HEAD_LEN+PRX_DATA_LEN, star_attr_value, slave_attr_handle);
+ }
+
+
+ } else if (attr_handle == perDevs.agm_char_handle[index]+1) {
+ uint8_t type_id;
+ uint8_t data_len;
+ uint8_t value_offset;
+
+ /* Acc values in mg, Gyro values in dps, Mag values in mGa */
+ if (attr_len != TSTAMP_LEN+MEMS_DATA_LEN)
+ printf("\r\n(NC)AGM Warning: received data length is %d (while expecting %d)\n", attr_len, (TSTAMP_LEN+MEMS_DATA_LEN));
+
+ if (perDevs.acc_event_enabled) {
+ Build_MEMS_Packet(attr_value+2, "Acc", devAddr, INT_VALUE);
+ type_id = ACC_TYPE_ID;
+ data_len = ACC_DATA_LEN;
+ value_offset = TSTAMP_LEN;
+
+ } else if (perDevs.gyr_event_enabled) {
+ Build_MEMS_Packet(attr_value+8, "Gyr", devAddr, FLOAT_VALUE);
+ type_id = GYR_TYPE_ID;
+ data_len = GYR_DATA_LEN;
+ value_offset = TSTAMP_LEN+ACC_DATA_LEN;
+
+ } else if (perDevs.mag_event_enabled) {
+ Build_MEMS_Packet(attr_value+14, "Mag", devAddr, INT_VALUE);
+ type_id = MAG_TYPE_ID;
+ data_len = MAG_DATA_LEN;
+ value_offset = TSTAMP_LEN+ACC_DATA_LEN+GYR_DATA_LEN;
+ }
+ notify = 1;
+
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | Acc/Gyro/Mag Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 6 bytes |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, type_id, attr_value+value_offset, data_len);
+
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) && (perDevs.agm_on[index])) {
+ perDevs.status = NOTIFICATIONS_DATA_READ;
+ notifyMaster(ATTR_HEAD_LEN+data_len, star_attr_value, slave_attr_handle);
+
+ } else {
+ perDevs.status = ALL_DATA_READ;
+ }
+
+
+
+ } else if (attr_handle == perDevs.sfusion_char_handle[index]+1) {
+
+ /* Quaternion values */
+ tmp = (int8_t)attr_value[3]<<8 | attr_value[2];
+ sprintf((char *)wifi_data, "\"Q1_0x%02x%02x\":%s0.%.4d; ", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], (tmp<0 ? "-" : ""), (tmp<0 ? -tmp : tmp));
+ tmp = (int8_t)attr_value[5]<<8 | attr_value[4];
+ sprintf((char *)tmp_data, "\"Q2_0x%02x%02x\":%s0.%.4d; ", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], (tmp<0 ? "-" : ""), (tmp<0 ? -tmp : tmp));
+ strcat((char *)wifi_data, (char *)tmp_data);
+ tmp = (int8_t)attr_value[7]<<8 | attr_value[6];
+ sprintf((char *)tmp_data, "\"Q3_0x%02x%02x\":%s0.%.4d; ", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], (tmp<0 ? "-" : ""), (tmp<0 ? -tmp : tmp));
+ strcat((char *)wifi_data, (char *)tmp_data);
+ notify = 1;
+
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | SFusion Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 6 bytes |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, SFUSION_TYPE_ID, attr_value+TSTAMP_LEN, SFUSION_DATA_LEN);
+
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) && (perDevs.sfusion_on[index])) {
+ perDevs.status = NOTIFICATIONS_DATA_READ;
+ notifyMaster(ATTR_HEAD_LEN+SFUSION_DATA_LEN, star_attr_value, slave_attr_handle);
+ perDevs.sfusion_char_read[index] = 1;
+
+ } else {
+ perDevs.status = ALL_DATA_READ;
+ }
+ } else if (attr_handle == perDevs.environmental_char_handle[index]+1) {
+ /* P value in mBar, H value in percentage, T2 and T1 values in Celtius degree */
+ if (attr_len != TSTAMP_LEN+ENV_DATA_LEN)
+ printf("\r\n(NC)SFUSION Warning: received data length is %d (while expecting %d)\n", attr_len, (TSTAMP_LEN+ENV_DATA_LEN));
+
+ tmp = (attr_value[5]<<24) | (attr_value[4]<<16) | (attr_value[3]<<8) | attr_value[2];
+ sprintf((char *)wifi_data, "\"Pressure_0x%02x%02x\":%ld.%ld,", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp/100, tmp%100);
+ tmp = (attr_value[7]<<8) | attr_value[6];
+ sprintf((char *)tmp_data, "\"Humidity_0x%02x%02x\":%ld.%ld,", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp/10, tmp%10);
+ strcat((char *)wifi_data, (char *)tmp_data);
+
+ /* Showing only one Temperature from the same peripheral node */
+ //tmp = (attr_value[9]<<8) | attr_value[8];
+ //sprintf((char *)tmp_data, "\"Temperature2_0x%02x%02x\":%d.%d,", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp/10, tmp%10);
+ //strcat((char *)wifi_data, (char *)tmp_data);
+ tmp = (attr_value[11]<<8) | attr_value[10];
+ sprintf((char *)tmp_data, "\"Temperature1_0x%02x%02x\":%ld.%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp/10, tmp%10);
+ strcat((char *)wifi_data, (char *)tmp_data);
+ notify = 1;
+
+ /*
+ * Modify the attribute value according to the following format:
+ * Tstamp | Node ID | P Type ID | value | H Type ID | value | T Type ID | value | T Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 4 bytes | 1 byte | 2 bytes | 1 byte | 2 bytes | 1 byte | 2 bytes |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, PRESS_TYPE_ID, attr_value+TSTAMP_LEN, attr_len-TSTAMP_LEN);
+
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ 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 if (attr_handle == perDevs.led_char_handle[index]+1) {
+ /* the status (0x01 = ON, 0x00 = OFF) */
+ if (attr_len != TSTAMP_LEN+LED_DATA_LEN)
+ printf("\r\n(NC)LED Warning: received data length is %d (while expecting %d)\n", attr_len, (TSTAMP_LEN+LED_DATA_LEN));
+
+ tmp = attr_value[2];
+ sprintf((char *)wifi_data, "\"LED_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+ notify = 1;
+
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | LED Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 1 byte |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, LED_TYPE_ID, attr_value+TSTAMP_LEN, LED_DATA_LEN);
+
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ if (perDevs.led_char_read[index]==1) {
+ perDevs.status = ALL_DATA_READ;
+ }
+ notifyMaster(ATTR_HEAD_LEN+LED_DATA_LEN, star_attr_value, slave_attr_handle);
+ }
+
+
+ } else if (attr_handle == perDevs.lux_char_handle[index]+1) {
+ /* Lux value */
+ if (attr_len != TSTAMP_LEN+LUX_DATA_LEN)
+ printf("\r\n(NC)LUX Warning: received data length is %d (while expecting %d)\n", attr_len, (TSTAMP_LEN+LUX_DATA_LEN));
+
+ tmp = (attr_value[3]<<8) | attr_value[2];
+ sprintf((char *)wifi_data, "\"LUX_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+ notify = 1;
+
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | LUX Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 2 bytes |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, LUX_TYPE_ID, attr_value+TSTAMP_LEN, LUX_DATA_LEN);
+
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ notifyMaster(ATTR_HEAD_LEN+LUX_DATA_LEN, star_attr_value, slave_attr_handle);
+ }
+
+
+ }//if-else-MAIN
+
+ if (notify == 1) {
+ strcat((char *)wifi_data, "\n");
+
+ if (discoveryCompleted == 1){
+ printf("\r\nNOTIFICATION => %s", (char *)wifi_data); /* print locally the buffer in JSON format */
+ }
+
+ data = wifi_data;
+ if ((attr_handle!=perDevs.sfusion_char_handle[index]+1) && (attr_handle!=perDevs.agm_char_handle[index]+1))
+ new_data = 1;
+ }//notify=1
+
+ if (readCompleted != 1){
+ readCompleted = 1;
+ }
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/**
+ * @brief Enable characteristic notifications
+ * @param Connection Handle
+ * @param Position of the peripheral in the struct containing all peripherals
+ * @param Peripheral node type
+ * @param uint8_t indicating if the notification has to be enabled (1) or disabled (0)
+ * @retval None
+ */
+void enableNotifications(uint16_t conn_handle, uint8_t index, uint8_t dev_v, uint8_t set_mode) {
+//printf("\r\nenableNotifications\n");//DEBUG
+
+
+ uint16_t attr_handle;
+ uint8_t attr_value[6]={0,0,0,0,0,0};
+ uint8_t attr_len;
+
+
+ const char * event_type;
+
+ switch (perDevs.status) {
+ case ENABLE_ME1_LED_NOTIFICATIONS:
+ {
+ event_type = "LED";
+ attr_handle = perDevs.led_char_handle[index] + 2;
+ }
+ break;
+
+ case ENABLE_ME1_WUP_NOTIFICATIONS:
+ {
+ event_type = "WUPEVT";
+
+ // Enable/Disable the WakeUp notifications on the Motenv1
+ setNotificationProperty(conn_handle, index, FEATURE_MASK_WAKEUP_EVENTS, WAKEUP_NOTIFICATION_CMD, set_mode);
+
+ attr_handle = perDevs.wup_char_handle[index] + 2;
+ }
+ break;
+ default:
+ break;
+ }
+
+ attr_value[0] = set_mode;
+ attr_len = 2;
+
+
+ if (perDevs.status == ENABLE_ME1_LED_NOTIFICATIONS){
+ writeDescriptorCompleted=0;
+ printf("\r\n%s notifications set to %d for periph %d (0x%04x)\n", event_type, set_mode, index+1, conn_handle);
+ writeCharacDescriptor(conn_handle, attr_handle, attr_len, attr_value);
+
+ }
+
+ if ((perDevs.status == ENABLE_ME1_WUP_NOTIFICATIONS)/* && (writeDescriptorCompleted == 1)*/){
+ writeDescriptorCompleted=0;
+ printf("\r\n%s notifications set to %d for periph %d (0x%04x)\n", event_type, set_mode, index+1, conn_handle);
+ writeCharacDescriptor(conn_handle, attr_handle, attr_len, attr_value);
+ }
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Method called after a periph descriptor is written */
+void perDescriptorWrittenCallback(const GattWriteCallbackParams* event){
+ //printf("\r\nperDescriptorWrittenCallback\n");//DEBUG
+
+ setNewStatus();
+ writeDescriptorCompleted = 1;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Write a characteristic descriptor */
+//cmd == GattClient::GATT_OP_WRITE_REQ
+void writeCharacDescriptor(uint16_t conn_handle, uint16_t attr_handle, uint8_t attr_len,
+ uint8_t* attr_value){
+
+ ble_error_t error = BLE::Instance().gattClient().write( GattClient::GATT_OP_WRITE_REQ,
+ conn_handle,
+ attr_handle,
+ attr_len,
+ attr_value );
+
+ if (error != BLE_ERROR_NONE){
+ printf("\r\nWrite charac descriptor failed!\n");
+ writeDescriptorCompleted = 1;
+ }
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+
+/* Get error */
+ble_error_t writeCharacDescriptorWithError(uint16_t conn_handle, uint16_t attr_handle, uint8_t attr_len,
+ uint8_t* attr_value){
+
+ ble_error_t error = BLE::Instance().gattClient().write( GattClient::GATT_OP_WRITE_REQ,
+ conn_handle,
+ attr_handle,
+ attr_len,
+ attr_value );
+ return error;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* Write a characteristic value without waiting for any response */
+//cmd == GATT_OP_WRITE_CMD
+void writeCharacValueWithoutResp(uint16_t conn_handle, uint16_t attr_handle, uint8_t attr_len,
+ uint8_t* attr_value){
+
+ ble_error_t error = BLE::Instance().gattClient().write( GattClient::GATT_OP_WRITE_CMD,
+ conn_handle,
+ attr_handle,
+ attr_len,
+ attr_value );
+
+ if (error != BLE_ERROR_NONE){
+ printf("\r\nWrite charac descriptor wo resp failed!\n");
+ writeDescriptorCompleted = 1;
+ }
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/* Get error */
+ble_error_t writeCharacValueWithoutRespWithError(uint16_t conn_handle, uint16_t attr_handle, uint8_t attr_len,
+ uint8_t* attr_value){
+
+ ble_error_t error = BLE::Instance().gattClient().write( GattClient::GATT_OP_WRITE_CMD,
+ conn_handle,
+ attr_handle,
+ attr_len,
+ attr_value );
+ return error;
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+
+/**
+ * @brief This function builds a Jason format string
+ * @param attribute value
+ * @param string indicating the data type (Acc, Gyr or Mag)
+ * @param address of the device
+ * @param data type (integer or float)
+ * @retval None
+ */
+void Build_MEMS_Packet(uint8_t *attr_value, char *data_type, tBDAddr devAddr, uint8_t num_type){
+
+ int32_t tmp = 0;
+ uint8_t tmp_data[256];
+ const char* sign = "";
+ char axes[3] = {'X','Y','Z'};
+ uint8_t i, j = 0;
+
+ for (i=0; i<5; i++) {
+ tmp = ((int8_t)attr_value[i+1]<<8) | attr_value[i];
+
+ if (num_type==1) {
+ if (i==0) {
+ sprintf((char *)wifi_data, "\"%s%c_0x%02x%02x\":%ld; ", data_type, axes[j], devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+
+ } else {
+ sprintf((char *)tmp_data, "\"%s%c_0x%02x%02x\":%ld; ", data_type, axes[j], devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+ strcat ((char *)wifi_data, (char *)tmp_data);
+ }
+
+ } else {
+ sign = (tmp < 0) ? "-" : "";
+ tmp = (tmp < 0) ? (-tmp) : (tmp);
+ if (i==0) {
+ sprintf((char *)wifi_data, "\"%s%c_0x%02x%02x\":%s%ld.%ld; ", data_type, axes[j], devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], sign, tmp/10, tmp%10);
+
+ } else {
+ sprintf((char *)tmp_data, "\"%s%c_0x%02x%02x\":%s%ld.%ld; ", data_type, axes[j], devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], sign, tmp/10, tmp%10);
+ strcat ((char *)wifi_data, (char *)tmp_data);
+ }
+ }//if-MAIN
+
+ i++;
+ j++;
+
+ }//for
+}
+/*----------------------------------------------------------------------------*/
+
+
+/**
+ * @brief This function creates, from an attribute value received from a
+ * peripheral node, a new attribute value to be sent to the client
+ * @param tstamp Timestamp
+ * @param data_type_id Data Type ID
+ * @param devAddr Device address
+ * @param data_length Data Length
+ * @retval None
+ */
+void Create_New_Attr_Value (uint8_t *tstamp, tBDAddr devAddr, uint8_t data_type_id, uint8_t *data, uint8_t data_length){
+ //printf("\r\nCreate_New_Attr_Value\n");//DEBUG
+
+
+
+ uint8_t* new_attr_value = star_attr_value;
+
+ memcpy(new_attr_value, tstamp, TSTAMP_LEN); /* Timestamp */
+ new_attr_value += TSTAMP_LEN;
+
+ memcpy(new_attr_value, devAddr+NODE_ID_OFFSET, NODE_ID_LEN); /* Node ID */
+ new_attr_value += NODE_ID_LEN;
+
+ memcpy(new_attr_value, &data_type_id, TYPE_ID_LEN); /* Data Type ID */
+ new_attr_value += TYPE_ID_LEN;
+
+ switch(data_type_id)
+ {
+
+ //
+ case PRESS_TYPE_ID:
+ {
+ memcpy(new_attr_value, data, PRESS_DATA_LEN); /* Pressure value */
+ new_attr_value += PRESS_DATA_LEN;
+ data += PRESS_DATA_LEN;
+
+ if (data_length == ENV_DATA_LEN_LONG-TSTAMP_LEN) {
+ data_type_id = HUM_TYPE_ID;
+
+ memcpy(new_attr_value, &data_type_id, TYPE_ID_LEN); /* Humidity Type ID */
+ new_attr_value += TYPE_ID_LEN;
+
+ memcpy(new_attr_value, data, HUM_DATA_LEN); /* Humidity value */
+ new_attr_value += HUM_DATA_LEN;
+ data += HUM_DATA_LEN;
+ data_type_id = TEMP_TYPE_ID;
+
+ memcpy(new_attr_value, &data_type_id, TYPE_ID_LEN); /* Temperature Type ID */
+ new_attr_value += TYPE_ID_LEN;
+
+ memcpy(new_attr_value, data, TEMP_DATA_LEN); /* Temperature value */
+ new_attr_value += TEMP_DATA_LEN;
+ data += TEMP_DATA_LEN;
+
+ memcpy(new_attr_value, &data_type_id, TYPE_ID_LEN); /* Temperature Type ID */
+ new_attr_value += TYPE_ID_LEN;
+ memcpy(new_attr_value, data, TEMP_DATA_LEN); /* Temperature value */
+ }
+
+ else { /* Sensor Tile */
+ data_type_id = TEMP_TYPE_ID;
+ memcpy(new_attr_value, &data_type_id, TYPE_ID_LEN); /* Temperature Type ID */
+ new_attr_value += TYPE_ID_LEN;
+ memcpy(new_attr_value, data, TEMP_DATA_LEN); /* Temperature value */
+ }
+ }
+ break;
+ case LED_TYPE_ID:
+ case MICLEVEL_TYPE_ID:
+ {
+ memcpy(new_attr_value, data, ONE_BYTE_LEN); /* LED or MIC value */
+ }
+ break;
+ case PRX_TYPE_ID:
+ case LUX_TYPE_ID:
+ {
+ memcpy(new_attr_value, data, TWO_BYTES_LEN); /* LUX or PRX value */
+ }
+ break;
+ case ACC_TYPE_ID:
+ {
+ memcpy(new_attr_value, data, X_DATA_LEN); /* ACC value */
+ new_attr_value += X_DATA_LEN;
+ data += X_DATA_LEN;
+ memcpy(new_attr_value, data, Y_DATA_LEN);
+ new_attr_value += Y_DATA_LEN;
+ data += Y_DATA_LEN;
+ memcpy(new_attr_value, data, Z_DATA_LEN);
+ new_attr_value += Z_DATA_LEN;
+ data += Z_DATA_LEN;
+ if (data_length == MEMS_DATA_LEN) {
+ data_type_id = GYR_TYPE_ID;
+ memcpy(new_attr_value, &data_type_id, TYPE_ID_LEN); /* GYR Type ID */
+ new_attr_value += TYPE_ID_LEN;
+ memcpy(new_attr_value, data, X_DATA_LEN); /* GYR value */
+ new_attr_value += X_DATA_LEN;
+ data += X_DATA_LEN;
+ memcpy(new_attr_value, data, Y_DATA_LEN);
+ new_attr_value += Y_DATA_LEN;
+ data += Y_DATA_LEN;
+ memcpy(new_attr_value, data, Z_DATA_LEN);
+ new_attr_value += Z_DATA_LEN;
+ data += Z_DATA_LEN;
+
+ data_type_id = MAG_TYPE_ID;
+ memcpy(new_attr_value, &data_type_id, TYPE_ID_LEN); /* MAG Type ID */
+ new_attr_value += TYPE_ID_LEN;
+ memcpy(new_attr_value, data, X_DATA_LEN); /* MAG value */
+ new_attr_value += X_DATA_LEN;
+ data += X_DATA_LEN;
+ memcpy(new_attr_value, data, Y_DATA_LEN);
+ new_attr_value += Y_DATA_LEN;
+ data += Y_DATA_LEN;
+ memcpy(new_attr_value, data, Z_DATA_LEN);
+ }
+ }
+ break;
+ case GYR_TYPE_ID:
+ case MAG_TYPE_ID:
+ case SFUSION_TYPE_ID:
+ {
+ memcpy(new_attr_value, data, X_DATA_LEN); /* X or Q1 value */
+ new_attr_value += X_DATA_LEN;
+ data += X_DATA_LEN;
+ memcpy(new_attr_value, data, Y_DATA_LEN); /* Y or Q2 value */
+ new_attr_value += Y_DATA_LEN;
+ data += Y_DATA_LEN;
+ memcpy(new_attr_value, data, Z_DATA_LEN); /* Z or Q3 value */
+ }
+ break;
+ default:
+ break;
+ }
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Method called after the reading of a characteristic */
+void readCharacteristicCallback(const GattReadCallbackParams *response) {
+ //printf("\r\nreadCharacteristicCallback - status (%d)\n\n", perDevs.status);//DEBUG
+
+
+
+ uint16_t handle = response->connHandle;
+ uint8_t data_length = response->len;
+ uint8_t * attr_value = (uint8_t *)response->data;
+
+
+ int32_t tmp = 0;
+ uint8_t tmp_data[256];
+ uint8_t index;
+ tBDAddr devAddr;
+ uint16_t attribute_handle; //slave
+ uint8_t new_buffer = 0;
+ getDeviceFromConnHandle(handle, &index, devAddr);
+
+
+ /* Building the buffer in JSON format */
+ switch (perDevs.status) {
+
+ case READING_ENVIRONMENTAL:
+ {
+ /* P in mBar, H in percentage, T2 and T1 value in Celtius degree */
+ if ((data_length != ENV_DATA_LEN_LONG) && (data_length != ENV_DATA_LEN_SHORT)) {
+ printf("\rENV Warning: received data length is %d (while expecting %d or %d) - status=%d\n\n", data_length, (ENV_DATA_LEN_LONG), (ENV_DATA_LEN_SHORT), perDevs.status);
+ }
+ else {
+ tmp = (attr_value[5]<<24) | (attr_value[4]<<16) | (attr_value[3]<<8) | attr_value[2];
+ sprintf((char *)wifi_data, "\"Pressure_0x%02x%02x\":%ld.%ld,", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp/100, tmp%100);
+ if (data_length == ENV_DATA_LEN_LONG) {
+ tmp = (attr_value[7]<<8) | attr_value[6];
+ sprintf((char *)tmp_data, "\"Humidity_0x%02x%02x\":%ld.%ld,", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp/10, tmp%10);
+ strcat((char *)wifi_data, (char *)tmp_data);
+ //Showing only one Temperature from the same peripheral node
+ //tmp = (attr_value[9]<<8) | attr_value[8];
+ //sprintf((char *)tmp_data, "\"Temperature2_0x%02x%02x\":%ld.%ld,", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp/10, tmp%10);
+ //strcat((char *)wifi_data, (char *)tmp_data);
+ tmp = (attr_value[11]<<8) | attr_value[10];
+ sprintf((char *)tmp_data, "\"Temperature1_0x%02x%02x\":%ld.%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp/10, tmp%10);
+ strcat((char *)wifi_data, (char *)tmp_data);
+ }
+ else { /* ENV_DATA_LEN_SHORT (that is when using the Sensor Tile) */
+ tmp = (attr_value[7]<<8) | attr_value[6];
+ sprintf((char *)tmp_data, "\"Temperature1_0x%02x%02x\":%ld.%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp/10, tmp%10);
+ strcat((char *)wifi_data, (char *)tmp_data);
+ }
+
+
+
+
+ attribute_handle = slaveDev.star_data_char_handle;
+
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ /*
+ * Modify the attribute value according to the following format:
+ * Tstamp | Node ID | P Type ID | value | H Type ID | value | T Type ID | value | T Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 4 bytes | 1 byte | 2 bytes | 1 byte | 2 bytes | 1 byte | 2 bytes |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, PRESS_TYPE_ID, attr_value+TSTAMP_LEN, data_length-TSTAMP_LEN);
+ if (data_length == ENV_DATA_LEN_LONG) {
+ slaveDev.notification_data.data_length = ATTR_HEAD_LEN+PRESS_DATA_LEN+(3*TYPE_ID_LEN)+HUM_DATA_LEN+(2*TEMP_DATA_LEN);
+ }
+ else { /* ENV_DATA_LEN_SHORT (that is when using the Sensor Tile) */
+ slaveDev.notification_data.data_length = ATTR_HEAD_LEN+PRESS_DATA_LEN+TYPE_ID_LEN+TEMP_DATA_LEN;
+ }
+
+ slaveDev.notification_data.attribute_value = star_attr_value;
+ memcpy(slaveDev.notification_data.devAddr, devAddr, 6);
+ slaveDev.notification_data.attribute_handle = attribute_handle;
+ }
+
+ new_buffer = 1;
+ }
+ }
+ break;
+
+
+
+ case READING_LED:
+ {
+ /* the status (0=OFF, 1=ON) */
+ if (data_length != TSTAMP_LEN+LED_DATA_LEN) {
+ printf("\rLED Warning: received data length is %d (while expecting %d) - status=%d\n\n", data_length, (TSTAMP_LEN+LED_DATA_LEN), perDevs.status);
+ }
+ else {
+ tmp = attr_value[2];
+ sprintf((char *)wifi_data, "\"LED_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+
+
+ attribute_handle = slaveDev.star_data_char_handle;
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | LED Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 1 byte |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, LED_TYPE_ID, attr_value+TSTAMP_LEN, LED_DATA_LEN);
+
+ slaveDev.notification_data.data_length = ATTR_HEAD_LEN+LED_DATA_LEN;
+ slaveDev.notification_data.attribute_value = star_attr_value;
+ memcpy(slaveDev.notification_data.devAddr, devAddr, 6);
+ slaveDev.notification_data.attribute_handle = attribute_handle;
+ }
+ new_buffer = 1;
+ }
+ }
+ break;
+
+
+
+ case READING_MIC:
+ {
+ tmp = 0; /* fake value used to just notify the mic presence */
+ sprintf((char *)wifi_data, "\"Mic1_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+
+ attribute_handle = slaveDev.star_data_char_handle;
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ uint8_t val = tmp;
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | MIC Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 1 byte |
+ */
+ // STORE_LE_16(star_attr_value, (HAL_GetTick()>>3)); /* Timestamp */
+ Create_New_Attr_Value(attr_value, devAddr, MICLEVEL_TYPE_ID, &val, MIC_DATA_LEN);
+
+ slaveDev.notification_data.data_length = ATTR_HEAD_LEN+MIC_DATA_LEN;
+ slaveDev.notification_data.attribute_value = star_attr_value;
+ memcpy(slaveDev.notification_data.devAddr, devAddr, 6);
+ slaveDev.notification_data.attribute_handle = attribute_handle;
+ }
+ new_buffer = 1;
+ }
+ break;
+
+
+
+ case READING_LUX:
+ {
+ /* Lux value */
+ if (data_length != TSTAMP_LEN+LUX_DATA_LEN) {
+ printf("\rLUX Warning: received data length is %d (while expecting %d) - status=%d\n\n", data_length, (TSTAMP_LEN+LUX_DATA_LEN), perDevs.status);
+ }
+ else {
+ tmp = (attr_value[3]<<8) | attr_value[2];
+ sprintf((char *)wifi_data, "\"LUX_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+
+ attribute_handle = slaveDev.star_data_char_handle;
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | LUX Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 2 bytes |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, LUX_TYPE_ID, attr_value+TSTAMP_LEN, LUX_DATA_LEN);
+
+ slaveDev.notification_data.data_length = ATTR_HEAD_LEN+LUX_DATA_LEN;
+ slaveDev.notification_data.attribute_value = star_attr_value;
+ memcpy(slaveDev.notification_data.devAddr, devAddr, 6);
+ slaveDev.notification_data.attribute_handle = attribute_handle;
+ }
+ new_buffer = 1;
+ }
+ }
+ break;
+
+
+
+ case READING_PRX:
+ {
+ /* the distance value in mm */
+ tmp = 0; /* fake value used to just notify the prx presence */
+ sprintf((char *)wifi_data, "\"PRX_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+
+// if (attr_value == NULL){
+// printf("\r\nattr_value == NULL\n");
+// }else{
+// printf("\r\nattr_value != NULL\n");
+// }
+
+ attribute_handle = slaveDev.star_data_char_handle;
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ uint8_t val[PRX_DATA_LEN];
+ memcpy((void *)val, (void *)tmp, sizeof(val));
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | Proximity Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 2 bytes |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, PRX_TYPE_ID, val, PRX_DATA_LEN);
+
+ slaveDev.notification_data.data_length = ATTR_HEAD_LEN+PRX_DATA_LEN;
+ slaveDev.notification_data.attribute_value = star_attr_value;
+ memcpy(slaveDev.notification_data.devAddr, devAddr, 6);
+ slaveDev.notification_data.attribute_handle = attribute_handle;
+ }
+ new_buffer = 1;
+ }
+ break;
+
+
+
+ case READING_DISCONNECTION:
+ {
+ /* Device disconnected */
+ attribute_handle = slaveDev.star_data_char_handle;
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | Status Type ID |
+ * 2 bytes | 2 bytes | 1 byte |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, STATUS_TYPE_ID, NULL, STATUS_DATA_LEN);
+
+ slaveDev.notification_data.data_length = ATTR_HEAD_LEN+STATUS_DATA_LEN;
+ slaveDev.notification_data.attribute_value = star_attr_value;
+ memcpy(slaveDev.notification_data.devAddr, devAddr, 6);
+ slaveDev.notification_data.attribute_handle = attribute_handle;
+
+ new_buffer = 1;
+ }
+ break;
+
+
+
+ case READING_AGM:
+ {
+ /* the acceleration value in mg */
+ tmp = 0; /* fake value used to just notify the agm presence */
+ sprintf((char *)wifi_data, "\"AGM_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+
+ attribute_handle = slaveDev.star_data_char_handle;
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ uint8_t val[MEMS_DATA_LEN];
+ memcpy((void *)val, (void *)tmp, sizeof(val));
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | ACC Type ID | value | GYR Type ID | value | MAG Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 6 bytes | 1 byte | 6 bytes | 1 byte | 6 bytes |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, ACC_TYPE_ID, val, MEMS_DATA_LEN);
+
+ slaveDev.notification_data.data_length = ATTR_HEAD_LEN+ACC_DATA_LEN+(2*TYPE_ID_LEN)+GYR_DATA_LEN+MAG_DATA_LEN;
+ slaveDev.notification_data.attribute_value = star_attr_value;
+ memcpy(slaveDev.notification_data.devAddr, devAddr, 6);
+ slaveDev.notification_data.attribute_handle = attribute_handle;
+ }
+ new_buffer = 1;
+ }
+ break;
+
+
+
+ case READING_SFUSION:
+ {
+ /* the mems sensor fusion value */
+ tmp = 0; /* fake value used to just notify the sensor fusion presence */
+ sprintf((char *)wifi_data, "\"SFUSION_0x%02x%02x\":%ld", devAddr[NODE_ID_B2], devAddr[NODE_ID_B1], tmp);
+
+ attribute_handle = slaveDev.star_data_char_handle;
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ uint8_t val[3];
+ memcpy((void *)val, (void *)tmp, sizeof(val));
+ /*
+ * Modify the attribute value according to the following format:
+ * Timestamp | Node ID | SFUSION Type ID | value |
+ * 2 bytes | 2 bytes | 1 byte | 6 bytes |
+ */
+ Create_New_Attr_Value(attr_value, devAddr, SFUSION_TYPE_ID, val, SFUSION_DATA_LEN);
+
+ slaveDev.notification_data.data_length = ATTR_HEAD_LEN+SFUSION_DATA_LEN;
+ slaveDev.notification_data.attribute_value = star_attr_value;
+ memcpy(slaveDev.notification_data.devAddr, devAddr, 6);
+ slaveDev.notification_data.attribute_handle = attribute_handle;
+ }
+ new_buffer = 1;
+ }
+ break;
+
+
+
+ default:
+ break;
+
+ }
+
+
+
+ if (new_buffer == 1){
+ strcat((char *)wifi_data, "\n");
+ printf("\r\n%s", (char *)wifi_data); /* print locally the buffer in JSON format */
+
+ data = wifi_data;
+ new_data = 1;
+ }
+
+
+ if (notificationPending == 1){
+ readCompleted = 1;
+ Change_Notification_Status(notifyP->att_data, notifyP->attr_value, notifyP->conn_handle, notifyP->i, notifyP->feature_mask, notifyP->frequency);
+ notificationPending = 0;
+ }
+
+ setNewStatus();
+ readCompleted = 1;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* This function retrieves the peripheral device index and address
+ * from the connection handle */
+void getDeviceFromConnHandle(uint16_t handle, uint8_t *index, tBDAddr devAddr){
+
+ //printf("\r\ngetDeviceFromConnHandle\n\n");//DEBUG
+
+ uint8_t i;
+
+ for (i=0; i<MAX_NUM_OF_NODES; i++){
+ if (perDevs.connection_handle[i] == handle){
+ memcpy(devAddr, perDevs.devInfo[i].bdaddr, 6);
+ *index = i;
+ break;
+ }
+ }
+}
+/*----------------------------------------------------------------------------*/
+
+
+/* Create a null gatt read pointer */
+GattReadCallbackParams * nullGattReadCallbackP(uint16_t connection_handle){
+ GattReadCallbackParams gattReadCallbackParamsStruct, *pointer;
+ pointer = &gattReadCallbackParamsStruct;
+
+ pointer->connHandle = connection_handle;
+ pointer->len = 0;
+ pointer->data = NULL;
+
+ return pointer;
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+void readingProcess(void) {
+ //printf("\r\nreadingProcess - status: %d - connDevices: %d\n", perDevs.status, perDevs.connDevices);//DEBUG
+
+
+ /* This index defines which peripheral is going to be read */
+ uint8_t i = perDevs.readDeviceIdx;
+ //fetch the current connection handle
+ uint16_t connHandle = perDevs.connection_handle[i];
+
+
+
+
+ if ((perDevs.status == NOTIFICATIONS_DATA_READ) && (perDevs.sfusion_event_enabled) && (!perDevs.sfusion_char_read[i])){
+ perDevs.status = ALL_DATA_READ;
+ }
+
+
+
+ //READ_INIT
+ 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:
+ case NODE_FL1:
+ /* start to read sensors data from environmental */
+
+ if ((perDevs.mic_event_enabled) || (perDevs.prx_event_enabled) || (perDevs.agm_event_enabled)
+ || (perDevs.sfusion_event_enabled)){
+ //printf("\r\nNotification event enabled, skip reading\n");//DEBUG
+ } else {
+ perDevs.status = READING_ENVIRONMENTAL;
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ } else {
+ //printf("\r\nconnDevices: %d - status = CONN_INIT - ELSE readingProces\n", perDevs.connDevices);//DEBUG
+ perDevs.status = CONN_INIT;
+ }
+ }//if-READ_INIT
+
+
+
+
+
+
+ //ENVIRONMENTAL
+ if ((perDevs.status == READING_ENVIRONMENTAL) && (readCompleted == 1)) {
+ //reading is starting
+ readCompleted = 0;
+ //fetch the characteristic declHandle
+ uint16_t declHandle = perDevs.environmental_char_handle[i];
+ //current data pointer
+ DiscoveredCharacteristic c = searchDCNode(headCharacteristic[i], declHandle)->data;
+ //pass characteristic and connection handle
+ readSensorData(c, connHandle, i);
+
+
+ }//if-ENVIRONMENTAL
+
+
+
+
+
+
+
+ //NOTIFY_ENV_TO_CLIENT
+ if ((perDevs.status == NOTIFY_ENV_TO_CLIENT) && (writeDescriptorCompleted ==1)) {
+ //printf("\r\nNOTIFY_ENV_TO_CLIENT\n");//DEBUG
+ notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
+ slaveDev.notification_data.attribute_handle);
+ eventQ.call(setNewStatus);
+ }//if-NOTIFY_ENV_TO_CLIENT
+
+
+
+
+
+
+
+ //READING_LED
+ if ((perDevs.status == READING_LED) && (readCompleted == 1)) {
+ //reading is starting
+ readCompleted = 0;
+ //fetch the characteristic declHandle
+ uint16_t declHandle = perDevs.led_char_handle[i];
+ //current data pointer
+ DiscoveredCharacteristic c = searchDCNode(headCharacteristic[i], declHandle)->data;
+ //pass characteristic and connection handle
+ readSensorData(c, connHandle, i);
+ }
+
+
+
+
+
+
+ //NOTIFY_LED_TO_CLIENT
+ if ((perDevs.status == NOTIFY_LED_TO_CLIENT) && (writeDescriptorCompleted ==1)) {
+ notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
+ slaveDev.notification_data.attribute_handle);
+ eventQ.call(setNewStatus);
+ }//if-NOTIFY_LED_TO_CLIENT
+
+
+
+
+
+
+ //READING_LUX
+ if ((perDevs.status == READING_LUX) && (readCompleted == 1)) {
+ //reading is starting
+ readCompleted = 0;
+ //fetch the characteristic declHandle
+ uint16_t declHandle = perDevs.lux_char_handle[i];
+ //current data pointer
+ DiscoveredCharacteristic c = searchDCNode(headCharacteristic[i], declHandle)->data;
+ //pass characteristic and connection handle
+ readSensorData(c, connHandle, i);
+ }//if-READING_LUX
+
+
+
+
+
+
+ //NOTIFY_LUX_TO_CLIENT
+ if ((perDevs.status == NOTIFY_LUX_TO_CLIENT) && (writeDescriptorCompleted ==1)) {
+ notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
+ slaveDev.notification_data.attribute_handle);
+ eventQ.call(setNewStatus);
+ }//if-NOTIFY_LUX_TO_CLIENT
+
+
+
+
+
+
+
+ //READING_MIC
+ if ((perDevs.status == READING_MIC) && (readCompleted == 1)){
+ //reading is starting
+ readCompleted = 0;
+ HAL_Delay(300);
+ /* Sending a 0 value to master just to notify the MIC sensor presence */
+ readCharacteristicCallback(nullGattReadCallbackP(connHandle));
+
+ perDevs.status = NOTIFY_MIC_TO_CLIENT;
+ notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
+ slaveDev.notification_data.attribute_handle);
+ eventQ.call(setNewStatus);
+ }//if-READING_MIC
+
+
+
+
+
+
+ //READING_PRX
+ if ((perDevs.status == READING_PRX) && (readCompleted == 1) && (writeDescriptorCompleted ==1)) {
+ //reading is starting
+ readCompleted = 0;
+ HAL_Delay(300);
+ /* Sending a 0 value to master just to notify the PRX sensor presence */
+ readCharacteristicCallback(nullGattReadCallbackP(connHandle));
+
+ perDevs.status = NOTIFY_PRX_TO_CLIENT;
+ notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
+ slaveDev.notification_data.attribute_handle);
+ eventQ.call(setNewStatus);
+ }//if-READING_PRX
+
+
+
+
+
+
+
+ //READING_AGM
+ if ((perDevs.status == READING_AGM) && (readCompleted == 1) && (writeDescriptorCompleted ==1)) {
+ //reading is starting
+ readCompleted = 0;
+ HAL_Delay(300);
+ /* Sending a 0 value to master just to notify the AGM sensors presence */
+ readCharacteristicCallback(nullGattReadCallbackP(connHandle));
+
+ perDevs.status = NOTIFY_AGM_TO_CLIENT;
+ notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
+ slaveDev.notification_data.attribute_handle);
+ eventQ.call(setNewStatus);
+ }//if-READING_AGM
+
+
+
+
+
+
+ //READING_SFUSION
+ if ((perDevs.status == READING_SFUSION) && (readCompleted == 1) && (writeDescriptorCompleted ==1)) {
+ //reading is starting
+ readCompleted = 0;
+ HAL_Delay(300);
+ /* Sending a 0 value to master just to notify the SFusion feature presence */
+ readCharacteristicCallback(nullGattReadCallbackP(connHandle));
+
+ perDevs.status = NOTIFY_SFUSION_TO_CLIENT;
+ notifyMaster(slaveDev.notification_data.data_length, slaveDev.notification_data.attribute_value,
+ slaveDev.notification_data.attribute_handle);
+ eventQ.call(setNewStatus);
+ }//if-READING_SFUSION
+
+
+
+
+
+
+
+
+
+ //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) {
+
+ if ((slaveDev.is_connected == false) && (slaveDev.is_discoverable == true)) {
+#if ENABLE_MEMS
+ disableAllNotifications(); /* Called here to disable the SFUSION notifications */
+#endif
+ setSlaveDiscoverable();
+
+ } else {
+ perDevs.status = CONN_INIT;
+ }//if-else-adv
+
+ }//if-i=0
+ }//if-ALL_DATA_READ
+
+
+
+
+
+
+
+ //DISABLE_NOTIFICATIONS
+ if ((perDevs.status == DISABLE_NOTIFICATIONS) && (readCompleted == 1) && (writeDescriptorCompleted ==1)) {
+ //printf("\r\nperDevs.status == DISABLE_NOTIFICATIONS\n");//DEBUG
+ disableAllNotifications();
+ perDevs.status = ALL_DATA_READ;
+ perDevs.readDeviceIdx = i;
+
+ }//if-DISABLE_NOTIFICATIONS
+
+
+
+
+
+ /* Start connection process */
+ eventQ.call(connectionProcess);
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* This method reads the characteristic in input */
+void readSensorData(const DiscoveredCharacteristic &characteristic, uint16_t connection_handle, uint8_t index){
+ //printf("\r\nreadSensorData\n");//DEBUG
+
+ ble_error_t error;
+
+
+
+
+ if (!perDevs.is_disconnected[index] && characteristic.getDeclHandle()){
+// printf("\r\n\nReading sensor data from periph %d (0x%04x - 0x%04x) - status=%d\n", index+1,
+// connection_handle, characteristic.getDeclHandle(), perDevs.status);//DEBUG
+
+ error = characteristic.read();
+
+ if (error != BLE_ERROR_NONE){
+ printf("\r\n\nUnable to read data from periph %d (err %d, cHndl 0x%04x - dHdl 0x%04x)\n", index+1,
+ error, connection_handle, characteristic.getDeclHandle());
+
+ eventQ.call(setNewStatus);
+ readCompleted = 1;
+ }//if-failed
+
+
+ } else {
+ eventQ.call(setNewStatus);
+
+ }//if-else-isConnected
+
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Set the new status */
+void setNewStatus(void) {
+ //printf("\r\nsetNewStatus\n");//DEBUG
+
+
+ uint8_t i = perDevs.readDeviceIdx;
+
+
+ //Switch status
+ switch (perDevs.status) {
+
+
+
+
+
+ //
+ case ENABLE_ME1_LED_NOTIFICATIONS:
+ if (perDevs.wup_event[perDevs.connDeviceIdx]) {
+ perDevs.status = ENABLE_ME1_WUP_NOTIFICATIONS;
+ }else {
+ perDevs.status = NOTIFICATIONS_ENABLED;
+ }
+ break;
+
+
+
+
+
+
+ //
+ case ENABLE_ME1_WUP_NOTIFICATIONS:
+ perDevs.status = NOTIFICATIONS_ENABLED;
+ break;
+
+
+
+
+
+
+
+ //
+ case READING_ENVIRONMENTAL:
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ perDevs.status = NOTIFY_ENV_TO_CLIENT;
+ }else {
+ 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
+ break;
+
+
+
+
+
+
+
+
+ //
+ case NOTIFY_ENV_TO_CLIENT:
+
+ //NODE_FL1
+ if (perDevs.devInfo[i].dev_v == NODE_FL1) {
+
+ if (perDevs.prx_on[i]==0) {
+ perDevs.status = READING_LUX;
+ }
+ 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;
+ }
+
+#if ENABLE_MEMS
+ else if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) &&
+ (perDevs.agm_event[i]) && (!perDevs.agm_char_read[i])) {
+ perDevs.status = READING_AGM;
+ }
+ else if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) &&
+ (perDevs.sfusion_event[i]) && (!perDevs.sfusion_char_read[i])) {
+ perDevs.status = READING_SFUSION;
+ }
+#endif
+ else {
+ perDevs.status = ALL_DATA_READ;
+ }
+ }//if-NODE_FL1
+
+
+ //NODE_ME1
+ else if (perDevs.devInfo[i].dev_v == NODE_ME1) {
+ if (!perDevs.led_char_read[i]) {
+ perDevs.status = READING_LED;
+ }
+#if ENABLE_MEMS
+ else if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) &&
+ (perDevs.agm_event[i]) && (!perDevs.agm_char_read[i])) {
+ perDevs.status = READING_AGM;
+ }
+ else if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) &&
+ (perDevs.sfusion_event[i]) && (!perDevs.sfusion_char_read[i])) {
+ perDevs.status = READING_SFUSION;
+ }
+#endif
+ else {
+ perDevs.status = ALL_DATA_READ;
+ }
+ }//if-NODE_ME1
+
+
+ //NODE_AM1
+ else if (perDevs.devInfo[i].dev_v == NODE_AM1) {
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) &&
+ (perDevs.mic_event[i]) && (!perDevs.mic_char_read[i])) {
+ perDevs.status = READING_MIC;
+ }
+#if ENABLE_MEMS
+ else if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) &&
+ (perDevs.agm_event[i]) && (!perDevs.agm_char_read[i])) {
+ perDevs.status = READING_AGM;
+ }
+ else if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) &&
+ (perDevs.sfusion_event[i]) && (!perDevs.sfusion_char_read[i])) {
+ perDevs.status = READING_SFUSION;
+ }
+#endif
+ else {
+ perDevs.status = ALL_DATA_READ;
+ }
+ }//if-NODE_AM1
+ break;
+
+
+
+
+
+
+
+ //
+ case READING_LED:
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ perDevs.status = NOTIFY_LED_TO_CLIENT;
+ }else {
+ perDevs.status = ALL_DATA_READ;
+ }
+ break;
+
+
+
+
+
+
+
+ //
+ case NOTIFY_LED_TO_CLIENT:
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify) &&
+ (!perDevs.led_char_read[i])) {
+ perDevs.led_char_read[i] = 1;
+ }
+#if ENABLE_MEMS
+ else if (perDevs.agm_event[i]) {
+ perDevs.status = READING_AGM;
+ }
+#endif
+ else {
+ perDevs.status = ALL_DATA_READ;
+ }
+ break;
+
+
+
+
+
+
+
+
+ //
+ case READING_LUX:
+ if ((slaveDev.is_connected) && (slaveDev.star_data_char_notify)) {
+ perDevs.status = NOTIFY_LUX_TO_CLIENT;
+ }else {
+ perDevs.status = ALL_DATA_READ;
+ }
+ break;
+
+
+
+
+
+
+
+
+ //
+ 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;
+ }
+ break;
+
+
+
+
+
+
+
+ //
+ case NOTIFY_MIC_TO_CLIENT:
+ perDevs.mic_char_read[i] = 1;
+
+#if ENABLE_MEMS
+ if (perDevs.agm_event[i]) {
+ perDevs.status = READING_AGM;
+ }
+ else if (perDevs.sfusion_event[i]) {
+ perDevs.status = READING_SFUSION;
+ }else {
+ perDevs.status = ALL_DATA_READ;
+ }
+#else
+ perDevs.status = ALL_DATA_READ;
+#endif
+ break;
+
+
+
+
+
+
+
+ //
+ case NOTIFY_PRX_TO_CLIENT:
+ perDevs.prx_char_read[i] = 1;
+
+#if ENABLE_MEMS
+ if (perDevs.agm_event[i]) {
+ perDevs.status = READING_AGM;
+ }
+ else if (perDevs.sfusion_event[i]) {
+ perDevs.status = READING_SFUSION;
+ }else {
+ perDevs.status = ALL_DATA_READ;
+ }
+#else
+ perDevs.status = ALL_DATA_READ;
+#endif
+ break;
+
+
+
+
+
+
+
+ //
+ case NOTIFY_AGM_TO_CLIENT:
+ perDevs.agm_char_read[i] = 1;
+ if (perDevs.sfusion_event[i]) {
+ perDevs.status = READING_SFUSION;
+ }else {
+ perDevs.status = ALL_DATA_READ;
+ }
+ break;
+
+
+
+
+
+ //
+ case NOTIFY_SFUSION_TO_CLIENT:
+ perDevs.sfusion_char_read[i] = 1;
+ perDevs.status = ALL_DATA_READ;
+ break;
+
+
+
+ //
+ default:
+ break;
+ }
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Creating a new DiscoveredCharacteristicNode */
+DiscoveredCharacteristicNode * createDCNode(const DiscoveredCharacteristic &ch, DiscoveredCharacteristicNode *next){
+
+ DiscoveredCharacteristicNode *newNode = (DiscoveredCharacteristicNode *)malloc(sizeof(DiscoveredCharacteristicNode));
+
+ if(newNode == NULL){
+ printf("\r\nError creating a new node\n\n");
+ }
+ newNode->data = ch;
+ newNode->next = next;
+
+ return newNode;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Prepend a DiscoveredCharacteristicNode */
+DiscoveredCharacteristicNode * prependDCNode(DiscoveredCharacteristicNode *head, const DiscoveredCharacteristic *characteristic){
+
+ DiscoveredCharacteristicNode * newNode = createDCNode(*characteristic, head);
+ head = newNode;
+
+ return head;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Append a DiscoveredCharacteristicNode */
+DiscoveredCharacteristicNode * appendDCNode(DiscoveredCharacteristicNode *head, const DiscoveredCharacteristic *characteristic){
+
+ DiscoveredCharacteristicNode * cursor = head;
+ // find the last node
+ while(cursor->next != NULL){
+ cursor = cursor->next;
+ }
+
+ DiscoveredCharacteristicNode * newNode = createDCNode(*characteristic, NULL);
+ cursor->next = newNode;
+
+ return head;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Number of DiscoveredCharacteristic nodes */
+int countElements(DiscoveredCharacteristicNode *head){
+
+ DiscoveredCharacteristicNode * cursor = head;
+
+ int c=0;
+ //cursor because starts from 0
+ while (cursor != NULL){
+ c++;
+ cursor = cursor->next;
+ }
+
+ return c;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Search for a DiscoveredCharacteristic node by declaration handle */
+DiscoveredCharacteristicNode * searchDCNode(DiscoveredCharacteristicNode *head, uint16_t declHandle) {
+
+ DiscoveredCharacteristicNode * cursor = head;
+
+ while(cursor != NULL){
+
+ // search for declaration handle
+ if(cursor->data.getDeclHandle() == declHandle){
+ return cursor;
+ }
+ cursor = cursor->next;
+ }
+ return NULL;
+}
+/*----------------------------------------------------------------------------*/
+
+
+
+/* Delete the DCN list */
+void deleteDCNList(DiscoveredCharacteristicNode *head){
+
+ DiscoveredCharacteristicNode * cursor, *tmp;
+
+ if (head != NULL){
+ cursor = head->next;
+ head->next = NULL;
+
+ while(cursor != NULL) {
+ tmp = cursor->next;
+ free(cursor);
+ cursor = tmp;
+ }
+ }
+}
+/*----------------------------------------------------------------------------*/