Revision 14:644f9e7278e9, committed 2016-03-16

Comitter:

fabiombed

Date:

Wed Mar 16 19:17:08 2016 +0000

Parent:

13:7880b45ceeac

Child:

15:019b8d60c89d

Commit message:

Update

Changed in this revision

--- a/CustomControlService.h	Fri Mar 11 09:53:49 2016 +0000
+++ b/CustomControlService.h	Wed Mar 16 19:17:08 2016 +0000
@@ -34,21 +34,16 @@
   ******************************************************************************
 */
 
-#ifndef __CUSTOM_BLE_CONTROL_SERVICE_H__
-#define __CUSTOM_BLE_CONTROL_SERVICE_H__
 #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 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+4+1+1
-
-/* 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 USER_BUTTON_CHAR_UUID    = {0x00,0x00,0x00,0x01,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A}; //User Button Control
+#define SIZEOF_CONTROL_DATA_LEN  1
 
 /* Custom Control Service */
 class CustomControlService  {
@@ -64,18 +59,68 @@
         }
 
         GattCharacteristic *charTable[] = {&userbuttonCharacteristic};
-
         GattService   controlService(CONTROL_SERVICE_UUID_128, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));              
-                
         ble.gattServer().addService(controlService);                              
                 
-        isEnabledControlNotify = false;
-        memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN); 
-        serviceAdded = true;
+        isEnabledControlNotify          = false;
+        memset (controlData, 0, SIZEOF_CONTROL_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);         
+            printf(" (Update)"); 
+    } 
+
+    void updateControlState(uint8_t Temp) {
+            if (memcmp (&pastcontrolData[0], &Temp, 1) != 0) {
+                sendControlState(Temp);           
+            }
+    }
+
+/* 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) {
             if (isContHandle(handle)) { 
                 PRINTF("enNotify! %d\n\r", handle); isEnabledControlNotify = true; return; }    
@@ -93,37 +138,17 @@
     
     void updateConnectionStatus(ConnectionStatus_t status) {            
             isEnabledControlNotify = false;
-            memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN);             
+            memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN);
+            isBTLEConnected = status;             
     }      
 
-/*
-  
-    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;
-    }
-
-*/    
-
 // Variables Initialization
 private:
         BLE                              &ble;
         uint8_t                          controlData[SIZEOF_CONTROL_DATA_LEN];                
-        uint8_t                          controlState;
+        uint8_t                          pastcontrolData[SIZEOF_CONTROL_DATA_LEN]; 
         GattCharacteristic               userbuttonCharacteristic;  
+        ConnectionStatus_t               isBTLEConnected;
         bool                             isEnabledControlNotify;
 
 };
-
-#endif /* #ifndef __CUSTOM_BLE_CONTROL_SERVICE_H__*/
-

--- a/CustomControlService_copy.h	Fri Mar 11 09:53:49 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,129 +0,0 @@
-/******************************************************************************
-  * @file    CustomControlService.h
-  * @author  Fabio Brembilla
-  * @version V1.0.0
-  * @date    January, 2016
-  * @brief   SunTracker Custom Service for BlueTooth (IDB0XA1 expansion board)
-  *****************************************************************************
-  * @attention
-  *
-  * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
-  *
-  * Redistribution and use in source and binary forms, with or without modification,
-  * are permitted provided that the following conditions are met:
-  *   1. Redistributions of source code must retain the above copyright notice,
-  *      this list of conditions and the following disclaimer.
-  *   2. Redistributions in binary form must reproduce the above copyright notice,
-  *      this list of conditions and the following disclaimer in the documentation
-  *      and/or other materials provided with the distribution.
-  *   3. Neither the name of STMicroelectronics nor the names of its contributors
-  *      may be used to endorse or promote products derived from this software
-  *      without specific prior written permission.
-  *
-  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-  *
-  ******************************************************************************
-*/
-
-#ifndef __CUSTOM_BLE_CONTROL_SERVICE_H__
-#define __CUSTOM_BLE_CONTROL_SERVICE_H__
-#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) ) )                         
-                            
-#define SIZEOF_CONTROL_DATA_LEN  2+4+1+1
-
-/* 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 USER_BUTTON_CHAR_UUID    = {0x00,0x00,0x00,0x01,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A}; //User Button Control
-
-/* 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)
-        {  
-        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 *));              
-                
-        ble.gattServer().addService(controlService);                              
-                
-        isEnabledControlNotify = false;
-        memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN); 
-        serviceAdded = true;
-    }
-
-// Tests Method
-
-    void enNotify (Gap::Handle_t handle) {
-            if (isContHandle(handle)) { 
-                PRINTF("enNotify! %d\n\r", handle); isEnabledControlNotify = true; return; }    
-    }
-    
-    void disNotify (Gap::Handle_t handle) {
-            if (isContHandle(handle)) { 
-                            isEnabledControlNotify = false; return; }    
-    }   
-    
-    bool isContHandle (Gap::Handle_t handle) {
-            if (handle == userbuttonCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    }
-    
-    void updateConnectionStatus(ConnectionStatus_t status) {            
-            isEnabledControlNotify = false;
-            memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN);             
-    }      
-
-/*
-  
-    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;
-    }
-
-*/    
-
-// Variables Initialization
-private:
-        BLE                              &ble;
-        uint8_t                          controlData[SIZEOF_CONTROL_DATA_LEN];                
-        uint8_t                          controlState;
-        GattCharacteristic               userbuttonCharacteristic;  
-        bool                             isEnabledControlNotify;
-
-};
-
-#endif /* #ifndef __CUSTOM_BLE_CONTROL_SERVICE_H__*/
-

--- a/CustomSensorsService.h	Fri Mar 11 09:53:49 2016 +0000
+++ b/CustomSensorsService.h	Wed Mar 16 19:17:08 2016 +0000
@@ -34,24 +34,30 @@
   ******************************************************************************
 */
 
-#ifndef __CUSTOM_BLE_SENSORS_SERVICE_H__
-#define __CUSTOM_BLE_SENSORS_SERVICE_H__
 #include "BLE.h"
 #include "UUID.h"
 
-/* BLE Services: Primary + 2 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_POSITION_CHAR_UUID_128 = {0x00,0x00,0x00,0x01,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,0x02,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B,0x0B}; //SunPanel Voltage
-
-#define POSITION_DATA_LEN   2+2
-#define SUNPANEL_DATA_LEN   2+2
+/* 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     4
+#define POSITION_DATA_LEN       4
+#define SUNPANEL_DATA_LEN       4
 
 /* 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,
@@ -62,32 +68,110 @@
             return;
         }
 
-        GattCharacteristic *charTable[] = {&positionCharacteristic, &sunpanelCharacteristic};                 
-                                                                                                                                                         
-        GattService   envService(SENS_SERVICE_UUID_128, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));               
-                
+        GattCharacteristic *charTable[] = {&differenceCharacteristic, &positionCharacteristic, &sunpanelCharacteristic};                                                                                                                          
+        GattService   envService(SENS_SERVICE_UUID_128, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
         ble.gattServer().addService(envService);             
 
+        isEnabledDifferenceNotify       = false;
         isEnabledPositionNotify         = false;
         isEnabledSunpanelNotify         = false;            
-
-        memset (envPosition, 0, POSITION_DATA_LEN);  
-        memset (envSunpanel, 0, SUNPANEL_DATA_LEN);                                
-                                                                         
+        memset (envDifference, 0, DIFFERENCE_DATA_LEN);  
+        memset (envPosition,   0, POSITION_DATA_LEN);  
+        memset (envSunpanel,   0, SUNPANEL_DATA_LEN);                                                       
         isBTLEConnected                 = DISCONNECTED;                                                                  
         serviceAdded                    = true;                                                                                                                                  
     }
 
 // Tests Method
 
+    void sendEnvDifference (uint32_t Dif) {
+            STORE_LE_32(envDifference,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);         
+            printf(" (Update)");
+    } 
+
+    void updateEnvDifference (uint32_t Temp) {
+            if (memcmp (&pastenvDifference[0], &Temp, 4) != 0) {
+                sendEnvDifference (Temp);
+            }
+    }
+        
+    void sendEnvPosition (uint32_t Pos) {
+            STORE_LE_32(envPosition,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 (uint32_t Temp) {
+            if (memcmp (&pastenvPosition[0], &Temp, 4) != 0) {
+                sendEnvPosition (Temp);
+            }
+    }
+
+    void sendEnvSunpanel (uint32_t Sun) {
+            STORE_LE_32(envSunpanel,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 (uint32_t Temp) {
+            if (memcmp (&pastenvSunpanel[0], &Temp, 4) != 0) {
+                sendEnvSunpanel (Temp);            
+            }
+    }
+
+/* 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());
+            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) {
@@ -190,15 +274,21 @@
 */    
                                         
     void enNotify (Gap::Handle_t handle) {
+            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; }                     
     }
 
     void disNotify (Gap::Handle_t handle) {
+            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 isDifferenceNotificationEn (void) {
+            return isEnabledDifferenceNotify;
+    }
+
     bool isPositionNotificationEn (void) {
             return isEnabledPositionNotify;
     }
@@ -207,6 +297,11 @@
             return isEnabledSunpanelNotify;
     }
                  
+    bool isDifferenceHandle (Gap::Handle_t handle) {
+            if (handle == differenceCharacteristic.getValueAttribute().getHandle()) return true;
+            return false;
+    }
+
     bool isPositionHandle (Gap::Handle_t handle) {
             if (handle == positionCharacteristic.getValueAttribute().getHandle()) return true;
             return false;
@@ -218,9 +313,10 @@
     }
         
     void updateConnectionStatus(ConnectionStatus_t status) {   
+            isEnabledDifferenceNotify = false;
             isEnabledPositionNotify = false;
             isEnabledSunpanelNotify = false;    
-            
+            memset (envDifference, 0, DIFFERENCE_DATA_LEN);  
             memset (envPosition, 0, POSITION_DATA_LEN);  
             memset (envSunpanel, 0, SUNPANEL_DATA_LEN);                             
             isBTLEConnected = status; 
@@ -230,19 +326,21 @@
 private:
 
     BLEDevice            &ble;
+    uint8_t              envDifference [DIFFERENCE_DATA_LEN];
     uint8_t              envPosition [POSITION_DATA_LEN];
     uint8_t              envSunpanel [SUNPANEL_DATA_LEN];        
+    uint8_t              pastenvDifference [POSITION_DATA_LEN];
     uint8_t              pastenvPosition [POSITION_DATA_LEN];
     uint8_t              pastenvSunpanel [SUNPANEL_DATA_LEN];
     
+    GattCharacteristic   differenceCharacteristic;   
     GattCharacteristic   positionCharacteristic;     
     GattCharacteristic   sunpanelCharacteristic;   
- 
+    
     ConnectionStatus_t   isBTLEConnected;
 
+    bool                 isEnabledDifferenceNotify;
     bool                 isEnabledPositionNotify;
     bool                 isEnabledSunpanelNotify;
                
 };
-
-#endif /* #ifndef __CUSTOM_BLE_SENSORS_SERVICE_H__*/

--- a/CustomSensorsService_copy.h	Fri Mar 11 09:53:49 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,129 +0,0 @@
-/******************************************************************************
-  * @file    CustomControlService.h
-  * @author  Fabio Brembilla
-  * @version V1.0.0
-  * @date    January, 2016
-  * @brief   SunTracker Custom Service for BlueTooth (IDB0XA1 expansion board)
-  *****************************************************************************
-  * @attention
-  *
-  * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
-  *
-  * Redistribution and use in source and binary forms, with or without modification,
-  * are permitted provided that the following conditions are met:
-  *   1. Redistributions of source code must retain the above copyright notice,
-  *      this list of conditions and the following disclaimer.
-  *   2. Redistributions in binary form must reproduce the above copyright notice,
-  *      this list of conditions and the following disclaimer in the documentation
-  *      and/or other materials provided with the distribution.
-  *   3. Neither the name of STMicroelectronics nor the names of its contributors
-  *      may be used to endorse or promote products derived from this software
-  *      without specific prior written permission.
-  *
-  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-  *
-  ******************************************************************************
-*/
-
-#ifndef __CUSTOM_BLE_CONTROL_SERVICE_H__
-#define __CUSTOM_BLE_CONTROL_SERVICE_H__
-#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) ) )                         
-                            
-#define SIZEOF_CONTROL_DATA_LEN  2+4+1+1
-
-/* 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 USER_BUTTON_CHAR_UUID    = {0x00,0x00,0x00,0x01,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A}; //User Button Control
-
-/* 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)
-        {  
-        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 *));              
-                
-        ble.gattServer().addService(controlService);                              
-                
-        isEnabledControlNotify = false;
-        memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN); 
-        serviceAdded = true;
-    }
-
-// Tests Method
-
-    void enNotify (Gap::Handle_t handle) {
-            if (isContHandle(handle)) { 
-                PRINTF("enNotify! %d\n\r", handle); isEnabledControlNotify = true; return; }    
-    }
-    
-    void disNotify (Gap::Handle_t handle) {
-            if (isContHandle(handle)) { 
-                            isEnabledControlNotify = false; return; }    
-    }   
-    
-    bool isContHandle (Gap::Handle_t handle) {
-            if (handle == userbuttonCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    }
-    
-    void updateConnectionStatus(ConnectionStatus_t status) {            
-            isEnabledControlNotify = false;
-            memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN);             
-    }      
-
-/*
-  
-    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;
-    }
-
-*/    
-
-// Variables Initialization
-private:
-        BLE                              &ble;
-        uint8_t                          controlData[SIZEOF_CONTROL_DATA_LEN];                
-        uint8_t                          controlState;
-        GattCharacteristic               userbuttonCharacteristic;  
-        bool                             isEnabledControlNotify;
-
-};
-
-#endif /* #ifndef __CUSTOM_BLE_CONTROL_SERVICE_H__*/
-

--- a/X_NUCLEO_6180XA1.lib	Fri Mar 11 09:53:49 2016 +0000
+++ b/X_NUCLEO_6180XA1.lib	Wed Mar 16 19:17:08 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/fabiombed/code/X_NUCLEO_6180XA1/#692c6223dc24
+http://developer.mbed.org/teams/ST/code/X_NUCLEO_6180XA1/#70e4abd970a9

--- a/X_NUCLEO_IHM01A1.lib	Fri Mar 11 09:53:49 2016 +0000
+++ b/X_NUCLEO_IHM01A1.lib	Wed Mar 16 19:17:08 2016 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/ST/code/X_NUCLEO_IHM01A1/#58264db10a17
+http://developer.mbed.org/teams/ST/code/X_NUCLEO_IHM01A1/#258f0f60d30c

--- a/main.cpp	Fri Mar 11 09:53:49 2016 +0000
+++ b/main.cpp	Wed Mar 16 19:17:08 2016 +0000
@@ -36,6 +36,13 @@
  ******************************************************************************
  */
 
+/* Define --------------------------------------------------------------------*/
+
+#define FlightSense //6180XA1 Main (remove only for debug)
+#define EasySpin    //IHM01A1 Main (remove only for debug)
+#define Sensors     //IKS01A1 Option
+#define Ble         //IDB0XA1 Option
+
 /* Includes ------------------------------------------------------------------*/
 
 // Mbed specific header files
@@ -46,9 +53,17 @@
 #include "DevI2C.h"
 
 // Component specific header files
-#include "l6474_class.h"
-#include "x_nucleo_6180xa1.h"
-#include "x_nucleo_iks01a1.h"
+#ifdef FlightSense
+    #include "x_nucleo_6180xa1.h"
+#endif
+
+#ifdef EasySpin
+    #include "l6474_class.h"
+#endif
+
+#ifdef Sensors
+    #include "x_nucleo_iks01a1.h"
+#endif
 
 // C header files
 #include <string.h>
@@ -58,8 +73,10 @@
 
 /* BlueTooth -----------------------------------------------------------------*/
 
+#ifdef Ble
+
 #include "debug.h" // Need for PRINTF
-#include "Utils.h" // Need for STORE_LE_16 and _32
+#include "Utils.h" // Need for STORE_LE_16 and STORE_LE_32
 
 const unsigned   LENGTH_OF_LONG_UUID = 16;
 typedef uint16_t ShortUUIDBytes_t;
@@ -77,7 +94,8 @@
 } cns_t;
 
 #define BLE_DEV_NAME "SunTracker"
-#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF
+#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF // must be the same of BLE_peer_address_BE into CLIENT
+//#define BLE_DEV_MAC 0xFD,0x66,0x05,0x13,0xBE,0xBA // Default ST
 #define BLE_ADVERTISING_INTERVAL 1000
 
 #include "CustomControlService.h"
@@ -87,6 +105,8 @@
 static CustomControlService *p_customcontrolservice = NULL;
 static CustomSensorService  *p_customsensorservice = NULL;
 
+#endif
+
 /* Definitions ---------------------------------------------------------------*/
 
 #define SET_ACC 400     // Set Motor Acceleration
@@ -100,18 +120,20 @@
 
 /* Variables -----------------------------------------------------------------*/
 
-int16_t dir=0;          // Motor Rotation Direction: 0 = Stop, 1 = Anticlockwise, 2 = Clockwise
-int16_t changedir=0;    // Change Direction: 0 = No, 1 = Yes
-int16_t arrest=0;       // Arrest: 0 = No, 1 = Yes
-int16_t babybear=0;     // Difference (in Lux) between Left and Right
-int acc_data[3];        // Difference of Accelerometer
-int16_t diff=0;         // Abs of Babybear or Accelerometer difference
-int16_t left=0;         // Left Command for Rotate Direction
-int16_t right=0;        // Right Command for Rotate Direction
-int16_t start=0;        // Waiting User Button Push
-int32_t pos=0;          // Motor Position
-char DisplayStr[5];     // Status Display
-int16_t Display=0;      // Shown on Display: 0 = Motor Speed, 1 = Solar Panel Value, 2 = Manual Control
+int8_t dir=0;          // Motor Rotation Direction: 0 = Stop, 1 = Anticlockwise, 2 = Clockwise
+int8_t changedir=0;    // Change Direction: 0 = No, 1 = Yes
+int8_t arrest=0;       // Arrest: 0 = No, 1 = Yes
+int32_t babybear=0;    // Difference (in Lux) between Left and Right
+int32_t acc_data[3];   // Difference of Accelerometer
+int32_t diff=0;        // Babybear or Accelerometer difference [--> Send BLE]
+int32_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
+int32_t pos=0;         // Motor Position [--> Send BLE]
+int32_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
 
 /* Initializations ------------------------------------------------------------*/
@@ -122,34 +144,50 @@
 // Initializing I2C bus
 DevI2C dev_i2c(D14, D15);
 
-// Initializing Motor Component IHM01A1
-static L6474 *motor;
-
-// Initializing Babybear Component 6180XA1
+#ifdef FlightSense
+// Initializing FlightSense Component 6180XA1
 static X_NUCLEO_6180XA1 *board;
 MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right;
-    
-// Initializing MEMS Component IKS01A1
+#endif
+
+#ifdef EasySpin
+// Initializing EasySpin Component IHM01A1
+static L6474 *motor;
+#endif
+
+#ifdef Sensors    
+// Initializing Sensors Component IKS01A1
 static X_NUCLEO_IKS01A1 *mems;
 MotionSensor *accelerometer;
+#endif
 
 InterruptIn mybutton(USER_BUTTON);
 
-AnalogIn analog_read(PC_3); // A1 Conflict with BLE SPI_CS del BLE --> Changed in Morpho PC_3
+AnalogIn analog_read(PC_3); // A1 Conflict with BLE SPI_CS --> Changed in Morpho PC_3
 
 /* User_Button_Pressed -------------------------------------------------------*/
 
-void User_Button_Pressed()
+void User_Button_Pressed(void)
 {
 
     if (start>0)    { Display++; }
+#ifdef Sensors
     if (Display>2)  { Display=0; }
+#else
+    if (Display>1)  { Display=0; }
+#endif 
     if (start==0)   { start=1; }
-    
-    printf("PUSH Display %d\r\n", Display+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);
 
 }
 
+#ifdef Ble
+
 /* Bluetooth CallBack ---------------------------------------------------------*/
 
 static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
@@ -167,33 +205,44 @@
     
 }
 
+// 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 );
+{   
+    //printf ("myonDataWriteCallback attr_handle: %x  att_data[3]: %x  data_length: %d\n\r", eventDataP->handle, eventDataP->data[3], eventDataP->len );
     
     if (p_customcontrolservice->isContHandle(eventDataP->handle)) // Only if write come from this Service
     {
-        uint8_t Command = eventDataP->data[3];  // Riceve 8 byte: data[0] + data[1] + data[2] + data[3]
-        printf("myonDataWriteCallback (Command %x)\r\n", Command);
+        // Receive 8 byte: data[0] + data[1] + data[2] + data[3]
+        printf("\r\n\r\nmyonDataWriteCallback (data[0] %x)", eventDataP->data[0]);
+        printf("\r\nmyonDataWriteCallback (data[1] %x)", eventDataP->data[1]);
+        printf("\r\nmyonDataWriteCallback (data[2] %x)", eventDataP->data[2]);
+        printf("\r\nmyonDataWriteCallback (data[3] %x)", eventDataP->data[3]);
+        printf("\r\nmyonDataWriteCallback (data %d)", eventDataP->data[0]+eventDataP->data[1]+eventDataP->data[2]+eventDataP->data[3]);
+        
+        User_Button_Pressed(); // Change Display Status
     }
 }
 
 static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
 {
-    printf("onConnectionCallback (Line %d)\r\n", __LINE__);   
+    printf("\r\n\r\nonConnectionCallback (Line %d)", __LINE__);   
 }
 
 static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
 {
-    printf("onDisconnectionCallback (Line %d)\r\n", __LINE__);
+    printf("\r\n\r\nonDisconnectionCallback (Line %d)", __LINE__);
     p_BLEdev->gap().startAdvertising();
 }
 
+#endif
+
 /* Bluetooth Initialization ---------------------------------------------------*/
 
 bool BLE_Initialization(void)
 {
 
+#ifdef Ble
+
     p_BLEdev = new BLE;
     if (!p_BLEdev) { printf("\r\nBLE Device creation failed\r\n"); }
     const Gap::Address_t BLE_address_BE = {BLE_DEV_MAC};        
@@ -213,7 +262,7 @@
     //p_BLEdev->gap().onTimeout(onTimeoutCallback);
 
     // BLE Services
-    p_customcontrolservice =  new CustomControlService(*p_BLEdev);   
+    p_customcontrolservice = new CustomControlService(*p_BLEdev);   
     p_customsensorservice = new CustomSensorService(*p_BLEdev);
 
     // Setup BLE Advertising
@@ -227,6 +276,10 @@
     p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
     p_BLEdev->gap().startAdvertising();
 
+    printf("Initialization Ble OK (Line %d)\r\n", __LINE__);  
+
+#endif
+
     return true;
     
 }
@@ -236,19 +289,21 @@
 bool Initialization(void)
 {
     
-    dev_i2c.frequency(100000);     // Default 100KHz. At 10KHz, 20KHz, 200KHz and 400KHz doesn't work
+    dev_i2c.frequency(100000);      // Default 100KHz. At 10KHz, 20KHz, 200KHz and 400KHz doesn't work
     
 //----    
-   
-    // Initializing Babybear Component
-    board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC); // OLD: Don't use NC because it doesn't set InterruptIn correctly. Used PA_4
-                                                                // NEW: Now can use NC, problem solved editing x_nucleo_6180xa1.h and vl6180x_class.h
+
+#ifdef FlightSense
+
+    // 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");
 
     // Put GPIO not used as Interrupt in Hi-Z
     status_t=board->sensor_top->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
-    //status_b=board->sensor_botton->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); No Present
+    //status_b=board->sensor_botton->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); Not Present
     status_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
     status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
     
@@ -258,19 +313,15 @@
     status_l=board->sensor_left->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
     status_r=board->sensor_right->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
 
-    printf("Init Babybear OK\r\n");
+    printf("Init FlightSense OK\r\n");
+
+#endif
     
 //----
 
-    // Initializing MEMS Component
-    mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
-    accelerometer = mems->GetAccelerometer();
-   
-    printf("Init MEMS OK\r\n");
-    
-//----   
+#ifdef EasySpin
 
-    // Initializing Motor Component
+    // Initializing EasySpin Component
     motor = new L6474(D2, D8, PB_2, D9, D10, dev_spi); // D7 conflict with BNRG_RST (when motor change direction 0-1 reset BLE)
                                                        // Used Morpho PB_2 and made HW wiring
     if (motor->Init(NULL) != COMPONENT_OK)
@@ -284,8 +335,23 @@
     motor->SetMaxSpeed(SET_MAX);        // Variable by Light/Mems Sensors
     motor->SetMinSpeed(SET_MIN);
 
-    printf("Init Motor OK\r\n");
+    printf("Init EasySpin OK\r\n");
+
+#endif
+
+//----  
+
+#ifdef Sensors
 
+    // Initializing Sensors Component
+    mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
+    accelerometer = mems->GetAccelerometer();
+   
+    printf("Init Sensors OK\r\n");
+
+#endif
+    
+    printf("Initialization OK (Line %d)\r\n", __LINE__);    
     return true;
 
 }
@@ -295,15 +361,16 @@
 void Measure_Babybear(void)
 {
 
+#ifdef FlightSense
+
     status_l=board->sensor_left->GetMeasurement(als_continuous_polling, &data_sensor_left);
     status_r=board->sensor_right->GetMeasurement(als_continuous_polling, &data_sensor_right);
 
     babybear = data_sensor_right.lux - data_sensor_left.lux;
 
-    diff = abs(babybear);
+    diff = babybear;
 
-    if (babybear>0) { left=0; right=1; }
-    if (babybear<0) { left=1; right=0; }
+#endif
 
 }
 
@@ -312,12 +379,13 @@
 void Measure_Accelerometer(void)
 {
 
+#ifdef Sensors
+
     accelerometer->Get_X_Axes(acc_data);
 
-    diff = abs(acc_data[0]);
+    diff = acc_data[0];
 
-    if (acc_data[0]>0) { left=0; right=1; }
-    if (acc_data[0]<0) { left=1; right=0; }
+#endif
 
 }
 
@@ -326,21 +394,28 @@
 void Control_Motor(void)
 {
 
-    //printf("Diff: %d lux/mems\n\r", diff);
-    motor->SetMaxSpeed(diff);
+#ifdef EasySpin
+
+    diff_abs = abs(diff);
 
-    if (diff>TOLLERANCE) {
-        if (diff <=RANGE_1)
+    //printf("Diff: %d lux/mems\n\r", diff_abs);
+    motor->SetMaxSpeed(diff_abs);
+
+    if (diff>0) { left=0; right=1; }
+    if (diff<0) { left=1; right=0; }
+
+    if (diff_abs>TOLLERANCE) {
+        if (diff_abs <=RANGE_1)
         {
             if (left)  { strcpy(DisplayStr,"E___"); }
             if (right) { strcpy(DisplayStr,"___3"); }
         }
-        else if (diff >RANGE_1 & diff <=RANGE_2)
+        else if (diff_abs >RANGE_1 & diff_abs <=RANGE_2)
         {
             if (left)  { strcpy(DisplayStr,"E==="); }
             if (right) { strcpy(DisplayStr,"===3"); }
         }
-        else if (diff >RANGE_2)
+        else if (diff_abs >RANGE_2)
         {
             if (left)  { strcpy(DisplayStr,"E~~~"); }
             if (right) { strcpy(DisplayStr,"~~~3"); }
@@ -351,10 +426,10 @@
         if (right & dir==1) { changedir=1; }
 
         // Run only if Stop or Change Direction
-        if (diff>TOLLERANCE & (dir==0 | changedir==1)) {
+        if (diff_abs>TOLLERANCE & (dir==0 | changedir==1)) {
             if (left)   { motor->Run(StepperMotor::FWD); dir=1; changedir=0; }
             if (right)  { motor->Run(StepperMotor::BWD); dir=2; changedir=0; }
-            printf("RUN\n\r");
+            printf("\n\rRUN");
         }
     }
 
@@ -363,11 +438,11 @@
     if (pos>STOP | pos<-STOP) {
         if (pos>0) { motor->GoTo(STOP); }
         if (pos<0) { motor->GoTo(-STOP); }
-        printf("GOTO\n\r");
+        printf("\n\rGOTO");
     }
 
     // Stop Motor
-    if (diff<=TOLLERANCE) { 
+    if (diff_abs<=TOLLERANCE) { 
         arrest=1; 
         if (Display==0) { strcpy(DisplayStr,"----"); }
         if (Display==2) { strcpy(DisplayStr,"E  3"); }
@@ -378,9 +453,11 @@
         dir=0;
         changedir=0;
         arrest=0;
-        printf("STOP\n\r");
+        printf("\n\rSTOP");
     }
 
+#endif
+
 }
 
 /* Measure_SolarPanel --------------------------------------------------------*/
@@ -389,15 +466,12 @@
 {
     
     // AnalogIn: 0V return 0.0 , 3.3V return 1.0
-    float measure = analog_read.read() * 3300;
+    measure = analog_read.read() * 3300;
     //printf("Measure = %.0f mV\r\n", measure);
     //board->display->DisplayDigit("A", 0);
 
-    if (Display==1) { sprintf(DisplayStr, "%.0f", measure); }
-
-    board->display->DisplayString(DisplayStr, 4);
-    //printf("%s\n\r", DisplayStr);
-    
+    if (Display==1) { sprintf(DisplayStr, "%d", measure); }
+     
 }
 
 /* Main ----------------------------------------------------------------------*/
@@ -406,30 +480,33 @@
 {
 
     // Printing to the console
-    printf("SunTracker by Fabio Brembilla\r\n\n");
+    printf("\r\n\r\nSunTracker by Fabio Brembilla\r\n\r\n");
        
     Initialization();
-    printf("Initialization OK (Line %d)\r\n", __LINE__);    
+    BLE_Initialization();
     
-    BLE_Initialization();
-    printf("BLE_Initialization OK (Line %d)\r\n", __LINE__);  
-        
     mybutton.fall(&User_Button_Pressed);
-    printf("Wait Push Button\r\n");
 
     // Loop until push User Button to Set 0 Point
+    printf("\r\nWait PUSH Button");
     strcpy(DisplayStr,"pusH");
     while(start<1) {
+        #ifdef FlightSense
         board->display->DisplayString(DisplayStr, 4);
-        //p_BLEdev->waitForEvent();
+        p_BLEdev->waitForEvent();
         //printf("%s\n\r", DisplayStr);
+        #endif 
     }
 
-    printf("Start Main Loop\r\n");
+    printf("\r\n\r\nStart Main Loop");
 
-    motor->Enable(); // To put the motor on hold execute CmdEnable
+    #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)
@@ -439,14 +516,41 @@
 
         Control_Motor();
         Measure_SolarPanel();
-                
+        
+        #ifdef FlightSense
+        board->display->DisplayString(DisplayStr, 4);
+        //printf("%s\n\r", DisplayStr);
+        #endif   
+        
+        #ifdef Ble
         INTLOOP++;
-        if (INTLOOP==100) { p_customsensorservice->sendEnvPosition(rand(),0); printf("Read BLE\n\r"); INTLOOP=0; }
-        
+        //if (INTLOOP==100) { p_customsensorservice->sendEnvPosition(rand(),0); printf("Read BLE\n\r"); INTLOOP=0; }
+        if (INTLOOP==100)
+        {
+            printf("\n\r\n\rSend BLE Display %d", Display);
+            //p_customcontrolservice->sendControlState(Display);
+            p_customcontrolservice->updateControlState(Display);            
+            
+            printf("\n\rSend BLE Difference %d lux/mems", diff); // Send BLE diff, no diff_abs
+            //p_customsensorservice->sendEnvDifference(diff);
+            p_customsensorservice->updateEnvDifference(diff);
+
+            printf("\n\rSend BLE Position %d", pos);
+            //p_customsensorservice->sendEnvPosition(pos);
+            p_customsensorservice->updateEnvPosition(pos);
+            
+            printf("\n\rSend BLE Sunpanel %d mV", measure);            
+            //p_customsensorservice->sendEnvSunpanel(measure);
+            p_customsensorservice->updateEnvSunpanel(measure);
+            
+            INTLOOP=0;
+        }
+
         p_BLEdev->waitForEvent();
+        #endif
     }
 
     //status_l=board->sensor_left->StopMeasurement(als_continuous_polling);
     //status_r=board->sensor_right->StopMeasurement(als_continuous_polling);
 
-}
+}
\ No newline at end of file

--- a/main_copy.cpp.h	Fri Mar 11 09:53:49 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,129 +0,0 @@
-/******************************************************************************
-  * @file    CustomControlService.h
-  * @author  Fabio Brembilla
-  * @version V1.0.0
-  * @date    January, 2016
-  * @brief   SunTracker Custom Service for BlueTooth (IDB0XA1 expansion board)
-  *****************************************************************************
-  * @attention
-  *
-  * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
-  *
-  * Redistribution and use in source and binary forms, with or without modification,
-  * are permitted provided that the following conditions are met:
-  *   1. Redistributions of source code must retain the above copyright notice,
-  *      this list of conditions and the following disclaimer.
-  *   2. Redistributions in binary form must reproduce the above copyright notice,
-  *      this list of conditions and the following disclaimer in the documentation
-  *      and/or other materials provided with the distribution.
-  *   3. Neither the name of STMicroelectronics nor the names of its contributors
-  *      may be used to endorse or promote products derived from this software
-  *      without specific prior written permission.
-  *
-  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-  *
-  ******************************************************************************
-*/
-
-#ifndef __CUSTOM_BLE_CONTROL_SERVICE_H__
-#define __CUSTOM_BLE_CONTROL_SERVICE_H__
-#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) ) )                         
-                            
-#define SIZEOF_CONTROL_DATA_LEN  2+4+1+1
-
-/* 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 USER_BUTTON_CHAR_UUID    = {0x00,0x00,0x00,0x01,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A,0x0A}; //User Button Control
-
-/* 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)
-        {  
-        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 *));              
-                
-        ble.gattServer().addService(controlService);                              
-                
-        isEnabledControlNotify = false;
-        memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN); 
-        serviceAdded = true;
-    }
-
-// Tests Method
-
-    void enNotify (Gap::Handle_t handle) {
-            if (isContHandle(handle)) { 
-                PRINTF("enNotify! %d\n\r", handle); isEnabledControlNotify = true; return; }    
-    }
-    
-    void disNotify (Gap::Handle_t handle) {
-            if (isContHandle(handle)) { 
-                            isEnabledControlNotify = false; return; }    
-    }   
-    
-    bool isContHandle (Gap::Handle_t handle) {
-            if (handle == userbuttonCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    }
-    
-    void updateConnectionStatus(ConnectionStatus_t status) {            
-            isEnabledControlNotify = false;
-            memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN);             
-    }      
-
-/*
-  
-    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;
-    }
-
-*/    
-
-// Variables Initialization
-private:
-        BLE                              &ble;
-        uint8_t                          controlData[SIZEOF_CONTROL_DATA_LEN];                
-        uint8_t                          controlState;
-        GattCharacteristic               userbuttonCharacteristic;  
-        bool                             isEnabledControlNotify;
-
-};
-
-#endif /* #ifndef __CUSTOM_BLE_CONTROL_SERVICE_H__*/
-

--- a/mbed.bld	Fri Mar 11 09:53:49 2016 +0000
+++ b/mbed.bld	Wed Mar 16 19:17:08 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/87f2f5183dfb
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/c0f6e94411f5
\ No newline at end of file