main.cpp

00001 /**
00002   ******************************************************************************
00003   * @file    main.cpp
00004   * @author  Central Labs / AST
00005   * @version V0.9.0
00006   * @date    23-Dec-2015
00007   * @brief   Main program body
00008   ******************************************************************************
00009   * @attention
00010   *
00011   * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
00012   *
00013   * Redistribution and use in source and binary forms, with or without modification,
00014   * are permitted provided that the following conditions are met:
00015   *   1. Redistributions of source code must retain the above copyright notice,
00016   *      this list of conditions and the following disclaimer.
00017   *   2. Redistributions in binary form must reproduce the above copyright notice,
00018   *      this list of conditions and the following disclaimer in the documentation
00019   *      and/or other materials provided with the distribution.
00020   *   3. Neither the name of STMicroelectronics nor the names of its contributors
00021   *      may be used to endorse or promote products derived from this software
00022   *      without specific prior written permission.
00023   *
00024   * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00025   * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00026   * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027   * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00028   * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00029   * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00030   * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031   * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00032   * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00033   * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034   *
00035   ******************************************************************************
00036   */
00037 
00038 #include "mbed.h"
00039 #include "main.h"   // contains the condt compiling configuration
00040 #include <cmath>
00041 #include <math.h>
00042 #include "Gap.h"
00043 #include "debug.h"
00044 #include "btle.h"
00045 #include "blecommon.h"
00046 #include "BLE.h"
00047 #include "UUID.h"
00048 #include "MotionFX_Manager.h"
00049 #include "InterruptManager.h"
00050 #include "DevI2C.h"
00051 #include "BlueNRGGattServer.h"
00052 #include "GattCharacteristic.h"
00053 #include "LPS22HBSensor.h"
00054 #include "LSM6DSLSensor.h"
00055 #include "LSM303AGRMagSensor.h"
00056 #include "LSM303AGRAccSensor.h"
00057 
00058 
00059 typedef struct {
00060     int32_t AXIS_X;
00061     int32_t AXIS_Y;
00062     int32_t AXIS_Z;
00063 } AxesRaw_TypeDef;
00064 
00065 #ifdef CUST_CONS_SERV
00066    #include "CustomConsoleService.h"
00067 #endif
00068 
00069 #ifdef CUST_BATT_SERV
00070    #include "CustomBatteryService.h"
00071 #endif
00072 
00073 
00074 #ifdef CUST_SENS_SERV
00075    #include "CustomSensorsService.h"
00076    #include "HTS221Sensor.h"
00077    #if defined (TARGET_NUCLEO_L476RG) && defined (MINI_CRADLE)
00078        static DevI2C sens_intf_I2C (PC_1, PC_0);
00079    #else    
00080        #if defined (TARGET_NUCLEO_F401RE)
00081            static DevI2C sens_intf_I2C(I2C_SDA, I2C_SCL); 
00082        #endif        
00083        #if defined (TARGET_DISCO_L475VG_IOT01A)
00084            static DevI2C sens_intf_I2C(PB_11,PB_10);
00085        #endif       
00086    #endif
00087 #endif
00088 
00089 #if defined (TARGET_NUCLEO_L476RG) && defined (MINI_CRADLE)
00090    static HTS221Sensor ht_sensor (&sens_intf_I2C); // sensor on the mini cradle only (not on the tile)       
00091 #endif   
00092 #if defined (TARGET_DISCO_L475VG_IOT01A) 
00093    #include "lis3mdl_class.h"
00094    static HTS221Sensor ht_sensor (&sens_intf_I2C); // sensor on the DISCO board
00095    static LPS22HBSensor pt_sensor(&sens_intf_I2C); 
00096    static LSM6DSLSensor acc_gyro(&sens_intf_I2C, LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW,PD_11);
00097    static LIS3MDL magnetometer(&sens_intf_I2C);  
00098 #endif
00099 
00100 
00101 #if defined (TARGET_NUCLEO_F401RE)
00102    static HTS221Sensor ht_sensor (&sens_intf_I2C); // sensor on the IKS01A2
00103    static LPS22HBSensor pt_sensor(&sens_intf_I2C);  
00104    static LSM6DSLSensor acc_gyro(&sens_intf_I2C); 
00105    static LSM303AGRMagSensor magnetometer(&sens_intf_I2C); //on SensorTile L476JG                 
00106    static LSM303AGRAccSensor accelerometer(&sens_intf_I2C); //on SensorTile L476JG  
00107 #else 
00108     #if defined (TARGET_NUCLEO_L476RG) && defined (SENSOR_TILE)
00109       #define SPI_TYPE_LPS22HB   LPS22HBSensor::SPI3W   
00110       #define SPI_TYPE_LSM6DSL   LSM6DSLSensor::SPI3W      
00111       SPI sens_intf(PB_15, NC, PB_13); // 3W mosi, sclk on Nucleo L476 same as BTLE
00112       static LPS22HBSensor pt_sensor(&sens_intf, PA_3, NC, SPI_TYPE_LPS22HB); 
00113       static LSM6DSLSensor acc_gyro(&sens_intf,PB_12, NC, PA_2, SPI_TYPE_LSM6DSL); 
00114       static LSM303AGRMagSensor magnetometer(&sens_intf, PB_1 ); //on SensorTile L476JG               
00115       static LSM303AGRAccSensor accelerometer(&sens_intf, PC_4 ); //on SensorTile L476JG    
00116     #endif
00117 #endif
00118 
00119 static unsigned char    isCal = 0;
00120 #ifdef CUST_CONFIG_SERV
00121    #include "CustomConfigService.h"
00122    static bool             ForceReCalibration =0;
00123 #endif
00124 
00125 #ifdef CUST_SW_SERV
00126    #include "CustomSoftwareService.h"
00127 #endif
00128 
00129 static bool             isBlueButtonSequence = false;
00130 static                  Ticker BlueButtonTimer;
00131 #if defined (TARGET_DISCO_L475VG_IOT01A) || defined (TARGET_NUCLEO_F401RE)
00132    static DigitalOut       led(LED1);
00133 #endif 
00134 #if defined (TARGET_NUCLEO_L476RG) && defined (SENSOR_TILE)
00135    static DigitalOut       led((PinName)0x6C);   // PG_12 pin on L476JG (PG_12 not yet listed)
00136 #endif
00137     
00138 static uint32_t         TimeStamp=0;
00139 static int              BLEConnectionStatus =0;
00140 static int              HowManyButtonPress =0;
00141 static BLE              * p_BLEdev = NULL;
00142 static bool             SensorFusionOK = false;
00143 static int              ble_started=0;
00144 
00145 static Queue <char, 8> QueueMainActivity;
00146 static EventQueue eventQueue(/* event count */ 16 * /* event size */ 32);
00147 
00148 #define BLE_STARTED       5
00149     
00150 #ifdef CUST_CONS_SERV
00151    static CustomConsoleService    * p_customconsoleservice   = NULL;
00152 #endif
00153 
00154 #ifdef CUST_SENS_SERV
00155    static CustomSensorService     * p_customsensorservice    = NULL;
00156 #endif  
00157 
00158 #ifdef CUST_SW_SERV
00159    static CustomSoftwareService   * p_customsoftwareservice  = NULL;
00160    static MFX_input_t               MotionFX_Engine_In;
00161    static MFX_input_t             * p_MotionFX_Engine_In = &MotionFX_Engine_In; 
00162    static MFX_output_t              MotionFX_Engine_Out;
00163    static MFX_output_t            * p_MotionFX_Engine_Out = &MotionFX_Engine_Out;
00164 #endif
00165 static osxMFX_calibFactor        magOffset;
00166 
00167 
00168 #ifdef CUST_CONFIG_SERV
00169    static CustomConfigService     * p_customconfigservice    = NULL;
00170 #endif
00171 
00172 
00173 /***************************    Calibration functions    **************************/
00174 
00175 /**
00176  * @brief  Check if there are a valid Calibration Values in Memory and read them
00177  * @param None
00178  * @retval unsigned char Success/Not Success
00179  */
00180 static unsigned char ReCallCalibrationFromMemory(void)
00181 #ifdef BLUEMSYS_STORE_CALIB_FLASH
00182 {
00183     /* ReLoad the Calibration Values from FLASH */
00184     unsigned char Success=1;
00185     uint32_t Address = BLUEMSYS_FLASH_ADD;
00186     __IO uint32_t data32 = *(__IO uint32_t*) Address;
00187     if(data32== BLUEMSYS_CHECK_CALIBRATION) {
00188         int32_t ReadIndex;
00189         float ReadmagOffset[7];
00190 
00191         for(ReadIndex=0; ReadIndex<7; ReadIndex++) {
00192             Address +=4;
00193             data32 = *(__IO uint32_t*) Address;
00194             ReadmagOffset[ReadIndex]=data32;
00195         }
00196         isCal=1;
00197         magOffset.magOffX    = (signed short) ReadmagOffset[0];
00198         magOffset.magOffY    = (signed short) ReadmagOffset[1];
00199         magOffset.magOffZ    = (signed short) ReadmagOffset[2];
00200 #ifdef BLUEMSYS_DEBUG_CALIB
00201         /* Debug */
00202         printf("magOffX   ->%d\r\n",magOffset.magOffX);
00203         printf("magOffY   ->%d\r\n",magOffset.magOffY);
00204         printf("magOffZ   ->%d\r\n",magOffset.magOffZ);
00205 #endif /* BLUEMSYS_DEBUG_CALIB */
00206         printf("Calibration Read from Flash\r\n");
00207     } else {
00208         printf("Calibration Not present in FLASH\r\n");
00209         magOffset.magOffX    = 0;
00210         magOffset.magOffY    = 0;   
00211         magOffset.magOffZ    = 0;               
00212         isCal=0;
00213     }
00214     return Success;
00215 }
00216 #else /* BLUEMSYS_STORE_CALIB_FLASH */
00217 {
00218     unsigned char Success= 1;           
00219     magOffset.magOffX    = 0;
00220     magOffset.magOffY    = 0;   
00221     magOffset.magOffZ    = 0;   
00222       isCal=0;
00223     return Success;
00224 }
00225 #endif /* BLUEMSYS_STORE_CALIB_FLASH */
00226 
00227 static unsigned char ResetCalibrationInMemory(void)
00228 #ifdef BLUEMSYS_STORE_CALIB_FLASH
00229 {
00230     /* Reset Calibration Values in FLASH */
00231     unsigned char Success=1;
00232 
00233     /* Erase First Flash sector */
00234     FLASH_EraseInitTypeDef EraseInitStruct;
00235     uint32_t SectorError = 0;
00236     EraseInitStruct.TypeErase = TYPEERASE_SECTORS;
00237     EraseInitStruct.VoltageRange = VOLTAGE_RANGE_3;
00238     EraseInitStruct.Sector = BLUEMSYS_FLASH_SECTOR;
00239     EraseInitStruct.NbSectors = 1;
00240 
00241     /* Unlock the Flash to enable the flash control register access *************/
00242     HAL_FLASH_Unlock();
00243 
00244     if (HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError) != HAL_OK) {
00245         /*
00246           Error occurred while sector erase.
00247           User can add here some code to deal with this error.
00248           SectorError will contain the faulty sector and then to know the code error on this sector,
00249           user can call function 'HAL_FLASH_GetError()'
00250         */
00251         /*
00252           FLASH_ErrorTypeDef errorcode = HAL_FLASH_GetError();
00253         */
00254 
00255 //   Error_Handler();
00256         Success=0;
00257     } else {
00258         printf("Erase calibration in FLASH Memory\r\n");
00259     }
00260 
00261     /* Lock the Flash to disable the flash control register access (recommended
00262     to protect the FLASH memory against possible unwanted operation) *********/
00263     HAL_FLASH_Lock();   
00264     return Success;
00265 }
00266 #else /* BLUEMSYS_STORE_CALIB_FLASH */
00267 {
00268       unsigned char Success=1;          
00269     return Success;
00270 }
00271 #endif /* BLUEMSYS_STORE_CALIB_FLASH */
00272 
00273 /**
00274  * @brief  Save the Magnetometer Calibration Values to Memory
00275  * @param None
00276  * @retval unsigned char Success/Not Success
00277  */
00278 static unsigned char SaveCalibrationToMemory(void)
00279 {
00280     unsigned char Success=1;
00281 
00282     /* Reset Before The data in Memory */
00283     Success = ResetCalibrationInMemory();
00284 
00285     if(Success)
00286 #ifdef BLUEMSYS_STORE_CALIB_FLASH
00287     {
00288         /* Store in Flash Memory */
00289         uint32_t Address = BLUEMSYS_FLASH_ADD;
00290         uint32_t WritemagOffset[8];
00291         int32_t WriteIndex;
00292         WritemagOffset[0] = BLUEMSYS_CHECK_CALIBRATION;
00293         WritemagOffset[1] = (uint32_t) magOffset.magOffX;
00294         WritemagOffset[2] = (uint32_t) magOffset.magOffY;
00295         WritemagOffset[3] = (uint32_t) magOffset.magOffZ;
00296 
00297         /* Unlock the Flash to enable the flash control register access *************/
00298         HAL_FLASH_Unlock();
00299 
00300         for(WriteIndex=0; WriteIndex<8; WriteIndex++) {
00301             if (HAL_FLASH_Program(TYPEPROGRAM_WORD, Address,WritemagOffset[WriteIndex]) == HAL_OK) {
00302                 Address = Address + 4;
00303             } else {
00304                 /* Error occurred while writing data in Flash memory.
00305                    User can add here some code to deal with this error */
00306                 /*
00307                   FLASH_ErrorTypeDef errorcode = HAL_FLASH_GetError();
00308                 */
00309 //       Error_Handler();
00310             }
00311         }
00312 #ifdef BLUEMSYS_DEBUG_CALIB
00313         /* Debug */
00314         printf("magOffX   ->%d\r\n",magOffset.magOffX);
00315         printf("magOffY   ->%d\r\n",magOffset.magOffY);
00316         printf("magOffZ   ->%d\r\n",magOffset.magOffZ);
00317 #endif /* BLUEMSYS_DEBUG_CALIB */
00318 
00319         printf("New Calibration Values Saved in FLASH\r\n");
00320 
00321         /* Lock the Flash to disable the flash control register access (recommended
00322          to protect the FLASH memory against possible unwanted operation) *********/
00323         HAL_FLASH_Lock();
00324     }
00325 #else /* BLUEMSYS_STORE_CALIB_FLASH */
00326     {
00327 #ifdef BLUEMSYS_DEBUG_CALIB
00328         /* Debug */
00329         printf("magOffX   ->%d\r\n",magOffset.magOffX);
00330         printf("magOffY   ->%d\r\n",magOffset.magOffY);
00331         printf("magOffZ   ->%d\r\n",magOffset.magOffZ);
00332 #endif /* BLUEMSYS_DEBUG_CALIB */
00333     }
00334 #endif /* BLUEMSYS_STORE_CALIB_FLASH */
00335 
00336     return Success;
00337 }
00338 
00339 /***************************    End Calibration functions    **************************/
00340 
00341 static void floatToInt(float in, int32_t *out_int, int32_t *out_dec, int32_t dec_prec)
00342 {
00343     *out_int = (int32_t)in;
00344     in = in - (float)(*out_int);
00345     *out_dec = (int32_t)(float)(int)(in*pow((float)10,(int)dec_prec));
00346 }
00347 
00348 static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
00349 {
00350     PRINTF("onUpdatesEnabledCallback! handle: %x\n\r", handle);
00351 #ifdef CUST_SENS_SERV
00352     if (p_customsensorservice)
00353         p_customsensorservice->enNotify (handle);
00354 #endif
00355 
00356 #ifdef CUST_SW_SERV
00357     if (p_customsoftwareservice)
00358         p_customsoftwareservice->enNotify (handle);
00359 #endif
00360 
00361 #ifdef CUST_CONFIG_SERV
00362     if (p_customconfigservice) {
00363         p_customconfigservice->enNotify (handle);
00364         p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
00365         p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
00366     }
00367 #endif
00368         
00369 #ifdef CUST_CONS_SERV
00370     if (p_customconsoleservice)
00371         p_customconsoleservice->enNotify (handle);
00372 #endif
00373 }
00374 
00375 static void onUpdatesDisabledCallback(Gap::Handle_t handle)
00376 {
00377     PRINTF("onUpdatesDisabledCallback! handle: %x\n\r", handle);
00378 #ifdef CUST_SENS_SERV
00379     if (p_customsensorservice)
00380         p_customsensorservice->disNotify (handle);
00381 #endif
00382 
00383 #ifdef CUST_SW_SERV
00384     if (p_customsoftwareservice)
00385         p_customsoftwareservice->disNotify (handle);
00386 #endif
00387 
00388 #ifdef CUST_CONFIG_SERV
00389     if (p_customconfigservice)
00390         p_customconfigservice->disNotify (handle);
00391 #endif
00392         
00393 #ifdef CUST_CONS_SERV
00394     if (p_customconsoleservice)
00395         p_customconsoleservice->disNotify (handle);
00396 #endif
00397 }
00398 
00399 static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
00400 {
00401     printf("Disconnected\n\r");
00402     printf("Restarting the advertising process\n\r");
00403     TimeStamp =0;
00404     BLEConnectionStatus =0;
00405 #ifdef CUST_SENS_SERV
00406     if (p_customsensorservice)
00407         p_customsensorservice->updateConnectionStatus(DISCONNECTED);
00408 #endif
00409 #ifdef CUST_SW_SERV
00410     if (p_customsoftwareservice)
00411         p_customsoftwareservice->updateConnectionStatus(DISCONNECTED);
00412 #endif
00413 #ifdef CUST_CONFIG_SERV
00414     if (p_customconfigservice)
00415         p_customconfigservice->updateConnectionStatus(DISCONNECTED);
00416 #endif
00417 #ifdef CUST_CONS_SERV
00418     if (p_customconsoleservice)
00419         p_customconsoleservice->updateConnectionStatus(DISCONNECTED);
00420 #endif
00421     if (p_BLEdev)
00422         p_BLEdev->startAdvertising();
00423 }
00424 
00425 static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
00426 {
00427     printf("\rConnected: %x", connectionParams->peerAddr[Gap::ADDR_LEN-1]);
00428     for(int i=Gap::ADDR_LEN-2; i>=0; i--) printf(":%x", connectionParams->peerAddr[i]);
00429     printf("\n\r");
00430             
00431     TimeStamp =0;
00432     BLEConnectionStatus =1;
00433 #ifdef CUST_SENS_SERV
00434     if (p_customsensorservice)
00435         p_customsensorservice->updateConnectionStatus(CONNECTED);
00436 #endif
00437 #ifdef CUST_SW_SERV
00438     if (p_customsoftwareservice)
00439         p_customsoftwareservice->updateConnectionStatus(CONNECTED);
00440 #endif
00441 #ifdef CUST_CONFIG_SERV
00442     if (p_customconfigservice)
00443         p_customconfigservice->updateConnectionStatus(CONNECTED);
00444 #endif
00445 #ifdef CUST_CONS_SERV
00446     if (p_customconsoleservice)
00447         p_customconsoleservice->updateConnectionStatus(CONNECTED);
00448 #endif
00449 }
00450 
00451 static void onDataReadCallback(const GattReadCallbackParams *eventDataP)
00452 {
00453     float temp, hum, pres;
00454     int16_t TempToSend, HumToSend;
00455     uint32_t    PresToSend;
00456     int32_t decPart, intPart;
00457     AxesRaw_TypeDef Magn, Acc, Gyro;
00458     Gap::Handle_t handle = eventDataP->handle - BLE_HANDLE_VALUE_OFFSET;
00459 #ifdef CUST_SENS_SERV
00460 #ifdef DISAGGREGATED_ENV_CHAR
00461     if (p_customsensorservice->isTempHandle(handle)) {
00462         pt_sensor.get_temperature(&temp);
00463         floatToInt(temp, &intPart, &decPart, 1);
00464         TempToSend = intPart*10+decPart;
00465         p_customsensorservice->sendEnvTemperature(TempToSend, TimeStamp);
00466 
00467     } else if (p_customsensorservice->isHumHandle(handle)) {
00468 #if defined (MINI_CRADLE) || defined (TARGET_DISCO_L475VG_IOT01A) || defined (TARGET_NUCLEO_F401RE)
00469         ht_sensor.get_humidity(&hum);
00470 #endif        
00471         floatToInt(hum, &intPart, &decPart, 1);
00472         HumToSend = intPart*10+decPart;
00473         p_customsensorservice->sendEnvHumidity(HumToSend, TimeStamp);
00474 
00475     } else if (p_customsensorservice->isPresHandle(handle)) {
00476         pt_sensor.get_pressure(&pres);
00477         floatToInt(pres, &intPart, &decPart, 1);
00478         PresToSend = intPart*100+decPart;
00479         p_customsensorservice->sendEnvPressure(PresToSend, TimeStamp);
00480 
00481     } else
00482 #endif  
00483 #ifdef DISAGGREGATED_MOT_CHAR      
00484         if (p_customsensorservice->isGyroHandle(handle)) {
00485         acc_gyro.get_g_axes((int32_t *)&Gyro);
00486         p_customsensorservice->sendEnvGyroscope (&Gyro, TimeStamp);
00487 
00488     } else if (p_customsensorservice->isAccHandle(handle)) {
00489         accelerometer.get_x_axes((int32_t *)&Acc);
00490         p_customsensorservice->sendEnvAccelerometer (&Acc, TimeStamp);
00491 
00492     } else if (p_customsensorservice->isMagHandle(handle)) {
00493         magnetometer.get_m_axes((int32_t *)&Magn);
00494         p_customsensorservice->sendEnvMagnetometer(&Magn, TimeStamp, &magOffset);
00495 
00496     } else
00497 #endif
00498         if (p_customsensorservice->isAccGyroMagHandle(handle)) {
00499         p_customsensorservice->sendEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, &magOffset);
00500         
00501     } else if (p_customsensorservice->isPresHumTempHandle(handle)) {
00502         pt_sensor.get_temperature(&temp);
00503         floatToInt(temp, &intPart, &decPart, 1);
00504         TempToSend = intPart*10+decPart;
00505 #if defined (MINI_CRADLE) || defined (TARGET_DISCO_L475VG_IOT01A) || defined (TARGET_NUCLEO_F401RE)
00506         ht_sensor.get_humidity(&hum);
00507 #endif        
00508         floatToInt(hum, &intPart, &decPart, 1);
00509         HumToSend = intPart*10+decPart;
00510         pt_sensor.get_pressure(&pres);
00511         floatToInt(pres, &intPart, &decPart, 1);
00512         PresToSend = intPart*100+decPart;        
00513         p_customsensorservice->sendEnvPresHumTemp (PresToSend, HumToSend, TempToSend, TimeStamp);
00514     }
00515 #endif
00516 
00517 #ifdef CUST_SW_SERV
00518     if (p_customsoftwareservice->isQuatHandle(handle)) {
00519 //          p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp);   // dont need to update because already calculated/update into main loop
00520 
00521     } else if (p_customsoftwareservice->isFloatQuatHandle(handle)) {
00522 //          p_customsoftwareservice->updateFloatQuaternions(QuatFloat, TimeStamp);  // dont need to update because already calculated/update into main loop
00523     }
00524 #endif
00525 
00526 #ifdef CUST_CONFIG_SERV
00527     if (p_customconfigservice->isConfHandle(handle)) {
00528         printf ("onDataReadCallback\n\r");
00529         p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
00530         p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
00531     }
00532 #endif
00533 }
00534 
00535 static void onDataWriteCallback(const GattWriteCallbackParams *eventDataP)
00536 {
00537     Gap::Handle_t handle = eventDataP->handle - BLE_HANDLE_VALUE_OFFSET;
00538     PRINTF("onDataWriteCallback!!\n\r");
00539 
00540 #ifdef CUST_CONFIG_SERV
00541             if (p_customconfigservice->isConfHandle(handle)) {
00542                 /* Received one write command from Client on calibration characteristc */
00543                 uint32_t FeatureMask = (eventDataP->data[3]) | (eventDataP->data[2]<<8) | (eventDataP->data[1]<<16) | (eventDataP->data[0]<<24);
00544                 uint8_t Command = eventDataP->data[4];                
00545                 switch (Command) {
00546                     case W2ST_COMMAND_CAL_STATUS:
00547 
00548                         /* Replay with the calibration status for the feature */
00549                         //  Config_Notify(FeatureMask,Command,isCal);
00550                         printf ("W2ST_COMMAND_CAL_STATUS\n\r");
00551                         p_customconfigservice->sendConfigState(FeatureMask, Command, isCal, TimeStamp);
00552                         break;
00553                     case W2ST_COMMAND_CAL_RESET:
00554                         /* Reset the calibration */
00555                         printf ("W2ST_COMMAND_CAL_RESET\n\r");
00556                         ForceReCalibration=1;
00557                         break;
00558                     case W2ST_COMMAND_CAL_STOP:
00559 
00560                         /* Do nothing in this case */
00561                         break;
00562                     default:
00563 //        if(StdErrNotification){
00564 //          BytesToWrite = sprintf((char *)BufferToWrite, "Calibration UNKNOW Signal For Features=%x\n\r",FeatureMask);
00565 //          Stderr_Update(BufferToWrite,BytesToWrite);
00566 //        } else {
00567 //          printf("Calibration UNKNOW Signal For Features=%x\n\r",FeatureMask);
00568 //        }
00569                         break;
00570                 }
00571 
00572             }
00573 #endif
00574 }
00575 
00576 static void onDataSentCallback(unsigned count)
00577 {
00578     PRINTF("onDataSentCallback!!\n\r");
00579 }
00580 
00581 
00582 static void onTimeoutCallback(Gap::TimeoutSource_t source)
00583 {
00584     PRINTF("onTimeoutCallback!!\n\r");
00585 }
00586 
00587 static void onConfirmationReceivedCallback(uint16_t attributeHandle)
00588 {
00589     PRINTF("onConfirmationReceivedCallback!!\n\r");
00590 }
00591 
00592 
00593 static void Ticker_BlueButton(void)
00594 {
00595 #ifdef CUST_CONS_SERV   
00596 static uint8_t          BufferToWrite[W2ST_CONSOLE_MAX_CHAR_LEN];
00597 static uint8_t          BytesToWrite;   
00598 #endif
00599     
00600         BlueButtonTimer.detach();   
00601 //        printf (" butt time expired \n\r"); 
00602 #ifdef CUST_CONS_SERV
00603     BytesToWrite = sprintf((char *)BufferToWrite, "nr%d push in %1.1fs\r\n",HowManyButtonPress, BUTTON_TIME);
00604     p_customconsoleservice->updateTerm(BufferToWrite,BytesToWrite);
00605 #endif      
00606         isBlueButtonSequence = false;       
00607         HowManyButtonPress =0;  
00608 }
00609 
00610 /**
00611  * CB Triggered by the user blue button press;
00612  */
00613 static void BlueButtonPressed ()
00614 {
00615 #ifdef CUST_CONS_SERV   
00616 static uint8_t          BufferToWrite[W2ST_CONSOLE_MAX_CHAR_LEN];
00617 static uint8_t          BytesToWrite;   
00618 #endif  
00619 //        printf (" CB  BlueButtonPressed  PRESSED!!!!!!!!! %d\n\r", HowManyButtonPress);
00620     if (HowManyButtonPress == 0) { // first push
00621         BlueButtonTimer.attach(&Ticker_BlueButton, BUTTON_TIME);            
00622                 HowManyButtonPress++;           
00623                 isBlueButtonSequence = false;           
00624     } else {
00625                 HowManyButtonPress++;           
00626                 if (HowManyButtonPress == BLUEMSYS_N_BUTTON_PRESS ) {
00627                         BlueButtonTimer.detach();                   
00628 //                        printf (" CB  BlueButtoon SEQUENCE!!!!!!!!! \n\r");
00629                         HowManyButtonPress =0;
00630                         if (BLEConnectionStatus) isBlueButtonSequence = true;  
00631 #ifdef CUST_CONS_SERV
00632                         BytesToWrite = sprintf((char *)BufferToWrite, "BlueButton SEQ\r\n");
00633                         p_customconsoleservice->updateTerm(BufferToWrite,BytesToWrite);
00634 #endif                                      
00635                 }       
00636         }
00637 }
00638 
00639 
00640 void onBleInitError(BLE &ble, ble_error_t error)
00641 {
00642     (void)ble;
00643     (void)error;
00644     printf ("--->> onBleInitError\n\r");
00645    /* Initialization error handling should go here */
00646 }
00647 
00648 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
00649 {
00650     BLE&        ble   = params->ble;
00651     ble_error_t error = params->error;
00652     if (error != BLE_ERROR_NONE) {
00653         onBleInitError(ble, error);
00654         printf ("--->> bleInitComplete ERROR\n\r");         
00655         return;
00656     }
00657     if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
00658         return;
00659     }
00660     char msg = BLE_STARTED;
00661     QueueMainActivity.put((char*)msg);          
00662 }
00663 
00664 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
00665     BLE &ble = BLE::Instance();
00666     eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
00667 }
00668 
00669 
00670 void updateSensorValue (void)
00671 {
00672     float temp, hum, pres;  
00673     AxesRaw_TypeDef Magn, Acc, Gyro;
00674     int32_t decPart, intPart;
00675     int16_t TempToSend, HumToSend;
00676     uint32_t    PresToSend;
00677 #ifdef CUST_SW_SERV    
00678     static AxesRaw_TypeDef quat_axes[SEND_N_QUATERNIONS];
00679     static float QuatFloat[SEND_N_QUATERNIONS];
00680     static uint32_t QuaternionNumber =0;
00681     static uint32_t CounterFloat =0;    
00682 #endif      
00683     static unsigned int envTime =0; 
00684     static unsigned int CounterAGM =0;  
00685     
00686           TimeStamp++;  
00687         if (!BLEConnectionStatus) {
00688             if (TimeStamp % 100 == 0) {  // flash the led
00689                led = 1;
00690             }  else led = 0;
00691         }
00692           
00693 #ifdef USE_SENSOR_FUSION_LIB    
00694         static unsigned int calibTime =0;   
00695           // Run Calibration from command 
00696         if (BLEConnectionStatus) {
00697             if ((isBlueButtonSequence || ForceReCalibration) && SensorFusionOK ) {
00698                 printf("ForceReCalibration\r\n");
00699                 /* Reset the Compass Calibration */
00700                 isCal=0;
00701                 calibTime =0;
00702                 ForceReCalibration =0;
00703                 isBlueButtonSequence = false;                
00704                 led =0;                         
00705                 /* Notifications of Compass Calibration */
00706 #ifdef CUST_CONFIG_SERV
00707                 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
00708                 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
00709 #endif
00710                 /* Reset the Calibration */
00711                 ResetCalibrationInMemory();
00712                 MotionFX_manager_MagCal_start(SAMPLE_PERIOD);
00713             }
00714         }   
00715 #endif
00716                 
00717         // Update MEMS @ 100Hz and calculate quaternions
00718         if (BLEConnectionStatus) {              
00719 #ifdef CUST_SENS_SERV
00720             int32_t err=0; 
00721             err = acc_gyro.get_x_axes((int32_t*)&Acc);
00722             if (err != 0) {
00723                 printf ("= * ERROR %d GetAccelerometer\n\r", (int)err);                          
00724             }
00725                         
00726             err = acc_gyro.get_g_axes((int32_t *)&Gyro);
00727             if (err != 0) {
00728                 printf ("= * ERROR %d GetGyroscope\n\r", (int)err);                          
00729             }   
00730                         
00731             err = magnetometer.get_m_axes((int32_t *)&Magn);
00732             if (err != 0) {
00733                 printf ("= * ERROR %d Get_M_Axes\n\r", (int)err);                            
00734             }    
00735             if(CounterAGM >= ACC_GYRO_MAG_UPDATE_MUL_10MS) {            
00736                 CounterAGM=0; 
00737 #ifdef DISAGGREGATED_MOT_CHAR                
00738                 p_customsensorservice->updateEnvAccelerometer (&Acc, TimeStamp);
00739                 p_customsensorservice->updateEnvGyroscope(&Gyro, TimeStamp);
00740                 p_customsensorservice->updateEnvMagnetometer(&Magn, TimeStamp, &magOffset);
00741 #endif                
00742                 p_customsensorservice->updateEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, &magOffset);                            
00743             } else { 
00744                             CounterAGM++; 
00745                         }
00746 #endif  
00747                         
00748 #ifdef USE_SENSOR_FUSION_LIB
00749             if (SensorFusionOK) {
00750                 MotionFX_Engine_In.gyro[0] = Gyro.AXIS_X  * FROM_MDPS_TO_DPS;
00751                 MotionFX_Engine_In.gyro[1] = Gyro.AXIS_Y  * FROM_MDPS_TO_DPS;
00752                 MotionFX_Engine_In.gyro[2] = Gyro.AXIS_Z  * FROM_MDPS_TO_DPS;
00753 
00754                 MotionFX_Engine_In.acc[0] = Acc.AXIS_X * FROM_MG_TO_G;
00755                 MotionFX_Engine_In.acc[1] = Acc.AXIS_Y * FROM_MG_TO_G;
00756                 MotionFX_Engine_In.acc[2] = Acc.AXIS_Z * FROM_MG_TO_G;
00757 
00758                 MotionFX_Engine_In.mag[0] = Magn.AXIS_X * FROM_MGAUSS_TO_UT50;
00759                 MotionFX_Engine_In.mag[1] = Magn.AXIS_Y * FROM_MGAUSS_TO_UT50;
00760                 MotionFX_Engine_In.mag[2] = Magn.AXIS_Z * FROM_MGAUSS_TO_UT50;   
00761 
00762                 MotionFX_manager_run(p_MotionFX_Engine_In, p_MotionFX_Engine_Out,  MOTIONFX_ENGINE_DELTATIME);
00763             }                       
00764 #endif                      
00765 #ifdef CUST_SW_SERV 
00766             if (SensorFusionOK) {
00767                 /* Update quaternions (compact)*/
00768                 if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) {
00769                     quat_axes[QuaternionNumber].AXIS_X = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[0] * (-10000));
00770                     quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[1] * (-10000));
00771                     quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[2] * (-10000));
00772                 } else {
00773                     quat_axes[QuaternionNumber].AXIS_X = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[0] * 10000);
00774                     quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[1] * 10000);
00775                     quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[2] * 10000);
00776                 }
00777 
00778                 if (QuaternionNumber >= SEND_N_QUATERNIONS-1) {
00779                     p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp);                    
00780                     QuaternionNumber =0;
00781                 } else {
00782                     QuaternionNumber++;
00783                 }
00784 
00785                 /* Update Float Quaternions */
00786                 /* Every QUAT_FLOAT_UPDATE_MUL_10MS*10 mSeconds Send Float Quaternion informations via bluetooth */
00787                 if(CounterFloat>=QUAT_FLOAT_UPDATE_MUL_10MS) {
00788                     if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) {
00789                         QuatFloat[0] = - p_MotionFX_Engine_Out->quaternion_9X[0];
00790                         QuatFloat[1] = - p_MotionFX_Engine_Out->quaternion_9X[1];
00791                         QuatFloat[2] = - p_MotionFX_Engine_Out->quaternion_9X[2];
00792                     } else {
00793                         QuatFloat[0] =   p_MotionFX_Engine_Out->quaternion_9X[0];
00794                         QuatFloat[1] =   p_MotionFX_Engine_Out->quaternion_9X[1];
00795                         QuatFloat[2] =   p_MotionFX_Engine_Out->quaternion_9X[2];
00796                     }
00797                     p_customsoftwareservice->updateFloatQuaternions(QuatFloat, TimeStamp);
00798                     CounterFloat=0;
00799                 } else  {
00800                     CounterFloat++;
00801                 }
00802             }
00803 #endif                      
00804                 }           
00805 #ifdef CUST_CONFIG_SERV             
00806                 MFX_MagCal_input_t   mag_data_in;
00807                 MFX_MagCal_output_t  cal_data_out;
00808                 // Check if is calibrated  @ 25Hz
00809                 calibTime = calibTime + MEMS_TIMER;
00810         if(isCal!=0x01 && calibTime>= CALIB_TIMER && BLEConnectionStatus && SensorFusionOK ) {
00811                 calibTime = 0;
00812             /* Run Compass Calibration @ 25Hz */
00813             mag_data_in.mag[0]= MotionFX_Engine_In.mag[0];
00814             mag_data_in.mag[1]= MotionFX_Engine_In.mag[1];
00815             mag_data_in.mag[2]= MotionFX_Engine_In.mag[2];
00816             mag_data_in.time_stamp = TimeStamp;
00817             MotionFX_manager_MagCal_run(&mag_data_in, &cal_data_out);
00818                     
00819             /* Control the calibration status */
00820             if( (cal_data_out.cal_quality == MFX_MAGCALOK) || (cal_data_out.cal_quality == MFX_MAGCALGOOD) )
00821             {   
00822                 magOffset.magOffX = (int32_t)(cal_data_out.hi_bias[0] * FROM_UT50_TO_MGAUSS);
00823                 magOffset.magOffY = (int32_t)(cal_data_out.hi_bias[1] * FROM_UT50_TO_MGAUSS);
00824                 magOffset.magOffZ = (int32_t)(cal_data_out.hi_bias[2] * FROM_UT50_TO_MGAUSS);
00825         
00826                 /* Disable magnetometer calibration */
00827                 MotionFX_manager_MagCal_stop(SAMPLE_PERIOD);
00828                             
00829                 isCal= 1;               
00830                 led = 1;
00831                 printf("Compass Calibrated\n\r");                   
00832                 
00833                 SaveCalibrationToMemory();
00834                             
00835                 /* Notifications of Compass Calibration */
00836                 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
00837                 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);                         
00838             }
00839         } else if(isCal == 0x01 && BLEConnectionStatus)  led = 1;
00840 #endif                                  
00841         // Update environmental sensors @ 2Hz                       
00842         if (BLEConnectionStatus) {
00843           envTime = envTime + MEMS_TIMER;                   
00844                     if ( envTime >= ENV_TIMER) {
00845                         envTime =0;
00846             int32_t err;                                            
00847 #ifdef CUST_SENS_SERV
00848 #ifdef DISAGGREGATED_ENV_CHAR
00849             if (p_customsensorservice->isTempNotificationEn()) {
00850                 err = pt_sensor.get_temperature(&temp);
00851                 if ( err != 0) {
00852                     printf ("= * ERROR %d GetTemperature\n\r", (int)err);
00853                 } else {
00854                     floatToInt(temp, &intPart, &decPart, 1);                                                    
00855                     TempToSend = intPart*10+decPart;
00856                     p_customsensorservice->updateEnvTemperature(TempToSend, TimeStamp);
00857                 }
00858             }
00859 
00860             if (p_customsensorservice->isHumNotificationEn()) {
00861 #if defined (MINI_CRADLE) || defined (TARGET_DISCO_L475VG_IOT01A) || defined (TARGET_NUCLEO_F401RE)
00862                 err = ht_sensor.get_humidity(&hum);              
00863                 if ( err != 0) {
00864                     printf ("= * ERROR %d GetHumidity\n\r", (int)err);                           
00865                 } else {
00866                     floatToInt(hum, &intPart, &decPart, 1);
00867                     HumToSend = intPart*10+decPart;
00868                     p_customsensorservice->updateEnvHumidity(HumToSend, TimeStamp);       
00869                 }
00870 #endif                    
00871             }
00872 
00873             if (p_customsensorservice->isPresNotificationEn()) {
00874                 err = pt_sensor.get_pressure(&pres);
00875                 if ( err != 0) {
00876                     printf ("= * ERROR GetPressure\n\r");                           
00877                 } else {
00878                     floatToInt(pres, &intPart, &decPart, 1);
00879                     PresToSend = intPart*100+decPart;
00880                     p_customsensorservice->updateEnvPressure(PresToSend, TimeStamp);
00881                 }
00882             }
00883 #endif            
00884             if (p_customsensorservice->isPresHumTempNotificationEn()) {
00885                 err = pt_sensor.get_pressure(&pres);
00886                 if ( err != 0) {
00887                     printf ("= * ERROR GetPressure\n\r");                           
00888                 } else {
00889                     floatToInt(pres, &intPart, &decPart, 1);
00890                     PresToSend = intPart*100+decPart;
00891                 }
00892 #if defined (MINI_CRADLE) || defined (TARGET_DISCO_L475VG_IOT01A) || defined (TARGET_NUCLEO_F401RE)
00893                 err = ht_sensor.get_humidity(&hum);  
00894                 if ( err != 0) {
00895                     printf ("= * ERROR %d GetHumidity\n\r", (int)err);                           
00896                 } else {
00897                     floatToInt(hum, &intPart, &decPart, 1);
00898                     HumToSend = intPart*10+decPart;
00899                 } 
00900 #endif                               
00901                 err = pt_sensor.get_temperature(&temp);
00902                 if ( err != 0) {
00903                     printf ("= * ERROR %d GetTemperature\n\r", (int)err);
00904                 } else {
00905                     floatToInt(temp, &intPart, &decPart, 1);                                                    
00906                     TempToSend = intPart*10+decPart;
00907                 }                
00908                 p_customsensorservice->updateEnvPresHumTemp(PresToSend, HumToSend, TempToSend, TimeStamp);                
00909             }            
00910 #endif
00911         }
00912     }                           
00913 }
00914 
00915 void periodicCallback(void)
00916 {
00917     eventQueue.call(updateSensorValue);
00918 }
00919 
00920     
00921 int main()
00922 {
00923     bool isgyro_lsm6ds0Present=false;
00924     bool isgyro_lsm6ds3Present=false;    
00925     TimeStamp =0;   
00926 
00927 #ifdef CUST_SENS_SERV
00928 //    if (p_mems_expansion_board->gyro_lsm6ds0) isgyro_lsm6ds0Present=true;
00929 //    if (p_mems_expansion_board->gyro_lsm6ds3) isgyro_lsm6ds3Present=true;    
00930 #endif
00931 
00932     led = 0;
00933 #if defined (TARGET_DISCO_L475VG_IOT01A) || defined (TARGET_NUCLEO_F401RE)
00934     InterruptIn BlueButton(USER_BUTTON);    
00935     BlueButton.fall(&BlueButtonPressed);
00936 #endif    
00937     
00938     printf("\r\nSTMicroelectronics MotEnvMbedOS:\r\n"
00939 #ifdef CUST_SENS_SERV
00940            "\tGyro lsmds0 present: %d, lsmds3 present: %d\n\r"
00941            "\tSend Every %dmS Temperature/Humidity/Pressure\r\n"
00942            "\tSend Every %dmS Acc/Gyro/Magneto\r\n\n"   
00943 #endif  
00944 #ifdef CUST_SW_SERV
00945            ,"\tSend Every %dmS %d Quaternions\r\n"
00946            "\tSend Every %dmS Float precision Quaternion\r\n"
00947 #endif
00948 #ifdef CUST_SENS_SERV
00949            ,isgyro_lsm6ds0Present,
00950            isgyro_lsm6ds3Present,
00951            ENV_TIMER,
00952            MEMS_TIMER*ACC_GYRO_MAG_UPDATE_MUL_10MS
00953 #endif
00954 #ifdef CUST_SW_SERV
00955            ,MEMS_TIMER*SEND_N_QUATERNIONS,SEND_N_QUATERNIONS,
00956            MEMS_TIMER*QUAT_FLOAT_UPDATE_MUL_10MS
00957 #endif
00958           );
00959 
00960     /* Initialize MotionFX library */
00961 #ifdef USE_SENSOR_FUSION_LIB    
00962 //    bool DS3_OnBoard = DS3_ONBOARD;    
00963     MotionFX_manager_init ((unsigned char)LSM6DSL_G_0);
00964     MotionFX_manager_start_9X();
00965     SensorFusionOK = true;
00966     printf("SW      Service sensor fusion library added successfully\r\n");
00967 #else 
00968     printf("sensor fusion lib disabled \n\r");    
00969     SensorFusionOK = false;     
00970 #endif
00971                     
00972     if (NUCLEO_I2C_SHIELDS_SPEED < MAX_I2C_SPEED) 
00973         printf ("Warning I2C speed set to: %d (max: %d)\n\r", NUCLEO_I2C_SHIELDS_SPEED, MAX_I2C_SPEED);
00974  
00975     ble_started = 0;
00976     BLE &ble = BLE::Instance();
00977     p_BLEdev = &ble;
00978     p_BLEdev->onEventsToProcess(scheduleBleEventsProcessing);
00979     p_BLEdev->init(bleInitComplete);
00980  
00981     osEvent RxEvt = QueueMainActivity.get();     // waiting for bleInitComplete cb to be called
00982     if (RxEvt.value.v == BLE_STARTED) ble_started = 1;
00983     
00984     /* set the BLE CB functions */
00985     p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
00986     p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
00987     p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback);    
00988     p_BLEdev->gattServer().onDataSent(onDataSentCallback);
00989     p_BLEdev->gattServer().onDataRead(onDataReadCallback);
00990     p_BLEdev->gattServer().onDataWritten(onDataWriteCallback);
00991         
00992     p_BLEdev->gap().onConnection(onConnectionCallback);
00993     p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
00994     p_BLEdev->gap().onTimeout(onTimeoutCallback);    
00995          
00996 #ifdef CUST_CONFIG_SERV
00997     p_customconfigservice = new CustomConfigService(*p_BLEdev);
00998     if (!p_customconfigservice) {
00999         printf("SW      Service W2ST calibration add FAILED!\n\r");
01000         return 0;  
01001     }
01002     printf("SW      Service W2ST calibration added successfully\r\n");
01003 #endif
01004 #ifdef CUST_SENS_SERV
01005     magnetometer.init(NULL);
01006 #if defined (TARGET_NUCLEO_F401RE) || defined (TARGET_NUCLEO_L476RG)
01007     magnetometer.enable();   
01008     accelerometer.enable();
01009 #endif     
01010     acc_gyro.enable_g();        
01011     acc_gyro.enable_x();    
01012 #if defined (MINI_CRADLE) || defined (TARGET_DISCO_L475VG_IOT01A) || defined (TARGET_NUCLEO_F401RE)
01013     ht_sensor.init(NULL);
01014     ht_sensor.enable();
01015 #endif    
01016     pt_sensor.enable();     
01017 
01018     uint8_t id22=0, idLSM6=0, id303mag=0, id303acc=0, id221=0, idlis3mdl=0;
01019     pt_sensor.read_id(&id22);
01020     acc_gyro.read_id(&idLSM6);              
01021     magnetometer.read_id(&id303mag);
01022 #if defined (TARGET_NUCLEO_F401RE) || defined (TARGET_NUCLEO_L476RG)    
01023     accelerometer.read_id(&id303acc);
01024 #endif
01025 #ifdef CUST_SENS_SERV    
01026     p_customsensorservice = new CustomSensorService(*p_BLEdev);
01027     if (!p_customsensorservice) {
01028         printf("\n\rHW      Service W2ST sensors add FAILED!\n\r");
01029         return 0;
01030     }
01031     printf("\rHW      Service W2ST sensors added successfully\r\n");
01032 #endif  
01033   
01034 #endif       
01035 #ifdef CUST_SW_SERV
01036     if (SensorFusionOK) {
01037         p_customsoftwareservice = new CustomSoftwareService(*p_BLEdev);
01038         if (!p_customsoftwareservice) {
01039             printf("SW      Service W2ST quaternions add FAILED!\n\r");
01040             return 0;
01041         }
01042         printf("SW      Service W2ST quaternions added successfully\r\n");
01043     }
01044 #endif      
01045 #ifdef CUST_CONS_SERV
01046     p_customconsoleservice = new CustomConsoleService(*p_BLEdev);
01047     if (!p_customconsoleservice) {
01048         printf("\n\rHW      Service W2ST console add FAILED!\n\r");
01049         return 0;
01050     }
01051     printf("\rHW      Service W2ST console added successfully\r\n");
01052 #endif
01053 
01054 #if defined (TARGET_NUCLEO_L476RG) && !defined (MINI_CRADLE)
01055     printf("LS303acc ID %x LS303mag ID %x LSM6DSL ID %x LPS22HB ID %x \r\n", id303acc, id303mag, idLSM6, id22);  
01056 #endif
01057 #if defined (TARGET_NUCLEO_L476RG) && defined (MINI_CRADLE) || defined (TARGET_NUCLEO_F401RE)
01058     ht_sensor.read_id(&id221);
01059     printf("LS303acc ID %x LS303mag ID %x LSM6DSL ID %x LPS22HB ID %x HTS221 ID %x\r\n", id303acc, id303mag, idLSM6, id22, id221);  
01060 #endif    
01061 #if defined (TARGET_DISCO_L475VG_IOT01A)
01062     ht_sensor.read_id(&id221);
01063     magnetometer.read_id(&idlis3mdl);
01064     printf("LSM6DSL ID %x LPS22HB ID %x HTS221 ID %x LIS3MDL ID %x\r\n", idLSM6, id22, id221, idlis3mdl);         
01065 #endif   
01066    
01067     const static char     DEVICE_NAME[]        = BLE_DEV_NAME;
01068     /* Setup advertising. */
01069     p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
01070 //    p_BLEdev->gap().accumulateAdvertisingPayloadTxPower(txPower); 
01071 #ifdef USE_SENSOR_FUSION_LIB        
01072     uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x01,0x80};
01073 #else
01074       uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x00,0x00};
01075 #endif      
01076     p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);    
01077     p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
01078     p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
01079     p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
01080     p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
01081     
01082     printf("SERVER: BLE Stack Initialized\r\n");
01083     printf("Starting Advertising...\n\r");
01084     p_BLEdev->gap().startAdvertising();
01085         
01086     /* Control if the calibration is already available in memory */
01087 #ifdef CUST_CONFIG_SERV     
01088     if (SensorFusionOK) {
01089         ReCallCalibrationFromMemory(); 
01090         /* Switch on/off the LED according to calibration */
01091         if(isCal) {
01092             led =1;
01093         } else {
01094             led =0;
01095         }
01096     }       
01097 #endif      
01098     eventQueue.call_every(MEMS_TIMER, periodicCallback); // Start the activity      
01099     eventQueue.dispatch_forever();
01100 }
01101 
01102