Revision 4:1d3d071a4c2c, committed 2016-01-26

Comitter:

fabiombed

Date:

Tue Jan 26 13:29:53 2016 +0000

Parent:

3:2382ac13497a

Child:

5:76fb6b783487

Commit message:

fixed compile error and all libraries updated at last release

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CustomSunTrackerService.h	Tue Jan 26 13:29:53 2016 +0000
@@ -0,0 +1,372 @@
+/******************************************************************************
+ * @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__*/

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotionFX_Manager.h	Tue Jan 26 13:29:53 2016 +0000
@@ -0,0 +1,140 @@
+/**
+  ******************************************************************************
+  * @file    MotionFX_Manager.h
+  * @author  Central Lab
+  * @version V1.1.0
+  * @date    20-Jannuary-2015
+  * @brief   This file includes sensor fusion interface functions
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+  *
+  * Licensed under MCD-ST Liberty SW License Agreement V2, (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.st.com/software_license_agreement_liberty_v2
+  *
+  * 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 _MOTIONFX_MANAGER_H_
+#define _MOTIONFX_MANAGER_H_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+//#define USE_SENSOR_FUSION_LIB    /** uncomment only if STM32_OSX_MotionFX_Library is linked and licensed **/
+
+#include <cstdio>
+#include <stdint.h> 
+#include <stdio.h>   
+     
+#ifdef USE_SENSOR_FUSION_LIB     
+#include "osx_motion_fx.h"
+#else
+
+#define NUM_AXES    3
+#define QNUM_AXES   4
+     
+typedef struct
+{
+  float mag[NUM_AXES];  /* calibrated mag [uT]/50 */
+  float acc[NUM_AXES];  /* acc [g] */
+  float gyro[NUM_AXES]; /* gyro [dps] */
+} osxMFX_input;
+
+typedef struct
+{
+  float rotation_9X[NUM_AXES];            /* 9 axes yaw, pitch and roll */
+  float quaternion_9X[QNUM_AXES];         /* 9 axes quaternion */
+  float gravity_9X[NUM_AXES];             /* 9 axes device frame gravity */
+  float linear_acceleration_9X[NUM_AXES]; /* 9 axes device frame linear acceleration */
+  float heading_9X;                       /* 9 axes heading */
+  float rotation_6X[NUM_AXES];            /* 6 axes yaw, pitch and roll */
+  float quaternion_6X[QNUM_AXES];         /* 6 axes quaternion */
+  float gravity_6X[NUM_AXES];             /* 6 axes device frame gravity */
+  float linear_acceleration_6X[NUM_AXES]; /* 6 axes device frame linear acceleration */
+  float heading_6X;                       /* 6 axes heading */
+} osxMFX_output;
+
+typedef struct
+{
+  signed short magOffX; /* X axis Offset */
+  signed short magOffY; /* Y axis Offset */
+  signed short magOffZ; /* Z axis Offset */
+  float magGainX;       /* X axis Gain  */
+  float magGainY;       /* Y axis Gain  */
+  float magGainZ;       /* Z axis Gain  */
+  float expMagVect;     /* expected magnetic field */
+} osxMFX_calibFactor;    
+     
+unsigned char osx_MotionFX_compass_isCalibrated(void);
+void osx_MotionFX_compass_forceReCalibration(void);
+void osx_MotionFX_compass_saveAcc(int acc_x, int acc_y, int acc_z);
+void osx_MotionFX_compass_saveMag(int mag_x, int mag_y, int mag_z);
+int  osx_MotionFX_compass_run(void);
+void osx_MotionFX_setCalibrationData(osxMFX_calibFactor* CalibrationData);
+void osx_MotionFX_getCalibrationData(osxMFX_calibFactor* CalibrationData);
+
+#endif
+     
+     
+typedef struct {
+    int32_t AXIS_X;
+    int32_t AXIS_Y;
+    int32_t AXIS_Z;
+} AxesRaw_t;
+     
+/* Exported functions ------------------------------------------------------- */
+
+bool MotionFX_manager_init(bool DS3_OnBoard, osxMFX_calibFactor & magOffset);
+void MotionFX_manager_run(AxesRaw_t *ACC_Value, AxesRaw_t *GYR_Value, AxesRaw_t *MAG_Value, osxMFX_calibFactor & magOffset);
+void MotionFX_manager_start_6X(void);
+void MotionFX_manager_stop_6X(void);
+void MotionFX_manager_start_9X(void);
+void MotionFX_manager_stop_9X(void);
+
+void MotionFX_manager_setDependencies(uint8_t sf, int32_t sensorMask);
+void MotionFX_manager_resetDependencies(uint8_t sf, int32_t sensorMask);
+uint32_t MotionFX_manager_getDependencies(uint8_t sf);
+
+osxMFX_output* MotionFX_manager_getDataOUT(void);
+osxMFX_input* MotionFX_manager_getDataIN(void);
+
+/* Exported constants --------------------------------------------------------*/
+
+/* Delta time mSec for Deltafusion */
+#define DELTATIMESENSORFUSION 0.01
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //_MOTIONFX_MANAGER_H_
+
+/******************* (C) COPYRIGHT 2015 STMicroelectronics *****END OF FILE****/

--- a/X_NUCLEO_6180XA1.lib	Fri Jan 22 14:28:13 2016 +0000
+++ b/X_NUCLEO_6180XA1.lib	Tue Jan 26 13:29:53 2016 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/ST-Expansion-SW-Team/code/X_NUCLEO_6180XA1/#18abb7f7dcb2
+http://developer.mbed.org/teams/ST-Expansion-SW-Team/code/X_NUCLEO_6180XA1/#3cce4ba37695

--- a/X_NUCLEO_IHM01A1.lib	Fri Jan 22 14:28:13 2016 +0000
+++ b/X_NUCLEO_IHM01A1.lib	Tue Jan 26 13:29:53 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/ST-Expansion-SW-Team/code/X_NUCLEO_IHM01A1/#ba2728afe4c2
+https://developer.mbed.org/teams/ST-Expansion-SW-Team/code/X_NUCLEO_IHM01A1/#83138e702683

--- a/X_NUCLEO_IKS01A1.lib	Fri Jan 22 14:28:13 2016 +0000
+++ b/X_NUCLEO_IKS01A1.lib	Tue Jan 26 13:29:53 2016 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/ST/code/X_NUCLEO_IKS01A1/#a91987e8cf51
+http://developer.mbed.org/teams/ST/code/X_NUCLEO_IKS01A1/#8a5184f66cd9

--- a/main.cpp	Fri Jan 22 14:28:13 2016 +0000
+++ b/main.cpp	Tue Jan 26 13:29:53 2016 +0000
@@ -2,14 +2,14 @@
  ******************************************************************************
  * @file    main.cpp
  * @author  Fabio Brembilla
- * @version V1.0.0
- * @date    December 1st, 2015
+ * @version V2.0.0
+ * @date    January 22th, 2016
  * @brief   SunTracker + RemoteControl Vertical Application
  *          This application use IHM01A1, 6180XA1, IKS01A1, IDB0XA1 expansion boards
  ******************************************************************************
  * @attention
- *  
- * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+ *
+ * <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:
@@ -35,12 +35,12 @@
  *
  ******************************************************************************
  */
- 
+
 /* Includes ------------------------------------------------------------------*/
- 
+
 /* mbed specific header files. */
 #include "mbed.h"
- 
+
 /* Helper header files. */
 #include "DevSPI.h"
 #include "DevI2C.h"
@@ -56,6 +56,12 @@
 #include <stdio.h>
 #include <assert.h>
 
+/* Calibration files. */
+#include "MotionFX_Manager.h" // Need for osxMFX_calibFactor
+
+/* BlueTooth Custom Service files. */
+#include "CustomSunTrackerService.h"
+
 /* Definitions ---------------------------------------------------------------*/
 
 #define SET_ACC 400     // Set Motor Acceleration
@@ -100,29 +106,35 @@
 /* Instance mems IKS01A1. */
 static X_NUCLEO_IKS01A1 *mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
 MotionSensor *accelerometer = mems->GetAccelerometer();
-   
-void DISP_ExecLoopBody(void){};
+
+void DISP_ExecLoopBody(void) {};
 
 AnalogIn analog_read_A1(A1);
 
-InterruptIn mybutton(USER_BUTTON); 
+InterruptIn mybutton(USER_BUTTON);
 
 /* User_Button_Pressed -------------------------------------------------------*/
 
 void User_Button_Pressed()
 {
-  
-    if (start>0) { Display++; }
-    if (Display>2) { Display=0; } 
-    if (start==0) { start=1; }
-    
+
+    if (start>0) {
+        Display++;
+    }
+    if (Display>2) {
+        Display=0;
+    }
+    if (start==0) {
+        start=1;
+    }
+
 }
 
 /* Initialization ------------------------------------------------------------*/
 
 bool Initialization(void)
 {
-  
+
     /* Initializing Babybear Component. */
     status=board->InitBoard();
     if(status)
@@ -131,24 +143,24 @@
     // 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_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);    
+    status_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
     status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
-      
+
     /* Initializing Motor Control Component. */
     motor = new L6474(D2, D8, D7, D9, D10, dev_spi);
     if (motor->Init(NULL) != COMPONENT_OK)
         return false;
- 
+
     motor->SetStepMode(STEP_MODE_1_8);  // Default is STEP_MODE_1_16
- 
+
     /* Set defaults Motor Speed. */
     motor->SetAcceleration(SET_ACC);
     motor->SetDeceleration(SET_DEC);
     motor->SetMaxSpeed(SET_MAX);        // Variable by Light/Mems Sensors
     motor->SetMinSpeed(SET_MIN);
-  
+
     return true;
-    
+
 }
 
 /* Measure_Babybear ----------------------------------------------------------*/
@@ -163,9 +175,15 @@
 
     diff = abs(babybear);
 
-    if (babybear>0) { left=0; right=1; }
-    if (babybear<0) { left=1; right=0; }
- 
+    if (babybear>0) {
+        left=0;
+        right=1;
+    }
+    if (babybear<0) {
+        left=1;
+        right=0;
+    }
+
 }
 
 /* Measure_Accelerometer -----------------------------------------------------*/
@@ -177,62 +195,97 @@
 
     diff = abs(acc_data[0]);
 
-    if (acc_data[0]>0) { left=0; right=1; }
-    if (acc_data[0]<0) { left=1; right=0; }
-    
+    if (acc_data[0]>0) {
+        left=0;
+        right=1;
+    }
+    if (acc_data[0]<0) {
+        left=1;
+        right=0;
+    }
+
 }
 
 
 /* Control_Motor -------------------------------------------------------------*/
 
 void Control_Motor(void)
-{  
-        
-    //printf("Diff: %d lux/mems\n\r", diff);   
-    motor->SetMaxSpeed(diff);        
+{
 
-    if (diff>TOLLERANCE)
-    {
+    //printf("Diff: %d lux/mems\n\r", diff);
+    motor->SetMaxSpeed(diff);
+
+    if (diff>TOLLERANCE) {
         if (diff <=RANGE_1) {
-            if (left)  { strcpy(DisplayStr,"E___"); }
-            if (right) { strcpy(DisplayStr,"___3"); }
-        }    
-        else if (diff >RANGE_1 & diff <=RANGE_2) {
-            if (left)  { strcpy(DisplayStr,"E==="); }
-            if (right) { strcpy(DisplayStr,"===3"); }
-        } 
-        else if (diff >RANGE_2) {
-            if (left)  { strcpy(DisplayStr,"E~~~"); }
-            if (right) { strcpy(DisplayStr,"~~~3"); }
-        }   
-      
+            if (left)  {
+                strcpy(DisplayStr,"E___");
+            }
+            if (right) {
+                strcpy(DisplayStr,"___3");
+            }
+        } else if (diff >RANGE_1 & diff <=RANGE_2) {
+            if (left)  {
+                strcpy(DisplayStr,"E===");
+            }
+            if (right) {
+                strcpy(DisplayStr,"===3");
+            }
+        } else if (diff >RANGE_2) {
+            if (left)  {
+                strcpy(DisplayStr,"E~~~");
+            }
+            if (right) {
+                strcpy(DisplayStr,"~~~3");
+            }
+        }
+
         // In Case of Change Direction
-        if (left & dir==2) { changedir=1; }
-        if (right & dir==1) { changedir=1; }
-        
+        if (left & dir==2) {
+            changedir=1;
+        }
+        if (right & dir==1) {
+            changedir=1;
+        }
+
         // Run only if Stop or Change Direction
         if (diff>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; }
-        } 
+            if (left) {
+                motor->Run(StepperMotor::FWD);
+                dir=1;
+                changedir=0;
+            }
+            if (right) {
+                motor->Run(StepperMotor::BWD);
+                dir=2;
+                changedir=0;
+            }
+        }
     }
-    
+
     // Get Motor Position and Control Rotation Block
     pos = motor->GetPosition();
     if (pos>STOP | pos<-STOP) {
-        if (pos>0) { motor->GoTo(STOP); }
-        if (pos<0) { motor->GoTo(-STOP); }
+        if (pos>0) {
+            motor->GoTo(STOP);
+        }
+        if (pos<0) {
+            motor->GoTo(-STOP);
+        }
     }
-    
+
     // Stop Motor
-    if (diff<=TOLLERANCE) {  
+    if (diff<=TOLLERANCE) {
         motor->HardStop();
-        if (Display==0) { strcpy(DisplayStr,"----"); }
-        if (Display==2) { strcpy(DisplayStr,"E  3"); }  
+        if (Display==0) {
+            strcpy(DisplayStr,"----");
+        }
+        if (Display==2) {
+            strcpy(DisplayStr,"E  3");
+        }
         dir=0;
         changedir=0;
     }
-  
+
 }
 
 /* Measure_SolarPanel --------------------------------------------------------*/
@@ -245,48 +298,52 @@
     //printf("Measure = %.0f mV\r\n", measure);
     //board->display->DisplayDigit("A", 0);
 
-    if (Display==1) { sprintf(DisplayStr, "%.0f", measure); }  
-    
-    board->display->DisplayString(DisplayStr, 4); 
+    if (Display==1) {
+        sprintf(DisplayStr, "%.0f", measure);
+    }
+
+    board->display->DisplayString(DisplayStr, 4);
 
 }
 
 /* Main ----------------------------------------------------------------------*/
 
 int main()
-{ 
-  
-    Initialization();    
-    
+{
+
+    Initialization();
+
     mybutton.fall(&User_Button_Pressed);
-    
+
     /* Printing to the console. */
     printf("SunTracker by Fabio Brembilla\r\n\n");
- 
+
     /* Set Babybears. */
     status_l=board->sensor_left->AlsSetAnalogueGain(3);
     status_r=board->sensor_right->AlsSetAnalogueGain(3);
     status_l=board->sensor_left->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
     status_r=board->sensor_right->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
-    
+
     /* Loop until push User Button to Set 0 Point. */
     strcpy(DisplayStr,"pusH");
-    while(start<1)
-    {
+    while(start<1) {
         board->display->DisplayString(DisplayStr, 4);
     }
-    
+
     /* Main Loop. */
-    while(true)
-    {
-        if (Display==0 | Display==1)    { Measure_Babybear(); }   
-        if (Display==2)                 { Measure_Accelerometer(); }   
-       
+    while(true) {
+        if (Display==0 | Display==1)    {
+            Measure_Babybear();
+        }
+        if (Display==2)                 {
+            Measure_Accelerometer();
+        }
+
         Control_Motor();
         Measure_SolarPanel();
     }
-    
+
     status_l=board->sensor_left->StopMeasurement(als_continuous_polling);
     status_r=board->sensor_right->StopMeasurement(als_continuous_polling);
-    
+
 }

--- a/mbed.bld	Fri Jan 22 14:28:13 2016 +0000
+++ b/mbed.bld	Tue Jan 26 13:29:53 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/165afa46840b
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/6f327212ef96
\ No newline at end of file