Revision 6:4cbf7303b496, committed 2016-02-08

Comitter:

fabiombed

Date:

Mon Feb 08 11:12:07 2016 +0000

Parent:

5:76fb6b783487

Child:

7:54984d031243

Commit message:

Work all except FlightSense with BLE

Changed in this revision

--- a/CustomControlService.h	Wed Feb 03 11:22:17 2016 +0000
+++ b/CustomControlService.h	Mon Feb 08 11:12:07 2016 +0000
@@ -2,7 +2,7 @@
   * @file    CustomControlService.h
   * @author  Fabio Brembilla
   * @version V1.0.0
-  * @date    January 22th, 2016
+  * @date    January, 2016
   * @brief   SunTracker Custom Service for BlueTooth (IDB0XA1 expansion board)
   *****************************************************************************
   * @attention

--- a/CustomSensorsService.h	Wed Feb 03 11:22:17 2016 +0000
+++ b/CustomSensorsService.h	Mon Feb 08 11:12:07 2016 +0000
@@ -2,7 +2,7 @@
   * @file    CustomSensorsService.h
   * @author  Fabio Brembilla
   * @version V1.0.0
-  * @date    January 22th, 2016
+  * @date    January, 2016
   * @brief   SunTracker Custom Service for BlueTooth (IDB0XA1 expansion board)
   *****************************************************************************
   * @attention

--- a/MotionFX_Manager.h	Wed Feb 03 11:22:17 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,140 +0,0 @@
-/**
-  ******************************************************************************
-  * @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/main.cpp	Wed Feb 03 11:22:17 2016 +0000
+++ b/main.cpp	Mon Feb 08 11:12:07 2016 +0000
@@ -3,7 +3,7 @@
  * @file    main.cpp
  * @author  Fabio Brembilla
  * @version V2.0.0
- * @date    January 22th, 2016
+ * @date    February, 2016
  * @brief   SunTracker + RemoteControl Vertical Application
  *          This application use IHM01A1, 6180XA1, IKS01A1, IDB0XA1 expansion boards
  ******************************************************************************
@@ -38,27 +38,24 @@
 
 /* Includes ------------------------------------------------------------------*/
 
-/* mbed specific header files. */
+// Mbed specific header files
 #include "mbed.h"
 
-/* Helper header files. */
+// Helper header files
 #include "DevSPI.h"
 #include "DevI2C.h"
 
-/* Component specific header files. */
+// Component specific header files
 #include "l6474_class.h"
 #include "x_nucleo_6180xa1.h"
 #include "x_nucleo_iks01a1.h"
 
-/* C header files. */
+// C header files
 #include <string.h>
 #include <stdlib.h>
 #include <stdio.h>
 #include <assert.h>
 
-/* Calibration files. */
-#include "MotionFX_Manager.h" // Need for osxMFX_calibFactor
-
 /* BlueTooth -----------------------------------------------------------------*/
 
 #include "debug.h" // Need for PRINTF
@@ -82,10 +79,11 @@
 #include "CustomControlService.h"
 #include "CustomSensorsService.h"
 
-static BLE * p_BLEdev = NULL;
+static BLE *p_BLEdev = NULL;
 
-#define BLE_DEV_NAME "SunTrack"
+#define BLE_DEV_NAME "SunTracker"
 #define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF
+#define BLE_ADVERTISING_INTERVAL 1000
 
 /* Definitions ---------------------------------------------------------------*/
 
@@ -115,26 +113,30 @@
 
 /* ---------------------------------------------------------------------------*/
 
-/* Motor Control Component. */
-L6474 *motor;
+// Motor Control Component
+static L6474 *motor;
 
-/* Initializing SPI bus. */
+// Initializing SPI bus
 DevSPI dev_spi(D11, D12, D13);
 
-/* Initializing I2C bus. */
+// Initializing I2C bus
 DevI2C dev_i2c(D14, D15);
 
-/* Instance board 6180XA1. */
-static X_NUCLEO_6180XA1 *board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC);
+// Instance board 6180XA1
+//static X_NUCLEO_6180XA1 *board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC);
+//MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right;
+static X_NUCLEO_6180XA1 *board;
 MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right;
-
-/* Instance mems IKS01A1. */
-static X_NUCLEO_IKS01A1 *mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
-MotionSensor *accelerometer = mems->GetAccelerometer();
+    
+// Instance mems IKS01A1
+//static X_NUCLEO_IKS01A1 *mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
+//MotionSensor *accelerometer = mems->GetAccelerometer();
+static X_NUCLEO_IKS01A1 *mems;
+MotionSensor *accelerometer;
 
 void DISP_ExecLoopBody(void) {};
 
-AnalogIn analog_read_A1(A1);
+//AnalogIn analog_read(A1); // A1 Conflict with BLE SPI_CS --> Changed in A????
 
 InterruptIn mybutton(USER_BUTTON);
 
@@ -152,6 +154,8 @@
     if (start==0) {
         start=1;
     }
+    
+    printf("PUSH Display %d Start %d\r\n", Display, start);
 
 }
 
@@ -159,26 +163,43 @@
 
 bool Initialization(void)
 {
+   
+    // Initializing MEMS Component
+    mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
+    accelerometer = mems->GetAccelerometer();
+   
+//----
 
-    /* Initializing Babybear Component. */
+    // Initializing Babybear Component
+    board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC);
+
+/* FABIO
     status=board->InitBoard();
-    if(status)
-        VL6180x_ErrLog("Failed to init the board!\n\r");
+    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_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
-    status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
+    status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);    
+    
+    // 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);
 
-    /* Initializing Motor Control Component. */
+//----
+
+    // Initializing Motor 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. */
+    // Set defaults Motor Speed
     motor->SetAcceleration(SET_ACC);
     motor->SetDeceleration(SET_DEC);
     motor->SetMaxSpeed(SET_MAX);        // Variable by Light/Mems Sensors
@@ -240,74 +261,49 @@
     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 (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"); }
         }
 
         // 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); }
+        printf("GOTO\n\r"); // Without this command, the motor remain in stop
     }
 
     // Stop Motor
-    if (diff<=TOLLERANCE) {
+    if (diff<=TOLLERANCE) { // It continues to send the command to stop the motor. Think to do it just one time
         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;
+        //printf("STOP\n\r");
     }
 
 }
@@ -317,8 +313,9 @@
 void Measure_SolarPanel(void)
 {
 
-    // AnalogIn A1: 0V return 0.0 , 3.3V return 1.0
-    float measure = analog_read_A1.read() * 3300;
+    // AnalogIn: 0V return 0.0 , 3.3V return 1.0
+//    float measure = analog_read.read() * 3300;
+    float measure = 0;
     //printf("Measure = %.0f mV\r\n", measure);
     //board->display->DisplayDigit("A", 0);
 
@@ -327,6 +324,7 @@
     }
 
     board->display->DisplayString(DisplayStr, 4);
+    printf("%s\n\r", DisplayStr);
 
 }
 
@@ -369,12 +367,14 @@
 
     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__);    
+    p_BLEdev->gap().setAddress(BLEProtocol::AddressType::PUBLIC, BLE_address_BE);  
+
+    printf("BLE Init (Line %d)\r\n", __LINE__);
+
+    p_BLEdev->init();
+    
+    printf("BLE Init OK (Line %d)\r\n", __LINE__);
 
     // Set BLE CallBack Functions
     p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
@@ -387,6 +387,21 @@
     //p_BLEdev->gattServer().onDataSent(onDataSentCallback);    
     //p_BLEdev->gap().onTimeout(onTimeoutCallback);
 
+    // Setup BLE Advertising
+    const static char DEVICE_NAME[] = BLE_DEV_NAME;
+    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+    #ifdef USE_SENSOR_FUSION_LIB        
+    uint8_t dat[] = {0x01,0x80,0x00,0xFC,0x01,0x80};
+    #else
+    uint8_t dat[] = {0x01,0x80,0x00,0xFC,0x00,0x00};
+    #endif      
+    p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);    
+    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
+    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
+    p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
+    p_BLEdev->gap().startAdvertising();
+
 }
 
 /* Main ----------------------------------------------------------------------*/
@@ -394,28 +409,33 @@
 int main()
 {
 
+    // Printing to the console
+    printf("SunTracker by Fabio Brembilla\r\n\n");
+
     Initialization();
 
-    //BLE_Initialization();
-
+    printf("Initialization OK (Line %d)\r\n", __LINE__);    
+    
+    BLE_Initialization();
+        
+    printf("BLE_Initialization OK (Line %d)\r\n", __LINE__);
+        
     mybutton.fall(&User_Button_Pressed);
-
-    /* Printing to the console. */
-    printf("SunTracker by Fabio Brembilla\r\n\n");
+    
+    printf("Main Initializations OK (Line %d)\r\n", __LINE__);
+    printf("Wait Push Button\r\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. */
+    // Loop until push User Button to Set 0 Point
     strcpy(DisplayStr,"pusH");
     while(start<1) {
         board->display->DisplayString(DisplayStr, 4);
+        printf("%s\n\r", DisplayStr);
+        //wait(0.1); // Need one command otherwise remain always in loop
     }
 
-    /* Main Loop. */
+    printf("Start Main Loop\r\n");
+
+    // Main Loop
     while(true) {
         if (Display==0 | Display==1)    {
             Measure_Babybear();
@@ -426,9 +446,11 @@
 
         Control_Motor();
         Measure_SolarPanel();
+        
+        p_BLEdev->waitForEvent();
     }
 
-    status_l=board->sensor_left->StopMeasurement(als_continuous_polling);
-    status_r=board->sensor_right->StopMeasurement(als_continuous_polling);
+    //status_l=board->sensor_left->StopMeasurement(als_continuous_polling);
+    //status_r=board->sensor_right->StopMeasurement(als_continuous_polling);
 
 }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_only_BLE.cpp.h	Mon Feb 08 11:12:07 2016 +0000
@@ -0,0 +1,215 @@
+/**
+ ******************************************************************************
+ * @file    main.cpp
+ * @author  Fabio Brembilla
+ * @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) 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.
+ *
+ ******************************************************************************
+ */
+
+/* Includes ------------------------------------------------------------------*/
+
+/* mbed specific header files. */
+#include "mbed.h"
+
+/* Helper header files. */
+#include "DevSPI.h"
+#include "DevI2C.h"
+
+/* Component specific header files. */
+#include "l6474_class.h"
+#include "x_nucleo_6180xa1.h"
+#include "x_nucleo_iks01a1.h"
+
+/* C header files. */
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.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
+#define BLE_ADVERTISING_INTERVAL 1000
+
+/* Definitions ---------------------------------------------------------------*/
+
+#define SET_ACC 400     // Set Motor Acceleration
+#define SET_DEC 400     // Set Motor Deceleration
+#define SET_MAX 200     // Set Motor MaxSpeed
+#define SET_MIN 100     // Set Motor MinSpeed
+#define STOP 1000       // Set Motor Stop Position
+#define TOLLERANCE 100  // Tollerance between Left and Right before Start Movement
+#define RANGE_1 200     // Range 1 for Motor Speed
+#define RANGE_2 500     // Range 2 for Motor Speed
+
+/* 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 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
+int16_t status, status_t, status_b, status_l, status_r; // Babybear Status
+
+/* ---------------------------------------------------------------------------*/
+
+
+
+/* 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()
+{
+
+    //Initialization();
+printf("Line: %d \r\n", __LINE__);  
+    BLE_Initialization();
+printf("Line: %d \r\n", __LINE__);  
+    
+    /* Setup advertising. */
+    const static char     DEVICE_NAME[] = BLE_DEV_NAME;
+    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+    #ifdef USE_SENSOR_FUSION_LIB        
+    uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x01,0x80};
+    #else
+    uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x00,0x00};
+    #endif      
+    p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);    
+    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
+    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
+    p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
+    p_BLEdev->gap().startAdvertising();
+
+    /* Printing to the console. */
+    printf("SunTracker by Fabio Brembilla\r\n\n");
+    
+    /* Main Loop. */
+    while(true) {
+        
+       
+        
+        
+        p_BLEdev->waitForEvent();
+    }
+
+
+}