Revision 17:582eba752042, committed 2016-04-19

Comitter:

fabiombed

Date:

Tue Apr 19 08:53:07 2016 +0000

Parent:

16:d69c0d5d5ab2

Child:

18:319a1bb8f837

Commit message:

Updated to BlueST Protocol

Changed in this revision

--- a/CustomControlService.h	Tue Apr 12 09:20:52 2016 +0000
+++ b/CustomControlService.h	Tue Apr 19 08:53:07 2016 +0000
@@ -2,7 +2,7 @@
   * @file    CustomControlService.h
   * @author  Fabio Brembilla
   * @version V1.0.0
-  * @date    January, 2016
+  * @date    April, 2016
   * @brief   SunTracker Custom Service for BlueTooth (IDB0XA1 expansion board)
   *****************************************************************************
   * @attention
@@ -36,127 +36,124 @@
 
 #include "BLE.h"
 #include "UUID.h"
-                              
+
+#define STORE_BE_32(buf, val)    ( ((buf)[3] =  (uint8_t) (val)     ) , \
+                                   ((buf)[2] =  (uint8_t) (val>>8)  ) , \
+                                   ((buf)[1] =  (uint8_t) (val>>16) ) , \
+                                   ((buf)[0] =  (uint8_t) (val>>24) ) )     
+                
 /* BLE Services: Primary + 1 Secondary (Char Desk) */
-//const LongUUIDBytes_t CONTROL_SERVICE_UUID_128 = {0x00,0x00,0x00,0x00,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A}; //Primary
+//const LongUUIDBytes_t CONTROL_SERVICE_UUID = {0x00,0x00,0x00,0x00,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A}; //Primary
 //const LongUUIDBytes_t USER_BUTTON_CHAR_UUID    = {0x00,0x00,0x00,0x01,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A}; //User Button Control
-const ShortUUIDBytes_t CONTROL_SERVICE_UUID_128 = 0xA000;
-const ShortUUIDBytes_t USER_BUTTON_CHAR_UUID    = 0xA001;                      
-                            
-#define SIZEOF_CONTROL_DATA_LEN     2
-// 1 byte   0xFF     8 bit      int8
-// 2 byte   0xFFFF   16 bit     int16
+//const ShortUUIDBytes_t CONTROL_SERVICE_UUID  = 0xA000;
+//const ShortUUIDBytes_t CONTROL_COMMAND_CHAR_UUID = 0xA001;                      
+
+/* BLE Services: Primary + 1 Secondary (Char Desk) */
+const UUID::LongUUIDBytes_t CONTROL_SERVICE_UUID        = {0x00,0x00,0x00,0x00,0x00,0x0F,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
+//const LongUUIDBytes_t CONTROL_REGISTER_CHAR_UUID  = {0x00,0x00,0x00,0x01,0x00,0x0F,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b}; //Not Used
+const UUID::LongUUIDBytes_t CONTROL_COMMAND_CHAR_UUID   = {0x00,0x00,0x00,0x02,0x00,0x0F,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
+
+//#define SIZEOF_CONTROL_DATA_LEN     2
+// 1 byte   0xFF        8 bit      int8
+// 2 byte   0xFFFF      16 bit     int16
+// 4 byte   0xFFFFFFFF  32 bit     int32
+#define COMMAND_DATA_LEN         2+4+1+2 // TimeStamp (only for Response) + Feature + Type + Data
 
 /* Custom Control Service */
 class CustomControlService  {
 public:
     CustomControlService(BLE &_ble) :
         ble(_ble),
-                userbuttonCharacteristic(USER_BUTTON_CHAR_UUID, controlData, SIZEOF_CONTROL_DATA_LEN, SIZEOF_CONTROL_DATA_LEN,
-                                         GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)
+                commandCharacteristic(CONTROL_COMMAND_CHAR_UUID, commandData, COMMAND_DATA_LEN, COMMAND_DATA_LEN,                                        
+                                         GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)
         {  
         static bool serviceAdded = false; // To be sure to define just one time
         if (serviceAdded) {
             return;
         }
 
-        GattCharacteristic *charTable[] = {&userbuttonCharacteristic};
-        GattService   controlService(CONTROL_SERVICE_UUID_128, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));              
+        GattCharacteristic *charTable[] = {&commandCharacteristic};
+        GattService   controlService(CONTROL_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));              
         ble.gattServer().addService(controlService);                              
                 
-        isEnabledControlNotify          = false;
-        memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN);
+        isEnabledCommandNotify          = false;
+        memset (commandData, 0, COMMAND_DATA_LEN);
         isBTLEConnected                 = DISCONNECTED;
         serviceAdded                    = true;
     }
 
 // Tests Method
 
-    void sendControlState(uint8_t State) {
-            STORE_LE_16(controlData,State);
-            PRINTF("sendControlState handle: %d\n\r", userbuttonCharacteristic.getValueAttribute().getHandle());
-            memcpy (pastcontrolData, controlData, SIZEOF_CONTROL_DATA_LEN);
-            ble.gattServer().write(userbuttonCharacteristic.getValueAttribute().getHandle(), controlData, SIZEOF_CONTROL_DATA_LEN, 0);         
+/* BlueMicrosystem1 (only for Response)
+
+    uint32_t sendcontrolState(uint8_t val, uint8_t Command, uint32_t Feature, uint16_t TimeStamp) {      
+            STORE_LE_16(commandData ,TimeStamp); 
+            STORE_BE_32(commandData+2,Feature);          
+            commandData[6] = Command;
+            commandData[7] = (val==0x01) ? 100: val;                     
+            return ble.gattServer().write(commandCharacteristic.getValueAttribute().getHandle(), commandData, COMMAND_DATA_LEN, 0);
+    }
+    
+    uint32_t updatecontrolState(uint8_t val, uint8_t Command, uint32_t Feature, uint16_t TimeStamp) {
+            if (ble.getGapState().connected && isEnabledCommandNotify ) {  
+                return sendcontrolState(val, Command, Feature, TimeStamp);
+            }
+            return 0;
+    }
+*/
+
+/* Old SunTracker
+
+   void sendCommandState(uint8_t State) {
+            STORE_LE_16(commandData,State);
+            PRINTF("sendCommandState handle: %d\n\r", commandCharacteristic.getValueAttribute().getHandle());
+            memcpy (pastcommandData, commandData, COMMAND_DATA_LEN);
+            ble.gattServer().write(commandCharacteristic.getValueAttribute().getHandle(), commandData, COMMAND_DATA_LEN, 0);         
             printf(" (Update)"); 
     } 
 
-    void updateControlState(uint8_t Temp) {
-            if (ble.getGapState().connected && isEnabledControlNotify ) { 
-                if (memcmp (&pastcontrolData[0], &Temp, SIZEOF_CONTROL_DATA_LEN) != 0) {
-                    sendControlState(Temp);           
+    void updateCommandState(uint8_t State) {
+            if (ble.getGapState().connected && isEnabledCommandNotify ) { 
+                if (memcmp (&pastcommandData[0], &State, COMMAND_DATA_LEN) != 0) {
+                    sendCommandState(State);           
                 }
             }
     }
 
-/* with TimeStamp
-
-    void sendControlState(int16_t State, uint16_t TimeStamp) {
-            STORE_LE_16(controlData,TimeStamp);
-            STORE_LE_16(controlData+2,State);
-            PRINTF("sendControlState!! handle: %d\n\r", userbuttonCharacteristic.getValueAttribute().getHandle());
-            memcpy (pastcontrolData, controlData, SIZEOF_CONTROL_DATA_LEN);
-            ble.gattServer().write(userbuttonCharacteristic.getValueAttribute().getHandle(), controlData, SIZEOF_CONTROL_DATA_LEN, 0);         
-    } 
-
-    void updateControlState(int16_t Temp, uint16_t TimeStamp) {
-            if (memcmp (&pastcontrolData[2], &Temp, 2) != 0) {
-                sendControlState(Temp,  TimeStamp);              
-            }
-    }
-    
-*/
-
-/* 
- 
-    uint32_t sendcontrolState(uint32_t Feature, uint8_t Command, uint8_t val, uint16_t TimeStamp) {      
-            STORE_LE_16(controlData ,TimeStamp); 
-            STORE_BE_32(controlData+2,Feature);          
-            controlData[6] = Command;
-            controlData[7] = (val==0x01) ? 100: val;                     
-            return ble.gattServer().write(userbuttonCharacteristic.getValueAttribute().getHandle(), controlData, SIZEOF_CONTROL_DATA_LEN, 0);
-    }
-    
-    uint32_t updatecontrolState(uint32_t Feature, uint8_t Command, uint8_t val, uint16_t TimeStamp) {
-            if (ble.getGapState().connected && isEnabledControlNotify ) {  
-            return sendcontrolState(Feature, Command, val, TimeStamp);
-            }
-            return 0;
-    }
-
 */  
 
     void enNotify (Gap::Handle_t handle) {
             printf("\n\r\n\renNotify (handle %d)", handle);
-            if (isContHandle(handle)) { isEnabledControlNotify = true; return; }    
+            if (isCommandHandle(handle)) { isEnabledCommandNotify = true; return; }    
     }
     
     void disNotify (Gap::Handle_t handle) {
             printf("\n\r\n\rdisNotify (handle %d)", handle);
-            if (isContHandle(handle)) { isEnabledControlNotify = false; return; }    
+            if (isCommandHandle(handle)) { isEnabledCommandNotify = false; return; }    
     }   
 
-    bool isControlNotificationEn (void) {
-            return isEnabledControlNotify;
+    bool isCommandNotificationEn (void) {
+            return isEnabledCommandNotify;
     }
     
-    bool isContHandle (Gap::Handle_t handle) {
-            if (handle == userbuttonCharacteristic.getValueAttribute().getHandle()) return true;
+    bool isCommandHandle (Gap::Handle_t handle) {
+            if (handle == commandCharacteristic.getValueAttribute().getHandle()) return true;
             return false;
     }
     
     void updateConnectionStatus(ConnectionStatus_t status) {            
-            isEnabledControlNotify = false;
-            memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN);
+            isEnabledCommandNotify = false;
+            memset (commandData, 0, COMMAND_DATA_LEN);
             isBTLEConnected = status;             
     }      
 
 // Variables Initialization
 private:
         BLE                              &ble;
-        uint8_t                          controlData[SIZEOF_CONTROL_DATA_LEN];                
-        uint8_t                          pastcontrolData[SIZEOF_CONTROL_DATA_LEN]; 
-        GattCharacteristic               userbuttonCharacteristic;  
+        uint8_t                          commandData[COMMAND_DATA_LEN];                
+        uint8_t                          pastcommandData[COMMAND_DATA_LEN]; 
+        GattCharacteristic               commandCharacteristic;  
         ConnectionStatus_t               isBTLEConnected;
-        bool                             isEnabledControlNotify;
+        bool                             isEnabledCommandNotify;
 
 };

--- a/CustomSensorsService.h	Tue Apr 12 09:20:52 2016 +0000
+++ b/CustomSensorsService.h	Tue Apr 19 08:53:07 2016 +0000
@@ -2,7 +2,7 @@
   * @file    CustomSensorsService.h
   * @author  Fabio Brembilla
   * @version V1.0.0
-  * @date    January, 2016
+  * @date    April, 2016
   * @brief   SunTracker Custom Service for BlueTooth (IDB0XA1 expansion board)
   *****************************************************************************
   * @attention
@@ -38,31 +38,46 @@
 #include "UUID.h"
 
 /* BLE Services: Primary + 3 Secondary (Char Desk) */
-//const LongUUIDBytes_t SENS_SERVICE_UUID_128           = {0x00,0x00,0x00,0x00,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B}; //Primary 
-//const LongUUIDBytes_t SENS_DIFFERENCE_CHAR_UUID_128   = {0x00,0x00,0x00,0x01,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B}; //Difference Light/Mems
-//const LongUUIDBytes_t SENS_POSITION_CHAR_UUID_128     = {0x00,0x00,0x00,0x02,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B}; //Motor Position
-//const LongUUIDBytes_t SENS_SUNPANEL_CHAR_UUID_128     = {0x00,0x00,0x00,0x03,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B}; //SunPanel Voltage
-const ShortUUIDBytes_t SENS_SERVICE_UUID_128            = 0xB000; //Primary 
-const ShortUUIDBytes_t SENS_DIFFERENCE_CHAR_UUID_128    = 0xB001; //Difference Light/Mems Sensors
-const ShortUUIDBytes_t SENS_POSITION_CHAR_UUID_128      = 0xB002; //Motor Position
-const ShortUUIDBytes_t SENS_SUNPANEL_CHAR_UUID_128      = 0xB003; //SunPanel Voltage    
-                                   
-#define DIFFERENCE_DATA_LEN     2
-#define POSITION_DATA_LEN       2
-#define SUNPANEL_DATA_LEN       2
+//const LongUUIDBytes_t SENS_SERVICE_UUID           = {0x00,0x00,0x00,0x00,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B}; //Primary 
+//const LongUUIDBytes_t SENS_DIFFERENCE_CHAR_UUID   = {0x00,0x00,0x00,0x01,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B}; //Difference Light/Mems
+//const LongUUIDBytes_t SENS_POSITION_CHAR_UUID     = {0x00,0x00,0x00,0x02,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B}; //Motor Position
+//const LongUUIDBytes_t SENS_SUNPANEL_CHAR_UUID     = {0x00,0x00,0x00,0x03,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B}; //SunPanel Voltage
+//const ShortUUIDBytes_t SENS_SERVICE_UUID            = 0xB000; //Primary 
+//const ShortUUIDBytes_t SENS_STATUS_CHAR_UUID        = 0xB001; //Status
+//const ShortUUIDBytes_t SENS_DIFFERENCE_CHAR_UUID    = 0xB002; //Difference Light/Mems Sensors
+//const ShortUUIDBytes_t SENS_POSITION_CHAR_UUID      = 0xB003; //Motor Position
+//const ShortUUIDBytes_t SENS_SUNPANEL_CHAR_UUID      = 0xB004; //SunPanel Voltage    
+
+/* BLE Services: Primary + 4 Secondary (Char Desk) */
+const UUID::LongUUIDBytes_t SENS_SERVICE_UUID           = {0x00,0x00,0x00,0x00,0x00,0x01,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b}; //Primary 
+const UUID::LongUUIDBytes_t SENS_STATUS_CHAR_UUID       = {0x00,0x00,0x04,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b}; //Status                 0x00000400
+const UUID::LongUUIDBytes_t SENS_DIFFERENCE_CHAR_UUID   = {0x00,0x00,0x08,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b}; //Difference Light/Mems  0x00000800
+const UUID::LongUUIDBytes_t SENS_POSITION_CHAR_UUID     = {0x00,0x00,0x10,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b}; //Motor Position         0x00001000
+const UUID::LongUUIDBytes_t SENS_SUNPANEL_CHAR_UUID     = {0x00,0x00,0x20,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b}; //SunPanel Voltage       0x00002000
+                      
+//#define DIFFERENCE_DATA_LEN     2
+//#define POSITION_DATA_LEN       2
+//#define SUNPANEL_DATA_LEN       2
 // 1 byte   0xFF     8 bit      int8
 // 2 byte   0xFFFF   16 bit     int16
 
+#define STATUS_DATA_LEN         2+2 // TimeStamp + Status
+#define DIFFERENCE_DATA_LEN     2+2 // TimeStamp + Difference
+#define POSITION_DATA_LEN       2+2 // TimeStamp + Position
+#define SUNPANEL_DATA_LEN       2+2 // TimeStamp + SunPanel
+
 /* Custom Sensors Service */
 class CustomSensorService  {
 public:
     CustomSensorService(BLEDevice &_ble) :
         ble(_ble),
-                differenceCharacteristic(SENS_DIFFERENCE_CHAR_UUID_128, envDifference, DIFFERENCE_DATA_LEN, DIFFERENCE_DATA_LEN,
-                                       GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
-                positionCharacteristic(SENS_POSITION_CHAR_UUID_128, envPosition, POSITION_DATA_LEN, POSITION_DATA_LEN,
-                                       GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
-                sunpanelCharacteristic(SENS_SUNPANEL_CHAR_UUID_128, envSunpanel, SUNPANEL_DATA_LEN, SUNPANEL_DATA_LEN,
+                statusCharacteristic(SENS_STATUS_CHAR_UUID, envStatus, STATUS_DATA_LEN, STATUS_DATA_LEN,
+                                       GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
+                differenceCharacteristic(SENS_DIFFERENCE_CHAR_UUID, envDifference, DIFFERENCE_DATA_LEN, DIFFERENCE_DATA_LEN,
+                                       GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
+                positionCharacteristic(SENS_POSITION_CHAR_UUID, envPosition, POSITION_DATA_LEN, POSITION_DATA_LEN,
+                                       GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
+                sunpanelCharacteristic(SENS_SUNPANEL_CHAR_UUID, envSunpanel, SUNPANEL_DATA_LEN, SUNPANEL_DATA_LEN,
                                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)         
         {  
         static bool serviceAdded = false; // To be sure to define just one time
@@ -70,13 +85,15 @@
             return;
         }
 
-        GattCharacteristic *charTable[] = {&differenceCharacteristic, &positionCharacteristic, &sunpanelCharacteristic};                                                                                                                          
-        GattService   envService(SENS_SERVICE_UUID_128, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
+        GattCharacteristic *charTable[] = {&statusCharacteristic, &differenceCharacteristic, &positionCharacteristic, &sunpanelCharacteristic};                                                                                                                          
+        GattService   envService(SENS_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
         ble.gattServer().addService(envService);             
 
+        isEnabledStatusNotify           = false;
         isEnabledDifferenceNotify       = false;
         isEnabledPositionNotify         = false;
-        isEnabledSunpanelNotify         = false;            
+        isEnabledSunpanelNotify         = false;
+        memset (envStatus,     0, STATUS_DATA_LEN);          
         memset (envDifference, 0, DIFFERENCE_DATA_LEN);  
         memset (envPosition,   0, POSITION_DATA_LEN);  
         memset (envSunpanel,   0, SUNPANEL_DATA_LEN);                                                       
@@ -86,203 +103,85 @@
 
 // Tests Method
 
-    void sendEnvDifference (uint16_t Dif) {
-            STORE_LE_16(envDifference,Dif);
-            PRINTF("sendEnvDifference handle: %d\n\r", positionCharacteristic.getValueAttribute().getHandle());
+    void sendEnvStatus (uint16_t Sta, uint16_t TimeStamp) {
+            STORE_LE_16(envStatus,TimeStamp);
+            STORE_LE_16(envStatus+2,Sta);
+            PRINTF("sendEnvStatus handle: %d\n\r", statusCharacteristic.getValueAttribute().getHandle());
+            memcpy (pastenvStatus, envStatus, STATUS_DATA_LEN);
+            ble.gattServer().write(statusCharacteristic.getValueAttribute().getHandle(), envStatus, STATUS_DATA_LEN, 0);         
+            printf(" (Update)");
+    } 
+
+    void updateEnvStatus (uint16_t Sta, uint16_t TimeStamp) {
+            if (ble.getGapState().connected && isEnabledStatusNotify ) { 
+                if (memcmp (&pastenvStatus[2], &Sta, STATUS_DATA_LEN) != 0) {
+                    sendEnvStatus (Sta, TimeStamp);
+                }
+            }
+    }
+
+//--
+
+    void sendEnvDifference (uint16_t Dif, uint16_t TimeStamp) {
+            STORE_LE_16(envDifference,TimeStamp);
+            STORE_LE_16(envDifference+2,Dif);            
+            PRINTF("sendEnvDifference handle: %d\n\r", differenceCharacteristic.getValueAttribute().getHandle());
             memcpy (pastenvDifference, envDifference, DIFFERENCE_DATA_LEN);
             ble.gattServer().write(differenceCharacteristic.getValueAttribute().getHandle(), envDifference, DIFFERENCE_DATA_LEN, 0);         
             printf(" (Update)");
     } 
 
-    void updateEnvDifference (uint16_t Temp) {
+    void updateEnvDifference (uint16_t Dif, uint16_t TimeStamp) {
             if (ble.getGapState().connected && isEnabledDifferenceNotify ) { 
-                if (memcmp (&pastenvDifference[0], &Temp, DIFFERENCE_DATA_LEN) != 0) {
-                    sendEnvDifference (Temp);
+                if (memcmp (&pastenvDifference[2], &Dif, DIFFERENCE_DATA_LEN) != 0) {
+                    sendEnvDifference (Dif, TimeStamp);
                 }
             }
     }
         
-    void sendEnvPosition (uint16_t Pos) {
-            STORE_LE_16(envPosition,Pos);
+//--
+
+    void sendEnvPosition (uint16_t Pos, uint16_t TimeStamp) {
+            STORE_LE_16(envPosition,TimeStamp);
+            STORE_LE_16(envPosition+2,Pos);             
             PRINTF("sendEnvPosition handle: %d\n\r", positionCharacteristic.getValueAttribute().getHandle());
             memcpy (pastenvPosition, envPosition, POSITION_DATA_LEN);
             ble.gattServer().write(positionCharacteristic.getValueAttribute().getHandle(), envPosition, POSITION_DATA_LEN, 0);         
             printf(" (Update)");
     } 
 
-    void updateEnvPosition (uint16_t Temp) {
+    void updateEnvPosition (uint16_t Pos, uint16_t TimeStamp) {
             if (ble.getGapState().connected && isEnabledPositionNotify ) { 
-                if (memcmp (&pastenvPosition[0], &Temp, POSITION_DATA_LEN) != 0) {
-                    sendEnvPosition (Temp);
+                if (memcmp (&pastenvPosition[2], &Pos, POSITION_DATA_LEN) != 0) {
+                    sendEnvPosition (Pos, TimeStamp);
                 }
             }
     }
 
-    void sendEnvSunpanel (uint16_t Sun) {
-            STORE_LE_16(envSunpanel,Sun);
+//--
+
+    void sendEnvSunpanel (uint16_t Sun, uint16_t TimeStamp) {
+            STORE_LE_16(envSunpanel,TimeStamp);
+            STORE_LE_16(envSunpanel+2,Sun);              
             PRINTF("sendEnvSunpanel handle: %d\n\r", sunpanelCharacteristic.getValueAttribute().getHandle());
             memcpy (pastenvSunpanel, envSunpanel, SUNPANEL_DATA_LEN);
             ble.gattServer().write(sunpanelCharacteristic.getValueAttribute().getHandle(), envSunpanel, SUNPANEL_DATA_LEN, 0);         
             printf(" (Update)");
     } 
 
-    void updateEnvSunpanel (uint16_t Temp) {
+    void updateEnvSunpanel (uint16_t Sun, uint16_t TimeStamp) {
             if (ble.getGapState().connected && isEnabledSunpanelNotify ) { 
-                if (memcmp (&pastenvSunpanel[0], &Temp, SUNPANEL_DATA_LEN) != 0) {
-                    sendEnvSunpanel (Temp);            
+                if (memcmp (&pastenvSunpanel[2], &Sun, SUNPANEL_DATA_LEN) != 0) {
+                    sendEnvSunpanel (Sun, TimeStamp);            
                 }
             }
     }
 
-/* with TimeStamp
-
-    void sendEnvDifference (int16_t Dif, uint16_t TimeStamp) {
-            STORE_LE_16(envDifference,TimeStamp);
-            STORE_LE_16(envDifference+2,Dif);
-            PRINTF("sendEnvDifference!! handle: %d\n\r", positionCharacteristic.getValueAttribute().getHandle());
-            memcpy (pastenvDifference, envDifference, DIFFERENCE_DATA_LEN);
-            ble.gattServer().write(differenceCharacteristic.getValueAttribute().getHandle(), envDifference, DIFFERENCE_DATA_LEN, 0);         
-    } 
-
-    void updateEnvDifference (int16_t Temp, uint16_t TimeStamp) {
-            if (memcmp (&pastenvDifference[2], &Temp, 2) != 0) {
-                sendEnvDifference (Temp, TimeStamp);              
-            }
-    }
-        
-    void sendEnvPosition (int16_t Pos, uint16_t TimeStamp) {
-            STORE_LE_16(envPosition,TimeStamp);
-            STORE_LE_16(envPosition+2,Pos);
-            PRINTF("sendEnvPosition!! handle: %d\n\r", positionCharacteristic.getValueAttribute().getHandle());
-            memcpy (pastenvPosition, envPosition, POSITION_DATA_LEN);
-            ble.gattServer().write(positionCharacteristic.getValueAttribute().getHandle(), envPosition, POSITION_DATA_LEN, 0);         
-    } 
-
-    void updateEnvPosition (int16_t Temp, uint16_t TimeStamp) {
-            if (memcmp (&pastenvPosition[2], &Temp, 2) != 0) {
-                sendEnvPosition (Temp, TimeStamp);              
-            }
-    }
-
-    void sendEnvSunpanel (int16_t Sun, uint16_t TimeStamp) {
-            STORE_LE_16(envSunpanel,TimeStamp);
-            STORE_LE_16(envSunpanel+2,Sun);
-            PRINTF("sendEnvSunpanel!! handle: %d\n\r", sunpanelCharacteristic.getValueAttribute().getHandle());
-            memcpy (pastenvSunpanel, envSunpanel, SUNPANEL_DATA_LEN);
-            ble.gattServer().write(sunpanelCharacteristic.getValueAttribute().getHandle(), envSunpanel, SUNPANEL_DATA_LEN, 0);         
-    } 
-
-    void updateEnvSunpanel (int16_t Temp, uint16_t TimeStamp) {
-            if (memcmp (&pastenvSunpanel[2], &Temp, 2) != 0) {
-                sendEnvSunpanel (Temp, TimeStamp);              
-            }
-    }
-    
-*/
-    
-/*
-
-    void sendEnvTemperature (int16_t Temp, uint16_t TimeStamp) {
-            STORE_LE_16(envTemperature,TimeStamp);
-            STORE_LE_16(envTemperature+2,Temp);
-            PRINTF("sendEnvTemperature!!  handle: %d\n\r",   envTemperatureCharacteristic.getValueAttribute().getHandle());
-            memcpy (pastenvTemperature, envTemperature, TEMP_DATA_LEN);
-            ble.gattServer().write(envTemperatureCharacteristic.getValueAttribute().getHandle(), envTemperature, TEMP_DATA_LEN, 0);         
-    }               
-        
-    void updateEnvTemperature (int16_t Temp, uint16_t TimeStamp) {
-                if (memcmp (&pastenvTemperature[2], &Temp, 2) != 0) {
-                    sendEnvTemperature (Temp,  TimeStamp);              
-                }
-        }
-                
-    void sendEnvHumidity(uint16_t Hum, uint16_t TimeStamp) {        
-            STORE_LE_16(envHumidity,TimeStamp);
-            STORE_LE_16(envHumidity+2,Hum);
-            memcpy (pastenvHumidity, envHumidity, HUM_DATA_LEN);                
-            ble.gattServer().write(envHumidityCharacteristic.getValueAttribute().getHandle(), envHumidity, HUM_DATA_LEN, 0);            
-            
-        }
-        
-    void updateEnvHumidity(uint16_t Hum, uint16_t TimeStamp) {
-                if (memcmp (&pastenvHumidity[2], &Hum, 2) != 0) {               
-                    sendEnvHumidity(Hum, TimeStamp);
-                }
-    }       
-                
-    void sendEnvPressure(uint32_t Press, uint16_t TimeStamp) {
-            STORE_LE_16(envPressure,TimeStamp);         
-            STORE_LE_32(envPressure+2,Press);
-            memcpy (pastenvPressure, envPressure, PRES_DATA_LEN);                           
-            ble.gattServer().write(envPressureCharacteristic.getValueAttribute().getHandle(), envPressure, PRES_DATA_LEN, 0);           
-        }
-        
-    void updateEnvPressure(uint32_t Press, uint16_t TimeStamp) {
-                if (memcmp (&pastenvPressure[2], &Press, 2) != 0) {             
-                    sendEnvPressure(Press, TimeStamp);
-                }
-    }       
-    
-    void sendEnvMagnetometer(AxesRaw_TypeDef *Magn, uint16_t TimeStamp, osxMFX_calibFactor magOffset) {         
-            STORE_LE_16(envMagn,TimeStamp);
-            STORE_LE_16(envMagn+2,(Magn->AXIS_X - magOffset.magOffX));
-            STORE_LE_16(envMagn+4,(Magn->AXIS_Y - magOffset.magOffY));
-            STORE_LE_16(envMagn+6,(Magn->AXIS_Z - magOffset.magOffZ));          
-            ble.gattServer().write(envMagnetometerCharacteristic.getValueAttribute().getHandle(), envMagn, MAG_DATA_LEN, 0);                
-    }
-        
-    void updateEnvMagnetometer(AxesRaw_TypeDef *Magn, uint16_t TimeStamp, osxMFX_calibFactor magOffset) {   
-            if (isMagNotificationEn()) sendEnvMagnetometer(Magn, TimeStamp, magOffset);
-    }               
-        
-    void sendEnvAccelerometer (AxesRaw_TypeDef *Acc, uint16_t TimeStamp) {          
-            STORE_LE_16(envAcce,TimeStamp);
-            STORE_LE_16(envAcce+2,Acc->AXIS_X);
-            STORE_LE_16(envAcce+4,Acc->AXIS_Y);
-            STORE_LE_16(envAcce+6,Acc->AXIS_Z);
-            ble.gattServer().write(envAccelerometerCharacteristic.getValueAttribute().getHandle(), envAcce, ACC_DATA_LEN, 0);           
-    }
-        
-    void updateEnvAccelerometer (AxesRaw_TypeDef *Acc, uint16_t TimeStamp) {            
-            if (isAccNotificationEn()) sendEnvAccelerometer (Acc, TimeStamp);
-    }                       
-
-    void sendEnvGyroscope (AxesRaw_TypeDef *Gyro, uint16_t TimeStamp) {     
-            STORE_LE_16(envGyro,TimeStamp);
-            STORE_LE_16(envGyro+2,Gyro->AXIS_X);
-            STORE_LE_16(envGyro+4,Gyro->AXIS_Y);
-            STORE_LE_16(envGyro+6,Gyro->AXIS_Z);
-            ble.gattServer().write(envGyroCharacteristic.getValueAttribute().getHandle(), envGyro, GYRO_DATA_LEN, 0);           
-    }       
-        
-    void updateEnvGyroscope (AxesRaw_TypeDef *Gyro, uint16_t TimeStamp) {       
-            if (isGyroNotificationEn()) sendEnvGyroscope (Gyro, TimeStamp);             
-    }                       
-
-    void sendEnvAccGyroMag (AxesRaw_TypeDef *Acc, AxesRaw_TypeDef *Gyro, AxesRaw_TypeDef *Magn, uint16_t TimeStamp, osxMFX_calibFactor magOffset) {
-            STORE_LE_16(envAccGyroMag,TimeStamp);
-            STORE_LE_16(envAccGyroMag+2,Acc->AXIS_X);
-            STORE_LE_16(envAccGyroMag+4,Acc->AXIS_Y);
-            STORE_LE_16(envAccGyroMag+6,Acc->AXIS_Z);
-            
-            STORE_LE_16(envAccGyroMag+8,Gyro->AXIS_X);
-            STORE_LE_16(envAccGyroMag+10,Gyro->AXIS_Y);
-            STORE_LE_16(envAccGyroMag+12,Gyro->AXIS_Z);
-            
-            STORE_LE_16(envAccGyroMag+14,(Magn->AXIS_X  - magOffset.magOffX));
-            STORE_LE_16(envAccGyroMag+16,(Magn->AXIS_Y  - magOffset.magOffY));
-            STORE_LE_16(envAccGyroMag+18,(Magn->AXIS_Z  - magOffset.magOffZ));                      
-            ble.gattServer().write(envAccGyroMagCharacteristic.getValueAttribute().getHandle(), envAccGyroMag, ACCGYROMAG_DATA_LEN, 0);         
-    }
-        
-    void updateEnvAccGyroMag (AxesRaw_TypeDef *Acc, AxesRaw_TypeDef *Gyro, AxesRaw_TypeDef *Magn, uint16_t TimeStamp, osxMFX_calibFactor magOffset) {       
-            if (isAccGyroMagNotificationEn())sendEnvAccGyroMag (Acc, Gyro, Magn, TimeStamp, magOffset);              
-    } 
-    
-*/    
+//---- 
                                         
     void enNotify (Gap::Handle_t handle) {
             printf("\n\r\n\renNotify (handle %d)", handle);
+            if (isStatusHandle(handle)) { isEnabledStatusNotify = true; memset(envStatus,0,STATUS_DATA_LEN); return; }
             if (isDifferenceHandle(handle)) { isEnabledDifferenceNotify = true; memset(envDifference,0,DIFFERENCE_DATA_LEN); return; }
             if (isPositionHandle(handle)) { isEnabledPositionNotify = true; memset(envPosition,0,POSITION_DATA_LEN); return; }
             if (isSunpanelHandle(handle)) { isEnabledSunpanelNotify = true; memset(envSunpanel,0,SUNPANEL_DATA_LEN); return; }                     
@@ -290,10 +189,17 @@
 
     void disNotify (Gap::Handle_t handle) {
             printf("\n\r\n\rdisNotify (handle %d)", handle);
+            if (isStatusHandle(handle)) { isEnabledStatusNotify = false; memset(envStatus,0,STATUS_DATA_LEN); return; }
             if (isDifferenceHandle(handle)) { isEnabledDifferenceNotify = false; memset(envDifference,0,DIFFERENCE_DATA_LEN); return; }
             if (isPositionHandle(handle)) { isEnabledPositionNotify = false; memset(envPosition,0,POSITION_DATA_LEN); return; }        
             if (isSunpanelHandle(handle)) { isEnabledSunpanelNotify = false; memset(envSunpanel,0,SUNPANEL_DATA_LEN); return; }                   
     }
+    
+//----    
+      
+    bool isStatusNotificationEn (void) {
+            return isEnabledStatusNotify;
+    }  
         
     bool isDifferenceNotificationEn (void) {
             return isEnabledDifferenceNotify;
@@ -306,7 +212,14 @@
     bool isSunpanelNotificationEn (void) {
             return isEnabledSunpanelNotify;
     }
-                 
+
+//----
+           
+    bool isStatusHandle (Gap::Handle_t handle) {
+            if (handle == statusCharacteristic.getValueAttribute().getHandle()) return true;
+            return false;
+    }
+
     bool isDifferenceHandle (Gap::Handle_t handle) {
             if (handle == differenceCharacteristic.getValueAttribute().getHandle()) return true;
             return false;
@@ -322,10 +235,12 @@
             return false;
     }
         
-    void updateConnectionStatus(ConnectionStatus_t status) {   
+    void updateConnectionStatus(ConnectionStatus_t status) {
+            isEnabledStatusNotify = false; 
             isEnabledDifferenceNotify = false;
             isEnabledPositionNotify = false;
-            isEnabledSunpanelNotify = false;    
+            isEnabledSunpanelNotify = false;
+            memset (envStatus, 0, STATUS_DATA_LEN);  
             memset (envDifference, 0, DIFFERENCE_DATA_LEN);  
             memset (envPosition, 0, POSITION_DATA_LEN);  
             memset (envSunpanel, 0, SUNPANEL_DATA_LEN);                             
@@ -336,19 +251,23 @@
 private:
 
     BLEDevice            &ble;
+    uint8_t              envStatus [STATUS_DATA_LEN];
     uint8_t              envDifference [DIFFERENCE_DATA_LEN];
     uint8_t              envPosition [POSITION_DATA_LEN];
-    uint8_t              envSunpanel [SUNPANEL_DATA_LEN];        
+    uint8_t              envSunpanel [SUNPANEL_DATA_LEN];    
+    uint8_t              pastenvStatus [STATUS_DATA_LEN];   
     uint8_t              pastenvDifference [POSITION_DATA_LEN];
     uint8_t              pastenvPosition [POSITION_DATA_LEN];
     uint8_t              pastenvSunpanel [SUNPANEL_DATA_LEN];
     
+    GattCharacteristic   statusCharacteristic;
     GattCharacteristic   differenceCharacteristic;   
     GattCharacteristic   positionCharacteristic;     
     GattCharacteristic   sunpanelCharacteristic;   
     
     ConnectionStatus_t   isBTLEConnected;
 
+    bool                 isEnabledStatusNotify;
     bool                 isEnabledDifferenceNotify;
     bool                 isEnabledPositionNotify;
     bool                 isEnabledSunpanelNotify;

--- a/main.cpp	Tue Apr 12 09:20:52 2016 +0000
+++ b/main.cpp	Tue Apr 19 08:53:07 2016 +0000
@@ -3,7 +3,7 @@
  * @file    main.cpp
  * @author  Fabio Brembilla
  * @version V2.0.0
- * @date    March, 2016
+ * @date    April, 2016
  * @brief   SunTracker + BLE (Server) Vertical Application
  *          This application use IHM01A1, 6180XA1, IKS01A1, IDB0XA1 expansion boards
  ******************************************************************************
@@ -78,9 +78,10 @@
 #include "debug.h" // Need for PRINTF
 #include "Utils.h" // Need for STORE_LE_16 and STORE_LE_32
 
-const unsigned   LENGTH_OF_LONG_UUID = 16;
-typedef uint16_t ShortUUIDBytes_t;
-typedef uint8_t  LongUUIDBytes_t[LENGTH_OF_LONG_UUID];
+// Not need to re-define it, it is already defined into UUID.h, just use UUID:: when you call it
+//const unsigned   LENGTH_OF_LONG_UUID = 16;
+//typedef uint16_t ShortUUIDBytes_t;
+//typedef uint8_t  LongUUIDBytes_t[LENGTH_OF_LONG_UUID];
 
 typedef enum ConnectionStatus_t {
     DISCONNECTED    =0,
@@ -88,8 +89,8 @@
 } cns_t;
 
 #define BLE_DEV_NAME "SunTracker"
-#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF // SERVER address (must be set into CLIENT BLE_peer_address_BE)
-//#define BLE_DEV_MAC 0xFD,0x66,0x05,0x13,0xBE,0xBA // Default ST
+//#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF // SERVER address (must be set into CLIENT BLE_peer_address_BE)
+#define BLE_DEV_MAC 0xFD,0x66,0x05,0x13,0xBE,0xBA // Default ST address (must be set into CLIENT BLE_peer_address_BE)
 #define BLE_ADVERTISING_INTERVAL 1000
 
 #include "CustomControlService.h"
@@ -99,7 +100,10 @@
 static CustomControlService *p_customcontrolservice = NULL;
 static CustomSensorService  *p_customsensorservice = NULL;
 
-int16_t value_read = 0;
+#define FeatureStatus      0x00000400
+#define FeatureDifference  0x00000800
+#define FeaturePosition    0x00001000
+#define FeatureSunPanel    0x00002000
 
 #endif
 
@@ -125,12 +129,12 @@
 int16_t diff_abs=0;    // Abs of Babybear or Accelerometer difference
 int8_t left=0;         // Left Command for Rotate Direction
 int8_t right=0;        // Right Command for Rotate Direction
-int8_t start=0;        // Waiting User Button Push
 int16_t pos=0;         // Motor Position [--> Send BLE]
 int16_t measure=0;     // ADC Value from SunPanel [--> Send BLE]
-char DisplayStr[5];    // Status Display
-int8_t display=0;      // Shown on Display: 0 = Motor Speed, 1 = Solar Panel Value, 2 = Manual Control [--> Send BLE]
-int16_t status, status_t, status_b, status_l, status_r; // Babybear Status
+char DisplayStr[5];    // Display String
+int16_t Status=0;      // Status Shown on Display: 0 = Idle, 1 = Motor Speed, 2 = Solar Panel Value, 3 = Manual Control [--> Send BLE]
+int16_t status_bb, status_t, status_b, status_l, status_r; // Babybear Status
+uint16_t TimeStamp=0;  // TimeStamp for BLE
 
 /* Initializations ------------------------------------------------------------*/
 
@@ -161,24 +165,33 @@
 
 AnalogIn analog_read(PC_3); // A1 Conflict with BLE SPI_CS --> Changed in Morpho PC_3
 
+/* Update_Status ------------------------------------------------------------*/
+
+void Update_Status(int16_t Stat)
+{
+
+    char State[12]; // Characters into State must be <12 and never =>12 or it crash!!!
+    if (Stat==1) strcpy(State,"FlightSense");
+    if (Stat==2) strcpy(State,"ADCSunPanel");
+    if (Stat==3) strcpy(State,"Sensors");
+    printf("\r\n\r\nPUSH Status: %s", State);
+
+}
+
 /* User_Button_Pressed -------------------------------------------------------*/
 
 void User_Button_Pressed(void)
 {
 
-    if (start>0)    { display++; }
+    Status++;
+
 #ifdef Sensors
-    if (display>2)  { display=0; }
+    if (Status>3)  { Status=1; }
 #else
-    if (display>1)  { display=0; }
+    if (Status>2)  { Status=1; }
 #endif 
-    if (start==0)   { start=1; }
 
-    char State[12]; // Characters into State must be <12 and never =>12 or it crash!!!
-    if (display==0) strcpy(State,"FlightSense");
-    if (display==1) strcpy(State,"ADCSunPanel");
-    if (display==2) strcpy(State,"Sensors");
-    printf("\r\n\r\nPUSH Display: %s", State);
+    Update_Status(Status);
 
 }
 
@@ -188,7 +201,9 @@
 
 static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
 {
-    if (p_customcontrolservice->isContHandle(handle)) p_customcontrolservice->enNotify(handle);
+    if (p_customcontrolservice->isCommandHandle(handle)) p_customcontrolservice->enNotify(handle); 
+    
+    if (p_customsensorservice->isStatusHandle(handle)) p_customsensorservice->enNotify(handle); 
     if (p_customsensorservice->isDifferenceHandle(handle)) p_customsensorservice->enNotify(handle);  
     if (p_customsensorservice->isPositionHandle(handle)) p_customsensorservice->enNotify(handle);
     if (p_customsensorservice->isSunpanelHandle(handle)) p_customsensorservice->enNotify(handle);     
@@ -196,7 +211,9 @@
 
 static void onUpdatesDisabledCallback(Gap::Handle_t handle)
 {
-    if (p_customcontrolservice->isContHandle(handle)) p_customcontrolservice->disNotify(handle);
+    if (p_customcontrolservice->isCommandHandle(handle)) p_customcontrolservice->disNotify(handle); 
+    
+    if (p_customsensorservice->isStatusHandle(handle)) p_customsensorservice->disNotify(handle); 
     if (p_customsensorservice->isDifferenceHandle(handle)) p_customsensorservice->disNotify(handle);  
     if (p_customsensorservice->isPositionHandle(handle)) p_customsensorservice->disNotify(handle);
     if (p_customsensorservice->isSunpanelHandle(handle)) p_customsensorservice->disNotify(handle);    
@@ -204,33 +221,71 @@
 
 static void onDataReadCallback(const GattReadCallbackParams *eventDataP)
 {
-    
+/*
+    // if receive a manual Read request
+
+    if (p_customsensorservice->isStatusHandle(eventDataP->handle))
+    {   
+        printf("\n\r\n\rSend BLE Display Status %d", Status);
+        p_customsensorservice->sendEnvStatus(Status, TimeStamp);
+        
+    } else if (p_customsensorservice->isDifferenceHandle(eventDataP->handle)) {
+
+        printf("\n\rSend BLE Difference %d lux/mems", diff); // Send BLE diff, no diff_abs
+        p_customsensorservice->sendEnvDifference(diff, TimeStamp);
+
+    } else if (p_customsensorservice->isPositionHandle(eventDataP->handle)) {
+
+        printf("\n\rSend BLE Position %d", pos);
+        p_customsensorservice->sendEnvPosition(pos, TimeStamp);
+
+    } else if (p_customsensorservice->isSunpanelHandle(eventDataP->handle)) {
+
+        printf("\n\rSend BLE Sunpanel %d mV", measure);            
+        p_customsensorservice->sendEnvSunpanel(measure, TimeStamp);
+
+    }
+*/  
 }
 
 // This Callback happen when it RECEIVE a WRITE
 static void myonDataWriteCallback(const GattWriteCallbackParams *eventDataP)
 {   
-    //printf ("myonDataWriteCallback attr_handle: %x  att_data[3]: %x  data_length: %d\n\r", eventDataP->handle, eventDataP->data[3], eventDataP->len );
 
-    // Return original value after inverted with STORE_LE_16 to send by BLE
-    value_read = eventDataP->data[0] + (eventDataP->data[1]<<8);
-    
-    if (p_customcontrolservice->isContHandle(eventDataP->handle))
-    {
-        //printf("\r\n\r\nmyonDataWriteCallback (data[0] %x)", eventDataP->data[0]);
-        //printf("\r\nmyonDataWriteCallback (data[1] %x)", eventDataP->data[1]);
-        printf("\r\n\r\nonDataRead BUTTON (data %d)", value_read);
+    if (p_customcontrolservice->isCommandHandle(eventDataP->handle))
+    {       
+        // From CLIENT it receives a command in this format: Feature (4) + Type (1) + Data (2)        
+        uint32_t Feature = eventDataP->data[0]<<24 | eventDataP->data[1]<<16 | eventDataP->data[2]<<8 | eventDataP->data[3];
+        uint8_t Type = eventDataP->data[4];
+        int16_t Data = eventDataP->data[5]<<8 | eventDataP->data[6];
         
-        User_Button_Pressed(); // Change Display Status
-    }
-    
-    if (p_customsensorservice->isDifferenceHandle(eventDataP->handle))
-    {
-        //printf("\r\n\r\nmyonDataWriteCallback (data[0] %x)", eventDataP->data[0]);
-        //printf("\r\nmyonDataWriteCallback (data[1] %x)", eventDataP->data[1]);
-        //printf("\r\n\r\nonDataRead (data %d)", value_read);
-        
-        diff = value_read; // Change Diff
+        //printf("\r\n\r\nmyonDataWriteCallback (Feature %x)", Feature);
+        //printf("\r\nmyonDataWriteCallback (Type %x)", Type);        // Not Used
+        //printf("\r\nmyonDataWriteCallback (Data Hex %x)", Data);
+        //printf("\r\nmyonDataWriteCallback (Data Dec %d)", Data);
+               
+        switch(Feature)
+        {
+            case FeatureStatus:
+                printf("\r\n\r\nReceived STATUS %d", Data);
+                Status = Data;
+                Update_Status(Data);
+                break;
+     
+            case FeatureDifference:
+                printf("\r\n\r\nReceived DIFF %d", Data);
+                diff = Data;
+                break;
+ 
+            case FeaturePosition:
+                break;
+            
+            case FeatureSunPanel:
+                break;
+                
+            default:
+                break;
+        }
     }
     
 }
@@ -310,8 +365,8 @@
     // Initializing FlightSense Component
     board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC); // NC as Interrupt fixed from FlightSense library 42
 
-    status=board->InitBoard();
-    if(status) VL6180x_ErrLog("Failed to init the board!\n\r");
+    status_bb=board->InitBoard();
+    if(status_bb) VL6180x_ErrLog("Failed to init the board!\n\r");
 
     // Put GPIO not used as Interrupt in Hi-Z
     status_t=board->sensor_top->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
@@ -456,8 +511,8 @@
     // Stop Motor
     if (diff_abs<=TOLLERANCE) { 
         arrest=1; 
-        if (display==0) { strcpy(DisplayStr,"----"); }
-        if (display==2) { strcpy(DisplayStr,"E  3"); }
+        if (Status==1) { strcpy(DisplayStr,"----"); }
+        if (Status==3) { strcpy(DisplayStr,"E  3"); }
     }
         
     if (arrest==1 && dir!=0 ) { // Stop just one time
@@ -482,7 +537,7 @@
     //printf("Measure = %.0f mV\r\n", measure);
     //board->display->DisplayDigit("A", 0);
 
-    if (display==1) { sprintf(DisplayStr, "%d", measure); }
+    if (Status==2) { sprintf(DisplayStr, "%d", measure); }
      
 }
 
@@ -499,34 +554,49 @@
     
     mybutton.fall(&User_Button_Pressed);
 
+    #ifdef Ble
+    static int INTLOOP=0;
+    #endif
+
     // Loop until push User Button to Set 0 Point
     printf("\r\nWait PUSH Button");
     strcpy(DisplayStr,"pusH");
-    while(start<1) {
+    while(Status==0) {
         #ifdef FlightSense
         board->display->DisplayString(DisplayStr, 4);
-        p_BLEdev->waitForEvent();
         //printf("%s\n\r", DisplayStr);
+        #endif
+        
+        #ifdef Ble
+        /*
+        INTLOOP++;
+        if (INTLOOP==100)
+        {
+            p_customsensorservice->sendEnvStatus(Status, TimeStamp);
+            p_customsensorservice->sendEnvDifference(diff, TimeStamp);
+            p_customsensorservice->sendEnvPosition(pos, TimeStamp);
+            p_customsensorservice->sendEnvSunpanel(measure, TimeStamp);
+            INTLOOP=0;
+        }
+        */    
+        //p_BLEdev->waitForEvent(); // Without it the CLIENT can't start to communicate
         #endif 
     }
 
     printf("\r\n\r\nStart Main Loop");
-
+    INTLOOP=0;
+    
     #ifdef EasySpin
     motor->Enable(); // To put the motor on hold by execute CmdEnable
     #endif
 
-    #ifdef Ble
-    static int INTLOOP=0;
-    #endif
-
     // Main Loop
     while(true)
     {
-        if (display==0 | display==1)    { Measure_Babybear(); }
+        if (Status==1 | Status==2)    { Measure_Babybear(); }
         
         #ifndef Ble // without BLE it uses Accelerometer from SERVER, otherwise receive the value from CLIENT
-        if (display==2)                 { Measure_Accelerometer(); }
+        if (Status==3)                { Measure_Accelerometer(); }
         #endif
         
         Control_Motor();
@@ -538,34 +608,37 @@
         #endif   
         
         #ifdef Ble
+        
         INTLOOP++;
         if (INTLOOP==100)
         {    
-            //if (p_customcontrolservice->isControlNotificationEn()) {
-                printf("\n\r\n\rSend BLE Display %d", display);
-                p_customcontrolservice->sendControlState(display);
-                //p_customcontrolservice->updateControlState(display);            
+            //if (p_customsensorservice->isStatusNotificationEn()) {
+                printf("\n\r\n\rSend BLE Display Status %d", Status);
+                p_customsensorservice->sendEnvStatus(Status, TimeStamp);
+                //p_customsensorservice->updateEnvStatus(Status, TimeStamp));            
             //}
             //if (p_customsensorservice->isDifferenceNotificationEn()) {
                 printf("\n\rSend BLE Difference %d lux/mems", diff); // Send BLE diff, no diff_abs
-                p_customsensorservice->sendEnvDifference(diff);
-                //p_customsensorservice->updateEnvDifference(diff);
+                p_customsensorservice->sendEnvDifference(diff, TimeStamp);
+                //p_customsensorservice->updateEnvDifference(diff, TimeStamp));
             //}
             //if (p_customsensorservice->isPositionNotificationEn()) {            
                 printf("\n\rSend BLE Position %d", pos);
-                p_customsensorservice->sendEnvPosition(pos);
-                //p_customsensorservice->updateEnvPosition(pos);
+                p_customsensorservice->sendEnvPosition(pos, TimeStamp);
+                //p_customsensorservice->updateEnvPosition(pos, TimeStamp));
             //}
             //if (p_customsensorservice->isSunpanelNotificationEn()) {            
                 printf("\n\rSend BLE Sunpanel %d mV", measure);            
-                p_customsensorservice->sendEnvSunpanel(measure);
-                //p_customsensorservice->updateEnvSunpanel(measure);
+                p_customsensorservice->sendEnvSunpanel(measure, TimeStamp);
+                //p_customsensorservice->updateEnvSunpanel(measure, TimeStamp));
             //}
             INTLOOP=0;
+            //diff=0; // To reset it in case from Mems don't arrive a future value
         }
-
+        
         p_BLEdev->waitForEvent();
         #endif
+        
     }
 
     //status_l=board->sensor_left->StopMeasurement(als_continuous_polling);