Revision 5:76fb6b783487, committed 2016-02-03

Comitter:

fabiombed

Date:

Wed Feb 03 11:22:17 2016 +0000

Parent:

4:1d3d071a4c2c

Child:

6:4cbf7303b496

Commit message:

BLE Services Created

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CustomControlService.h	Wed Feb 03 11:22:17 2016 +0000
@@ -0,0 +1,130 @@
+/******************************************************************************
+  * @file    CustomControlService.h
+  * @author  Fabio Brembilla
+  * @version V1.0.0
+  * @date    January 22th, 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 Config Service */
+class CustomConfigService  {
+public:
+    CustomConfigService(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)
+        {  
+        static bool serviceAdded = false; // To be sure to define just one time
+        if (serviceAdded) {
+            return;
+        }
+
+        GattCharacteristic *charTable[] = {&userbuttonCharacteristic};
+
+        GattService   configService(CONTROL_SERVICE_UUID_128, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));              
+                
+        ble.gattServer().addService(configService);                              
+                
+        isEnabledControlNotify = false;
+        memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN); 
+        serviceAdded = true;
+    }
+
+// Tests Method
+//----------------INIZIO  
+/*
+  
+    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 updateConnectionStatus(ConnectionStatus_t status) {            
+            isEnabledControlNotify = false;
+            memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN);             
+            }   
+    
+    bool isConfHandle (Gap::Handle_t handle) {
+            if (handle == userbuttonCharacteristic.getValueAttribute().getHandle()) return true;
+            return false;
+    }           
+
+    void enNotify (Gap::Handle_t handle) {
+            if (isConfHandle(handle)) { 
+                PRINTF("enNotify! %d\n\r", handle); isEnabledControlNotify = true; return; }    
+    }
+    
+    void disNotify (Gap::Handle_t handle) {
+            if (isConfHandle(handle)) { 
+                            isEnabledControlNotify = false; return; }    
+    }   
+
+*/    
+//----------------FINE  
+
+// 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__*/
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CustomSensorsService.h	Wed Feb 03 11:22:17 2016 +0000
@@ -0,0 +1,240 @@
+/******************************************************************************
+  * @file    CustomSensorsService.h
+  * @author  Fabio Brembilla
+  * @version V1.0.0
+  * @date    January 22th, 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_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
+
+/* Custom Sensors Service */
+class CustomSensorService  {
+public:
+    CustomSensorService(BLEDevice &_ble) :
+        ble(_ble),
+                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,
+                                       GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE)          
+        {  
+        static bool serviceAdded = false; // To be sure to define just one time
+        if (serviceAdded) {
+            return;
+        }
+
+        GattCharacteristic *charTable[] = {&positionCharacteristic, &sunpanelCharacteristic};                 
+                                                                                                                                                         
+        GattService   envService(SENS_SERVICE_UUID_128, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));               
+                
+        ble.gattServer().addService(envService);             
+
+        isEnabledPositionNotify         = false;
+        isEnabledSunpanelNotify         = false;            
+
+        memset (envPosition, 0, POSITION_DATA_LEN);  
+        memset (envSunpanel, 0, SUNPANEL_DATA_LEN);                                
+                                                                         
+        isBTLEConnected                 = DISCONNECTED;                                                                  
+        serviceAdded                    = true;                                                                                                                                  
+    }
+
+// Tests Method
+//----------------INIZIO  
+/*
+
+    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);              
+    } 
+    
+*/    
+//----------------FINE  
+                                  
+        
+    void enNotify (Gap::Handle_t handle) {
+            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 (isPositionHandle(handle)) { isEnabledPositionNotify = false; memset(envPosition,0,POSITION_DATA_LEN); return; }        
+            if (isSunpanelHandle(handle)) { isEnabledSunpanelNotify = false; memset(envSunpanel,0,SUNPANEL_DATA_LEN); return; }                   
+    }
+        
+    bool isPositionNotificationEn (void) {
+            return isEnabledPositionNotify;
+    }
+        
+    bool isSunpanelNotificationEn (void) {
+            return isEnabledSunpanelNotify;
+    }
+                 
+    bool isPositionHandle (Gap::Handle_t handle) {
+            if (handle == positionCharacteristic.getValueAttribute().getHandle()) return true;
+            return false;
+    }
+
+    bool isSunpanelHandle (Gap::Handle_t handle) {
+            if (handle == sunpanelCharacteristic.getValueAttribute().getHandle()) return true;
+            return false;
+    }
+        
+    void updateConnectionStatus(ConnectionStatus_t status) {   
+            isEnabledPositionNotify = false;
+            isEnabledSunpanelNotify = false;    
+            
+            memset (envPosition, 0, POSITION_DATA_LEN);  
+            memset (envSunpanel, 0, SUNPANEL_DATA_LEN);                             
+            isBTLEConnected = status; 
+    }
+                
+// Variables Initialization     
+private:
+
+    BLEDevice            &ble;
+    uint8_t              envPosition [POSITION_DATA_LEN];
+    uint8_t              envSunpanel [SUNPANEL_DATA_LEN];        
+
+    GattCharacteristic   positionCharacteristic;     
+    GattCharacteristic   sunpanelCharacteristic;   
+ 
+    ConnectionStatus_t   isBTLEConnected;
+
+    bool                 isEnabledPositionNotify;
+    bool                 isEnabledSunpanelNotify;
+               
+};
+
+#endif /* #ifndef __CUSTOM_BLE_SENSORS_SERVICE_H__*/

--- a/CustomSunTrackerService.h	Tue Jan 26 13:29:53 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,372 +0,0 @@
-/******************************************************************************
- * @file    CustomSunTrackerService.h
- * @author  Fabio Brembilla
- * @version V1.0.0
- * @date    January 22th, 2016
- * @brief   SunTracker Custom Service for BlueTooth (IDB0XA1 expansion board)
- ******************************************************************************
-
- * mbed Microcontroller Library
- * Copyright (c) 2006-2013 ARM Limited
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef __CUSTOM_BLE_SENSORS_SERVICE_H__
-#define __CUSTOM_BLE_SENSORS_SERVICE_H__
-
-#include "BLE.h"
-#include "debug.h" // Need for PRINTF
-#include "Utils.h" // Need for STORE_LE_16 and _32
-
-typedef struct {
-    int32_t AXIS_X;
-    int32_t AXIS_Y;
-    int32_t AXIS_Z;
-} AxesRaw_TypeDef;
-
-typedef enum ConnectionStatus_t {
-    DISCONNECTED    =0,
-    CONNECTED       =1
-}cns_t;
-
-const unsigned   LENGTH_OF_LONG_UUID = 16;
-typedef uint16_t ShortUUIDBytes_t;
-typedef uint8_t  LongUUIDBytes_t[LENGTH_OF_LONG_UUID];
-
-
-const LongUUIDBytes_t SENS_SERVICE_UUID_128 =           { 0x1b,0xc5,0xa5,0xd5,0x02,0x00,0xb4,0x9a,0xe1,0x11,0x01,0x00,0x00,0x00,0x00,0x00}; // temp, pressure, humidity, 
-
-const LongUUIDBytes_t SENS_TEMP_CHAR_UUID_128 =         { 0x1b,0xc5,0xa5,0xd5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x04,0x00};  
-const LongUUIDBytes_t SENS_HUMI_CHAR_UUID_128 =         { 0x1b,0xc5,0xa5,0xd5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x08,0x00};  
-const LongUUIDBytes_t SENS_PRES_CHAR_UUID_128 =         { 0x1b,0xc5,0xa5,0xd5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x10,0x00};  
-const LongUUIDBytes_t SENS_MAGN_CHAR_UUID_128   =       { 0x1b,0xc5,0xa5,0xd5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x20,0x00}; 
-const LongUUIDBytes_t SENS_GYRO_CHAR_UUID_128   =       { 0x1b,0xc5,0xa5,0xd5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x40,0x00}; 
-const LongUUIDBytes_t SENS_ACCE_CHAR_UUID_128   =       { 0x1b,0xc5,0xa5,0xd5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0x80,0x00};
-const LongUUIDBytes_t SENS_ACC_GYRO_MAG_CHAR_UUID_128 = { 0x1b,0xc5,0xa5,0xd5,0x02,0x00,0x36,0xac,0xe1,0x11,0x01,0x00,0x00,0x00,0xE0,0x00};
-
-
-#define TEMP_DATA_LEN   2+2
-#define HUM_DATA_LEN    2+2
-#define PRES_DATA_LEN   2+4
-#define ACC_DATA_LEN    6+2 
-#define MAG_DATA_LEN    6+2
-#define GYRO_DATA_LEN 6+2
-#define ACCGYROMAG_DATA_LEN 2+3*3*2
-
-
-/* Custom Sensors Service */
-class CustomSensorService  {
-public:
-    CustomSensorService(BLEDevice &_ble) :
-        ble(_ble),
-                envTemperatureCharacteristic(SENS_TEMP_CHAR_UUID_128,envTemperature, TEMP_DATA_LEN, TEMP_DATA_LEN,
-                                                                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
-                envHumidityCharacteristic(SENS_HUMI_CHAR_UUID_128, envHumidity, HUM_DATA_LEN, HUM_DATA_LEN,
-                                                                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
-                envPressureCharacteristic(SENS_PRES_CHAR_UUID_128, envPressure, PRES_DATA_LEN, PRES_DATA_LEN,
-                                                                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),               
-                envMagnetometerCharacteristic(SENS_MAGN_CHAR_UUID_128,envMagn, MAG_DATA_LEN, MAG_DATA_LEN,                  
-                                                                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),                                               
-                envAccelerometerCharacteristic(SENS_ACCE_CHAR_UUID_128,envAcce, ACC_DATA_LEN, ACC_DATA_LEN,
-                                                                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),                   
-                envGyroCharacteristic(SENS_GYRO_CHAR_UUID_128,envGyro, GYRO_DATA_LEN, GYRO_DATA_LEN,
-                                                                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),                   
-                envAccGyroMagCharacteristic(SENS_ACC_GYRO_MAG_CHAR_UUID_128,envAccGyroMag, ACCGYROMAG_DATA_LEN, ACCGYROMAG_DATA_LEN,
-                                                                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)
-        {
-            
-
- 
-        static bool serviceAdded = false; /* We should only ever need to add the env service once. */
-        if (serviceAdded) {
-            return;
-        }
-
-        GattCharacteristic *charTable[] = {&envTemperatureCharacteristic, &envHumidityCharacteristic, &envPressureCharacteristic, &envMagnetometerCharacteristic,
-                                                                             &envAccelerometerCharacteristic, &envGyroCharacteristic, &envAccGyroMagCharacteristic};                 
-                                                                                                                                                         
-        GattService   envService(SENS_SERVICE_UUID_128, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));               
-                
-        ble.gattServer().addService(envService);             
-
-        isEnabledTempNotify             = false;
-        isEnabledHumNotify              = false;
-        isEnabledPresNotify             = false;
-        isEnabledGyroNotify             = false;            
-        isEnabledAccNotify              = false;
-        isEnabledMagNotify              = false;
-        isEnabledAccGyroMagNotify       = false;                  
-                                                                             
-        isTempCalibrated                = false;
-        isHumCalibrated                 = false;
-        isPresCalibrated                = false;        
-        isMagCalibrated                 = false;
-        isAccCalibrated                 = false;        
-        isAGyroCalibrated               = false;                
-
-        memset (pastenvTemperature, 0, TEMP_DATA_LEN);  
-        memset (pastenvHumidity,        0, HUM_DATA_LEN);           
-        memset (pastenvPressure,        0, PRES_DATA_LEN);                          
-                                                                         
-        isBTLEConnected                 = DISCONNECTED;                                                                  
-        serviceAdded                    = true;                                                                                                                                  
-    }
-
-    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);         
-    }               
-        
-    /**
-     * Update the temperature with a new value. Valid values range from
-     * 0..100. Anything outside this range will be ignored.
-     * @param newLevel New level. */
-    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) {
-            if (isTempHandle(handle)) { isEnabledTempNotify = true; memset(pastenvTemperature,0,TEMP_DATA_LEN); return; }
-            if (isHumHandle(handle))  { isEnabledHumNotify  = true; memset(pastenvHumidity,0,HUM_DATA_LEN);       return; }           
-            if (isPresHandle(handle)) { isEnabledPresNotify = true; memset(pastenvPressure,0,PRES_DATA_LEN);      return; }                       
-            if (isGyroHandle(handle)) { isEnabledGyroNotify = true; return; } 
-            if (isAccHandle(handle))  { isEnabledAccNotify  = true; return; } 
-            if (isMagHandle(handle))  { isEnabledMagNotify  = true; return; } 
-            if (isAccGyroMagHandle(handle)) { isEnabledAccGyroMagNotify = true; return; }             
-    }
-
-    void disNotify (Gap::Handle_t handle) {
-            if (isTempHandle(handle)) { isEnabledTempNotify = false; memset(pastenvTemperature,0,TEMP_DATA_LEN); return; }        
-            if (isHumHandle(handle))  { isEnabledHumNotify  = false; memset(pastenvHumidity,0,HUM_DATA_LEN);       return; }      
-            if (isPresHandle(handle)) { isEnabledPresNotify = false; memset(pastenvPressure,0,PRES_DATA_LEN);      return; }                                  
-            if (isGyroHandle(handle)) { isEnabledGyroNotify = false; return; }    
-            if (isAccHandle(handle))  { isEnabledAccNotify  = false; return; }    
-            if (isMagHandle(handle))  { isEnabledMagNotify  = false; return; }    
-            if (isAccGyroMagHandle(handle)) { isEnabledAccGyroMagNotify = false; return; }                
-    }
-        
-    bool isTempNotificationEn (void) {
-            return isEnabledTempNotify;
-    }
-        
-    bool isHumNotificationEn (void) {
-            return isEnabledHumNotify;
-    }
-
-    bool isPresNotificationEn (void) {
-            return isEnabledPresNotify;
-    }
-
-    bool isGyroNotificationEn (void) {
-            return isEnabledGyroNotify;
-    }   
-
-    bool isAccNotificationEn (void) {
-            return isEnabledAccNotify;
-    }   
-        
-    bool isMagNotificationEn (void) {
-            return isEnabledMagNotify;
-    }   
-            
-    bool isAccGyroMagNotificationEn (void) {
-            return isEnabledAccGyroMagNotify;
-    }           
-        
-    bool isTempHandle (Gap::Handle_t handle) {
-            if (handle == envTemperatureCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    }
-
-    bool isHumHandle (Gap::Handle_t handle) {
-            if (handle == envHumidityCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    }
-    
-    bool isPresHandle (Gap::Handle_t handle) {
-            if (handle == envPressureCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    } 
-
-    bool isMagHandle (Gap::Handle_t handle) {
-            if (handle == envMagnetometerCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    }
-    bool isAccHandle (Gap::Handle_t handle) {
-            if (handle == envAccelerometerCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    }
-    bool isGyroHandle (Gap::Handle_t handle) {
-            if (handle == envGyroCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    }
-    bool isAccGyroMagHandle (Gap::Handle_t handle) {
-            if (handle == envAccGyroMagCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    }
-        
-    void updateConnectionStatus(ConnectionStatus_t status) {   
-            isEnabledTempNotify             = false;
-            isEnabledHumNotify              = false;
-            isEnabledPresNotify             = false;
-            isEnabledGyroNotify             = false;            
-            isEnabledAccNotify              = false;
-            isEnabledMagNotify              = false;
-            isEnabledAccGyroMagNotify = false;                  
-                                                                             
-            isTempCalibrated                = false;
-            isHumCalibrated                 = false;
-            isPresCalibrated                = false;        
-            isMagCalibrated                 = false;
-            isAccCalibrated                 = false;        
-            isAGyroCalibrated               = false;            
-            
-            memset (pastenvTemperature, 0, TEMP_DATA_LEN);  
-            memset (pastenvHumidity, 0, HUM_DATA_LEN);          
-            memset (pastenvPressure, 0, PRES_DATA_LEN);                     
-            isBTLEConnected = status; 
-    }
-                
-        
-private:
-
-    BLEDevice            &ble;
-    uint8_t              envTemperature     [TEMP_DATA_LEN];                /* in C  */
-    uint8_t              pastenvTemperature [TEMP_DATA_LEN];        
-    uint8_t              envHumidity        [HUM_DATA_LEN];                     /* in %  */
-    uint8_t              pastenvHumidity    [HUM_DATA_LEN];             
-    uint8_t              envPressure        [PRES_DATA_LEN];                        /* in mBar */
-    uint8_t              pastenvPressure    [PRES_DATA_LEN];            
-    uint8_t              envMagn            [MAG_DATA_LEN];         
-    uint8_t              envGyro            [GYRO_DATA_LEN];
-    uint8_t              envAcce            [ACC_DATA_LEN];
-    uint8_t              envAccGyroMag      [ACCGYROMAG_DATA_LEN];
-
-    GattCharacteristic   envTemperatureCharacteristic;     
-    GattCharacteristic   envHumidityCharacteristic;   
-    GattCharacteristic   envPressureCharacteristic;
-
-    GattCharacteristic   envMagnetometerCharacteristic;
-    GattCharacteristic   envAccelerometerCharacteristic;
-    GattCharacteristic   envGyroCharacteristic;
-    GattCharacteristic   envAccGyroMagCharacteristic;
-
-    ConnectionStatus_t   isBTLEConnected;
-
-    bool                 isEnabledTempNotify;
-    bool                 isEnabledHumNotify;
-    bool                 isEnabledPresNotify;
-    bool                 isEnabledGyroNotify;           
-    bool                 isEnabledAccNotify;
-    bool                 isEnabledMagNotify;
-    bool                 isEnabledAccGyroMagNotify;                 
-        
-    bool                 isTempCalibrated;
-    bool                 isHumCalibrated;
-    bool                 isPresCalibrated;      
-    bool                 isMagCalibrated;
-    bool                 isAccCalibrated;       
-    bool                 isAGyroCalibrated;            
-               
-};
-
-#endif /* #ifndef __CUSTOM_BLE_SENSORS_SERVICE_H__*/

--- a/main.cpp	Tue Jan 26 13:29:53 2016 +0000
+++ b/main.cpp	Wed Feb 03 11:22:17 2016 +0000
@@ -59,8 +59,33 @@
 /* Calibration files. */
 #include "MotionFX_Manager.h" // Need for osxMFX_calibFactor
 
-/* BlueTooth Custom Service files. */
-#include "CustomSunTrackerService.h"
+/* BlueTooth -----------------------------------------------------------------*/
+
+#include "debug.h" // Need for PRINTF
+#include "Utils.h" // Need for STORE_LE_16 and _32
+
+typedef struct {
+    int32_t AXIS_X;
+    int32_t AXIS_Y;
+    int32_t AXIS_Z;
+} AxesRaw_TypeDef;
+
+typedef enum ConnectionStatus_t {
+    DISCONNECTED    =0,
+    CONNECTED       =1
+}cns_t;
+
+const unsigned   LENGTH_OF_LONG_UUID = 16;
+typedef uint16_t ShortUUIDBytes_t;
+typedef uint8_t  LongUUIDBytes_t[LENGTH_OF_LONG_UUID];
+
+#include "CustomControlService.h"
+#include "CustomSensorsService.h"
+
+static BLE * p_BLEdev = NULL;
+
+#define BLE_DEV_NAME "SunTrack"
+#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF
 
 /* Definitions ---------------------------------------------------------------*/
 
@@ -206,7 +231,6 @@
 
 }
 
-
 /* Control_Motor -------------------------------------------------------------*/
 
 void Control_Motor(void)
@@ -306,6 +330,65 @@
 
 }
 
+/* Bluetooth CallBack ---------------------------------------------------------*/
+
+static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
+{
+    
+}
+
+static void onUpdatesDisabledCallback(Gap::Handle_t handle)
+{
+    
+}
+
+static void onDataReadCallback(const GattReadCallbackParams *eventDataP)
+{
+    
+}
+
+static void myonDataWriteCallback(const GattWriteCallbackParams *eventDataP)
+{
+    
+}
+
+static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
+{
+    
+}
+
+static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
+{
+    
+}
+
+/* Bluetooth Initialization ---------------------------------------------------*/
+
+void BLE_Initialization(void)
+{
+
+    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};        
+    p_BLEdev->gap().setAddress(BLEProtocol::AddressType::PUBLIC, BLE_address_BE);
+printf("Line: %d \r\n", __LINE__);       
+    p_BLEdev->init();         
+printf("Line: %d \r\n", __LINE__);    
+
+    // Set BLE CallBack Functions
+    p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
+    p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
+    p_BLEdev->gattServer().onDataRead(onDataReadCallback);
+    p_BLEdev->gattServer().onDataWritten(myonDataWriteCallback);
+    p_BLEdev->gap().onConnection(onConnectionCallback);
+    p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
+    //p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback);    
+    //p_BLEdev->gattServer().onDataSent(onDataSentCallback);    
+    //p_BLEdev->gap().onTimeout(onTimeoutCallback);
+
+}
+
 /* Main ----------------------------------------------------------------------*/
 
 int main()
@@ -313,6 +396,8 @@
 
     Initialization();
 
+    //BLE_Initialization();
+
     mybutton.fall(&User_Button_Pressed);
 
     /* Printing to the console. */