ST
X-NUCLEO-IKS01A1 Environmental/Motion sensors data transmitted via X-NUCLEO-IDB04A1 BLE board. Compatible with iOS/Android ST BlueMS V2.1 application.
Dependencies: BLE_API X_NUCLEO_IDB0XA1 X_NUCLEO_IKS01A1 mbed
Fork of
Bluemicrosystem1
by 
ST Expansion SW Team
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
Go to the documentation of this file.
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>© 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 00040 /*L0_BlueNRG_xx version main*/ 00041 00042 #include <cmath> 00043 #include <math.h> 00044 #include "mbed.h" 00045 #include "Gap.h" 00046 #include "debug.h" 00047 #include "btle.h" 00048 #include "blecommon.h" 00049 #include "BLE.h" 00050 #include "UUID.h" 00051 #include "Utils.h" 00052 #include "MotionFX_Manager.h" 00053 #include "InterruptManager.h" 00054 #include "DevI2C.h" 00055 #include "BlueNRGGattServer.h" 00056 #include "GattCharacteristic.h" 00057 #include "main.h" // contains the condt compiling configuration 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 #ifdef CUST_SENS_SERV 00074 #include "x_nucleo_iks01a1.h" 00075 #include "CustomSensorsService.h" 00076 #endif 00077 00078 #ifdef CUST_CONFIG_SERV 00079 #include "CustomConfigService.h" 00080 #endif 00081 00082 #ifdef CUST_SW_SERV 00083 #include "CustomSoftwareService.h" 00084 #endif 00085 00086 static bool isBlueButtonSequence = false; 00087 static Ticker BlueButtonTimer; 00088 static DigitalOut greenled(LED1); 00089 //static int8_t txPower=-3; 00090 static unsigned char isCal = 0; 00091 static uint16_t TimeStamp=0; 00092 static int BLEConnectionStatus =0; 00093 static bool ForceReCalibration =0; 00094 static int HowManyButtonPress =0; 00095 static uint32_t timeoutEnv =0; 00096 static uint32_t timeoutCalib =0; 00097 static uint32_t timeoutMems =0; 00098 static BLE * p_BLEdev = NULL; 00099 static int32_t CounterAGM =0; 00100 00101 #ifdef CUST_CONS_SERV 00102 static CustomConsoleService * p_customconsoleservice = NULL; 00103 #endif 00104 00105 #ifdef CUST_SENS_SERV 00106 static CustomSensorService * p_customsensorservice = NULL; 00107 static X_NUCLEO_IKS01A1 * p_mems_expansion_board = NULL; 00108 #endif 00109 00110 #ifdef CUST_SW_SERV 00111 static CustomSoftwareService * p_customsoftwareservice = NULL; 00112 static osxMFX_output * p_MotionFX_Engine_Out = NULL; 00113 #endif 00114 static osxMFX_calibFactor magOffset; 00115 00116 #ifdef CUST_CONFIG_SERV 00117 static CustomConfigService * p_customconfigservice = NULL; 00118 #endif 00119 00120 00121 /*************************** Calibration functions **************************/ 00122 00123 /** 00124 * @brief Check if there are a valid Calibration Values in Memory and read them 00125 * @param None 00126 * @retval unsigned char Success/Not Success 00127 */ 00128 static unsigned char ReCallCalibrationFromMemory(void) 00129 #ifdef BLUEMSYS_STORE_CALIB_FLASH 00130 { 00131 /* ReLoad the Calibration Values from FLASH */ 00132 unsigned char Success=1; 00133 uint32_t Address = BLUEMSYS_FLASH_ADD; 00134 __IO uint32_t data32 = *(__IO uint32_t*) Address; 00135 if(data32== BLUEMSYS_CHECK_CALIBRATION) { 00136 int32_t ReadIndex; 00137 uint32_t ReadmagOffset[7]; 00138 00139 for(ReadIndex=0; ReadIndex<7; ReadIndex++) { 00140 Address +=4; 00141 data32 = *(__IO uint32_t*) Address; 00142 ReadmagOffset[ReadIndex]=data32; 00143 } 00144 00145 magOffset.magOffX = (signed short) ReadmagOffset[0]; 00146 magOffset.magOffY = (signed short) ReadmagOffset[1]; 00147 magOffset.magOffZ = (signed short) ReadmagOffset[2]; 00148 magOffset.magGainX = *((float *) &(ReadmagOffset[3])); 00149 magOffset.magGainY = *((float *) &(ReadmagOffset[4])); 00150 magOffset.magGainZ = *((float *) &(ReadmagOffset[5])); 00151 magOffset.expMagVect = *((float *) &(ReadmagOffset[6])); 00152 00153 #ifdef BLUEMSYS_DEBUG_CALIB 00154 /* Debug */ 00155 printf("magOffX ->%d\r\n",magOffset.magOffX); 00156 printf("magOffY ->%d\r\n",magOffset.magOffY); 00157 printf("magOffZ ->%d\r\n",magOffset.magOffZ); 00158 printf("magGainX ->%X\r\n",magOffset.magGainX); 00159 printf("magGainY ->%X\r\n",magOffset.magGainY); 00160 printf("magGainZ ->%X\r\n",magOffset.magGainZ); 00161 printf("expMagVect->%X\r\n",magOffset.expMagVect); 00162 #endif /* BLUEMSYS_DEBUG_CALIB */ 00163 00164 /* Set the Calibration Structure */ 00165 #ifdef USE_SENSOR_FUSION_LIB 00166 osx_MotionFX_setCalibrationData(&magOffset); 00167 00168 printf("Calibration Read from Flash\r\n"); 00169 /* Control the calibration status */ 00170 isCal = osx_MotionFX_compass_isCalibrated(); 00171 printf("Check the Calibration =%d\r\n",isCal); 00172 #endif 00173 } else { 00174 printf("Calibration Not present in FLASH\r\n"); 00175 isCal=0; 00176 } 00177 00178 return Success; 00179 } 00180 #else /* BLUEMSYS_STORE_CALIB_FLASH */ 00181 { 00182 /* ReLoad the Calibration Values from RAM */ 00183 unsigned char Success=1; 00184 00185 if(CalibrationStructureRAM[0]== BLUEMSYS_CHECK_CALIBRATION) { 00186 magOffset.magOffX = (signed short) CalibrationStructureRAM[1]; 00187 magOffset.magOffY = (signed short) CalibrationStructureRAM[2]; 00188 magOffset.magOffZ = (signed short) CalibrationStructureRAM[3]; 00189 magOffset.magGainX = *((float *) &(CalibrationStructureRAM[4])); 00190 magOffset.magGainY = *((float *) &(CalibrationStructureRAM[5])); 00191 magOffset.magGainZ = *((float *) &(CalibrationStructureRAM[6])); 00192 magOffset.expMagVect = *((float *) &(CalibrationStructureRAM[7])); 00193 00194 #ifdef BLUEMSYS_DEBUG_CALIB 00195 /* Debug */ 00196 printf("magOffX ->%d\r\n",magOffset.magOffX); 00197 printf("magOffY ->%d\r\n",magOffset.magOffY); 00198 printf("magOffZ ->%d\r\n",magOffset.magOffZ); 00199 printf("magGainX ->%X\r\n",magOffset.magGainX); 00200 printf("magGainY ->%X\r\n",magOffset.magGainY); 00201 printf("magGainZ ->%X\r\n",magOffset.magGainZ); 00202 printf("expMagVect->%X\r\n",magOffset.expMagVect); 00203 #endif /* BLUEMSYS_DEBUG_CALIB */ 00204 00205 #ifdef USE_SENSOR_FUSION_LIB 00206 /* Set the Calibration Structure */ 00207 osx_MotionFX_setCalibrationData(&magOffset); 00208 printf("Calibration Read from RAM\r\n"); 00209 00210 /* Control the calibration status */ 00211 isCal = osx_MotionFX_compass_isCalibrated(); 00212 printf("Check the Calibration =%d\r\n",isCal); 00213 #endif 00214 } else { 00215 printf("Calibration Not present in RAM\r\n"); 00216 isCal=0; 00217 } 00218 00219 return Success; 00220 } 00221 #endif /* BLUEMSYS_STORE_CALIB_FLASH */ 00222 00223 static unsigned char ResetCalibrationInMemory(void) 00224 #ifdef BLUEMSYS_STORE_CALIB_FLASH 00225 { 00226 /* Reset Calibration Values in FLASH */ 00227 unsigned char Success=1; 00228 00229 /* Erase First Flash sector */ 00230 FLASH_EraseInitTypeDef EraseInitStruct; 00231 uint32_t SectorError = 0; 00232 EraseInitStruct.TypeErase = TYPEERASE_SECTORS; 00233 EraseInitStruct.VoltageRange = VOLTAGE_RANGE_3; 00234 EraseInitStruct.Sector = BLUEMSYS_FLASH_SECTOR; 00235 EraseInitStruct.NbSectors = 1; 00236 00237 /* Unlock the Flash to enable the flash control register access *************/ 00238 HAL_FLASH_Unlock(); 00239 00240 if (HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError) != HAL_OK) { 00241 /* 00242 Error occurred while sector erase. 00243 User can add here some code to deal with this error. 00244 SectorError will contain the faulty sector and then to know the code error on this sector, 00245 user can call function 'HAL_FLASH_GetError()' 00246 */ 00247 /* 00248 FLASH_ErrorTypeDef errorcode = HAL_FLASH_GetError(); 00249 */ 00250 00251 // Error_Handler(); 00252 Success=0; 00253 } else { 00254 printf("Erase calibration in FLASH Memory\r\n"); 00255 } 00256 00257 /* Lock the Flash to disable the flash control register access (recommended 00258 to protect the FLASH memory against possible unwanted operation) *********/ 00259 HAL_FLASH_Lock(); 00260 00261 return Success; 00262 } 00263 #else /* BLUEMSYS_STORE_CALIB_FLASH */ 00264 { 00265 /* Reset Calibration Values in RAM */ 00266 unsigned char Success=1; 00267 int32_t Counter; 00268 00269 for(Counter=0; Counter<8; Counter++) 00270 CalibrationStructureRAM[Counter]=0xFFFFFFFF; 00271 00272 printf("Erase Calibration in RAM Memory\r\n"); 00273 return Success; 00274 } 00275 #endif /* BLUEMSYS_STORE_CALIB_FLASH */ 00276 00277 /** 00278 * @brief Save the Magnetometer Calibration Values to Memory 00279 * @param None 00280 * @retval unsigned char Success/Not Success 00281 */ 00282 static unsigned char SaveCalibrationToMemory(void) 00283 { 00284 unsigned char Success=1; 00285 00286 /* Reset Before The data in Memory */ 00287 Success = ResetCalibrationInMemory(); 00288 00289 if(Success) 00290 #ifdef BLUEMSYS_STORE_CALIB_FLASH 00291 { 00292 /* Store in Flash Memory */ 00293 uint32_t Address = BLUEMSYS_FLASH_ADD; 00294 uint32_t WritemagOffset[8]; 00295 int32_t WriteIndex; 00296 WritemagOffset[0] = BLUEMSYS_CHECK_CALIBRATION; 00297 WritemagOffset[1] = (uint32_t) magOffset.magOffX; 00298 WritemagOffset[2] = (uint32_t) magOffset.magOffY; 00299 WritemagOffset[3] = (uint32_t) magOffset.magOffZ; 00300 WritemagOffset[4] = *((uint32_t *) &(magOffset.magGainX)); 00301 WritemagOffset[5] = *((uint32_t *) &(magOffset.magGainY)); 00302 WritemagOffset[6] = *((uint32_t *) &(magOffset.magGainZ)); 00303 WritemagOffset[7] = *((uint32_t *) &(magOffset.expMagVect)); 00304 00305 /* Unlock the Flash to enable the flash control register access *************/ 00306 HAL_FLASH_Unlock(); 00307 00308 for(WriteIndex=0; WriteIndex<8; WriteIndex++) { 00309 if (HAL_FLASH_Program(TYPEPROGRAM_WORD, Address,WritemagOffset[WriteIndex]) == HAL_OK) { 00310 Address = Address + 4; 00311 } else { 00312 /* Error occurred while writing data in Flash memory. 00313 User can add here some code to deal with this error */ 00314 /* 00315 FLASH_ErrorTypeDef errorcode = HAL_FLASH_GetError(); 00316 */ 00317 // Error_Handler(); 00318 } 00319 } 00320 #ifdef BLUEMSYS_DEBUG_CALIB 00321 /* Debug */ 00322 printf("magOffX ->%d\r\n",magOffset.magOffX); 00323 printf("magOffY ->%d\r\n",magOffset.magOffY); 00324 printf("magOffZ ->%d\r\n",magOffset.magOffZ); 00325 printf("magGainX ->%X\r\n",magOffset.magGainX); 00326 printf("magGainY ->%X\r\n",magOffset.magGainY); 00327 printf("magGainZ ->%X\r\n",magOffset.magGainZ); 00328 printf("expMagVect->%X\r\n",magOffset.expMagVect); 00329 #endif /* BLUEMSYS_DEBUG_CALIB */ 00330 00331 printf("New Calibration Values Saved in FLASH\r\n"); 00332 00333 /* Lock the Flash to disable the flash control register access (recommended 00334 to protect the FLASH memory against possible unwanted operation) *********/ 00335 HAL_FLASH_Lock(); 00336 } 00337 #else /* BLUEMSYS_STORE_CALIB_FLASH */ 00338 { 00339 /* Store in RAM */ 00340 CalibrationStructureRAM[0] = BLUEMSYS_CHECK_CALIBRATION; 00341 CalibrationStructureRAM[1] = (uint32_t) magOffset.magOffX; 00342 CalibrationStructureRAM[2] = (uint32_t) magOffset.magOffY; 00343 CalibrationStructureRAM[3] = (uint32_t) magOffset.magOffZ; 00344 CalibrationStructureRAM[4] = *((uint32_t *) &(magOffset.magGainX)); 00345 CalibrationStructureRAM[5] = *((uint32_t *) &(magOffset.magGainY)); 00346 CalibrationStructureRAM[6] = *((uint32_t *) &(magOffset.magGainZ)); 00347 CalibrationStructureRAM[7] = *((uint32_t *) &(magOffset.expMagVect)); 00348 00349 #ifdef BLUEMSYS_DEBUG_CALIB 00350 /* Debug */ 00351 printf("magOffX ->%d\r\n",magOffset.magOffX); 00352 printf("magOffY ->%d\r\n",magOffset.magOffY); 00353 printf("magOffZ ->%d\r\n",magOffset.magOffZ); 00354 printf("magGainX ->%X\r\n",magOffset.magGainX); 00355 printf("magGainY ->%X\r\n",magOffset.magGainY); 00356 printf("magGainZ ->%X\r\n",magOffset.magGainZ); 00357 printf("expMagVect->%X\r\n",magOffset.expMagVect); 00358 #endif /* BLUEMSYS_DEBUG_CALIB */ 00359 00360 printf("New Calibration Values Saved in RAM\r\n"); 00361 } 00362 #endif /* BLUEMSYS_STORE_CALIB_FLASH */ 00363 00364 return Success; 00365 } 00366 00367 /*************************** End Calibration functions **************************/ 00368 00369 static void floatToInt(float in, int32_t *out_int, int32_t *out_dec, int32_t dec_prec) 00370 { 00371 *out_int = (int32_t)in; 00372 in = in - (float)(*out_int); 00373 // *out_dec = (int32_t)trunc(in*pow((float)10,(int)dec_prec)); 00374 *out_dec = (int32_t)(float)(int)(in*pow((float)10,(int)dec_prec)); 00375 00376 } 00377 00378 static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle) 00379 { 00380 PRINTF("onUpdatesEnabledCallback! handle: %x\n\r", handle); 00381 #ifdef CUST_SENS_SERV 00382 if (p_customsensorservice) 00383 p_customsensorservice->enNotify (handle); 00384 #endif 00385 00386 #ifdef CUST_SW_SERV 00387 if (p_customsoftwareservice) 00388 p_customsoftwareservice->enNotify (handle); 00389 #endif 00390 00391 #ifdef CUST_CONFIG_SERV 00392 if (p_customconfigservice) { 00393 p_customconfigservice->enNotify (handle); 00394 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp); 00395 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp); 00396 } 00397 #endif 00398 #ifdef CUST_CONS_SERV 00399 if (p_customconsoleservice) 00400 p_customconsoleservice->enNotify (handle); 00401 #endif 00402 /* TODO inform other obj implementing BLE services */ 00403 } 00404 00405 static void onUpdatesDisabledCallback(Gap::Handle_t handle) 00406 { 00407 PRINTF("onUpdatesDisabledCallback! handle: %x\n\r", handle); 00408 #ifdef CUST_SENS_SERV 00409 if (p_customsensorservice) 00410 p_customsensorservice->disNotify (handle); 00411 #endif 00412 00413 #ifdef CUST_SW_SERV 00414 if (p_customsoftwareservice) 00415 p_customsoftwareservice->disNotify (handle); 00416 #endif 00417 00418 #ifdef CUST_CONFIG_SERV 00419 if (p_customconfigservice) 00420 p_customconfigservice->disNotify (handle); 00421 #endif 00422 #ifdef CUST_CONS_SERV 00423 if (p_customconsoleservice) 00424 p_customconsoleservice->disNotify (handle); 00425 #endif 00426 /* TODO inform other obj implementing BLE services */ 00427 } 00428 00429 //static void onDisconnectionCallback(const Gap::Handle_t handle, const Gap::DisconnectionReason_t disConnectionReason) 00430 static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason) 00431 { 00432 printf("Disconnected\n\r"); 00433 printf("Restarting the advertising process\n\r"); 00434 TimeStamp =0; 00435 BLEConnectionStatus =0; 00436 #ifdef CUST_SENS_SERV 00437 if (p_customsensorservice) 00438 p_customsensorservice->updateConnectionStatus(DISCONNECTED); 00439 #endif 00440 #ifdef CUST_SW_SERV 00441 if (p_customsoftwareservice) 00442 p_customsoftwareservice->updateConnectionStatus(DISCONNECTED); 00443 #endif 00444 #ifdef CUST_CONFIG_SERV 00445 if (p_customconfigservice) 00446 p_customconfigservice->updateConnectionStatus(DISCONNECTED); 00447 #endif 00448 #ifdef CUST_CONS_SERV 00449 if (p_customconsoleservice) 00450 p_customconsoleservice->updateConnectionStatus(DISCONNECTED); 00451 #endif 00452 if (p_BLEdev) 00453 p_BLEdev->startAdvertising(); 00454 } 00455 00456 static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams) 00457 { 00458 printf("\rConnected: %x", connectionParams->peerAddr[Gap::ADDR_LEN-1]); 00459 for(int i=Gap::ADDR_LEN-2; i>=0; i--) printf(":%x", connectionParams->peerAddr[i]); 00460 printf("\n\r"); 00461 00462 TimeStamp =0; 00463 BLEConnectionStatus =1; 00464 #ifdef CUST_SENS_SERV 00465 if (p_customsensorservice) 00466 p_customsensorservice->updateConnectionStatus(CONNECTED); 00467 #endif 00468 #ifdef CUST_SW_SERV 00469 if (p_customsoftwareservice) 00470 p_customsoftwareservice->updateConnectionStatus(CONNECTED); 00471 #endif 00472 #ifdef CUST_CONFIG_SERV 00473 if (p_customconfigservice) 00474 p_customconfigservice->updateConnectionStatus(CONNECTED); 00475 #endif 00476 #ifdef CUST_CONS_SERV 00477 if (p_customconsoleservice) 00478 p_customconsoleservice->updateConnectionStatus(CONNECTED); 00479 #endif 00480 } 00481 00482 static void onDataReadCallback(const GattReadCallbackParams *eventDataP) 00483 { 00484 float temp, hum, pres; 00485 int16_t TempToSend, HumToSend; 00486 uint32_t PresToSend; 00487 int32_t decPart, intPart; 00488 AxesRaw_TypeDef Magn, Acc, Gyro; 00489 Gap::Handle_t handle = eventDataP->handle - BLE_HANDLE_VALUE_OFFSET; 00490 00491 #ifdef CUST_SENS_SERV 00492 if (p_customsensorservice->isTempHandle(handle)) { 00493 p_mems_expansion_board->ht_sensor->GetTemperature(&temp); 00494 floatToInt(temp, &intPart, &decPart, 1); 00495 TempToSend = intPart*10+decPart; 00496 p_customsensorservice->sendEnvTemperature(TempToSend, TimeStamp); 00497 00498 } else if (p_customsensorservice->isHumHandle(handle)) { 00499 p_mems_expansion_board->ht_sensor->GetHumidity(&hum); 00500 floatToInt(hum, &intPart, &decPart, 1); 00501 HumToSend = intPart*10+decPart; 00502 p_customsensorservice->sendEnvHumidity(HumToSend, TimeStamp); 00503 00504 } else if (p_customsensorservice->isPresHandle(handle)) { 00505 p_mems_expansion_board->pt_sensor->GetPressure(&pres); 00506 floatToInt(pres, &intPart, &decPart, 1); 00507 PresToSend = intPart*100+decPart; 00508 p_customsensorservice->sendEnvPressure(PresToSend, TimeStamp); 00509 00510 } else if (p_customsensorservice->isGyroHandle(handle)) { 00511 p_mems_expansion_board->GetGyroscope()->Get_G_Axes((int32_t *)&Gyro); 00512 p_customsensorservice->sendEnvGyroscope (&Gyro, TimeStamp); 00513 00514 } else if (p_customsensorservice->isAccHandle(handle)) { 00515 p_mems_expansion_board->GetAccelerometer()->Get_X_Axes((int32_t *)&Acc); 00516 p_customsensorservice->sendEnvAccelerometer (&Acc, TimeStamp); 00517 00518 } else if (p_customsensorservice->isMagHandle(handle)) { 00519 p_mems_expansion_board->magnetometer->Get_M_Axes((int32_t *)&Magn); 00520 p_customsensorservice->sendEnvMagnetometer(&Magn, TimeStamp, magOffset); 00521 00522 } else if (p_customsensorservice->isAccGyroMagHandle(handle)) { 00523 p_customsensorservice->sendEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, magOffset); 00524 } 00525 #endif 00526 00527 #ifdef CUST_SW_SERV 00528 if (p_customsoftwareservice->isQuatHandle(handle)) { 00529 // p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp); // dont need to update because already calculated/update into main loop 00530 00531 } else if (p_customsoftwareservice->isFloatQuatHandle(handle)) { 00532 // p_customsoftwareservice->updateFloatQuaternions(QuatFloat, TimeStamp); // dont need to update because already calculated/update into main loop 00533 } 00534 #endif 00535 00536 #ifdef CUST_CONFIG_SERV 00537 if (p_customconfigservice->isConfHandle(handle)) { 00538 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp); 00539 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp); 00540 } 00541 #endif 00542 } 00543 00544 static void onDataWriteCallback(const GattWriteCallbackParams *eventDataP) 00545 { 00546 Gap::Handle_t handle = eventDataP->handle - BLE_HANDLE_VALUE_OFFSET; 00547 PRINTF("onEventCallback!!\n\r"); 00548 printf (" myonDataWriteCallback attr_handle: %x att_data[4]: %d data_length: %d\n\r", eventDataP->handle, eventDataP->data[4], eventDataP->len ); 00549 00550 00551 #ifdef CUST_CONFIG_SERV 00552 if (p_customconfigservice->isConfHandle(handle)) { 00553 /* Received one write command from Client on calibration characteristc */ 00554 uint32_t FeatureMask = (eventDataP->data[3]) | (eventDataP->data[2]<<8) | (eventDataP->data[1]<<16) | (eventDataP->data[0]<<24); 00555 uint8_t Command = eventDataP->data[4]; 00556 switch (Command) { 00557 case W2ST_COMMAND_CAL_STATUS: 00558 00559 /* Replay with the calibration status for the feature */ 00560 // Config_Notify(FeatureMask,Command,isCal); 00561 p_customconfigservice->sendConfigState(FeatureMask, Command, isCal, TimeStamp); 00562 break; 00563 case W2ST_COMMAND_CAL_RESET: 00564 /* Reset the calibration */ 00565 ForceReCalibration=1; 00566 break; 00567 case W2ST_COMMAND_CAL_STOP: 00568 00569 /* Do nothing in this case */ 00570 break; 00571 default: 00572 // if(StdErrNotification){ 00573 // BytesToWrite = sprintf((char *)BufferToWrite, "Calibration UNKNOW Signal For Features=%x\n\r",FeatureMask); 00574 // Stderr_Update(BufferToWrite,BytesToWrite); 00575 // } else { 00576 // printf("Calibration UNKNOW Signal For Features=%x\n\r",FeatureMask); 00577 // } 00578 break; 00579 } 00580 00581 } 00582 #endif 00583 #ifdef CUST_CONS_SERV 00584 if (p_customconsoleservice->isTermHandle(handle)) { 00585 printf ("Console handle: %x data: %s\n\r", handle, eventDataP->data); 00586 // p_customconsoleservice->updateTerm((uint8_t*)eventDataP->data,eventDataP->len); 00587 } 00588 #endif 00589 00590 00591 } 00592 00593 static void onDataSentCallback(unsigned count) 00594 { 00595 PRINTF("onDataSentCallback!!\n\r"); 00596 } 00597 00598 00599 static void onTimeoutCallback(Gap::TimeoutSource_t source) 00600 { 00601 PRINTF("onTimeoutCallback!!\n\r"); 00602 } 00603 00604 static void onConfirmationReceivedCallback(uint16_t attributeHandle) 00605 { 00606 PRINTF("onConfirmationReceivedCallback!!\n\r"); 00607 } 00608 00609 00610 static void Ticker_BlueButton(void) 00611 { 00612 #ifdef CUST_CONS_SERV 00613 static uint8_t BufferToWrite[W2ST_CONSOLE_MAX_CHAR_LEN]; 00614 static uint8_t BytesToWrite; 00615 #endif 00616 00617 BlueButtonTimer.detach(); 00618 printf (" butt time expired \n\r"); 00619 #ifdef CUST_CONS_SERV 00620 BytesToWrite = sprintf((char *)BufferToWrite, "nr%d push in %1.1fs\r\n",HowManyButtonPress, BUTTON_TIME); 00621 p_customconsoleservice->updateTerm(BufferToWrite,BytesToWrite); 00622 #endif 00623 isBlueButtonSequence = false; 00624 HowManyButtonPress =0; 00625 00626 } 00627 00628 /** 00629 * CB Triggered by the user blue button press; 00630 */ 00631 static void BlueButtonPressed () 00632 { 00633 //printf (" CB BlueButtonPressed PRESSED!!!!!!!!! %d\n\r", HowManyButtonPress); 00634 if (HowManyButtonPress == 0) { // first push 00635 BlueButtonTimer.attach(&Ticker_BlueButton, BUTTON_TIME); 00636 HowManyButtonPress++; 00637 isBlueButtonSequence = false; 00638 } else { 00639 HowManyButtonPress++; 00640 if (HowManyButtonPress == BLUEMSYS_N_BUTTON_PRESS ) { 00641 BlueButtonTimer.detach(); 00642 printf (" CB BlueButtoon SEQUENCE!!!!!!!!! \n\r"); 00643 HowManyButtonPress =0; 00644 if (BLEConnectionStatus) isBlueButtonSequence = true; 00645 } 00646 } 00647 } 00648 00649 /** 00650 * CB Triggered periodically by the 'ticker' interrupt; 00651 */ 00652 static void Ticker_Env(void) 00653 { 00654 timeoutEnv = 1; 00655 } 00656 00657 static void Ticker_Calib(void) 00658 { 00659 timeoutCalib = 1; 00660 } 00661 00662 static void Ticker_Mems(void) 00663 { 00664 TimeStamp++; 00665 CounterAGM++; 00666 timeoutMems = 1; 00667 } 00668 00669 00670 int main() 00671 { 00672 Ticker EnvTimer; 00673 Ticker CalibTimer; 00674 Ticker MemsTimer; 00675 int32_t decPart, intPart; 00676 int16_t TempToSend,HumToSend; 00677 uint32_t PresToSend; 00678 float temp,hum,pres; 00679 AxesRaw_TypeDef Magn; 00680 AxesRaw_TypeDef Acc; 00681 AxesRaw_TypeDef Gyro; 00682 bool isgyro_lsm6ds0Present=false; 00683 bool isgyro_lsm6ds3Present=false; 00684 #ifdef CUST_CONS_SERV 00685 // static uint8_t BufferToWrite[256]; 00686 // static uint8_t BytesToWrite; 00687 #endif 00688 static bool SensorFusionOK = false; 00689 00690 DevI2C *i2c = new DevI2C(I2C_SDA, I2C_SCL); 00691 i2c->frequency(NUCLEO_I2C_SHIELDS_SPEED); 00692 00693 #ifdef CUST_SENS_SERV 00694 p_mems_expansion_board = X_NUCLEO_IKS01A1::Instance(i2c); 00695 if (p_mems_expansion_board->gyro_lsm6ds0) isgyro_lsm6ds0Present=true; 00696 if (p_mems_expansion_board->gyro_lsm6ds3) isgyro_lsm6ds3Present=true; 00697 #endif 00698 //#ifdef CUST_CONFIG_SERV 00699 InterruptIn BlueButton(USER_BUTTON); 00700 BlueButton.fall(&BlueButtonPressed); 00701 //#endif 00702 printf("\r\nSTMicroelectronics BlueMicrosystem1 W2ST:\r\n" 00703 #ifdef CUST_SENS_SERV 00704 "\tGyro lsmds0 present: %d, lsmds3 present: %d\n\r" 00705 "\tSend Every %dmS Temperature/Humidity/Pressure\r\n" 00706 "\tSend Every %dmS Acc/Gyro/Magneto\r\n\n" 00707 #endif 00708 #ifdef CUST_SW_SERV 00709 ,"\tSend Every %dmS %d Quaternions\r\n" 00710 "\tSend Every %dmS Float precision Quaternion\r\n" 00711 #endif 00712 #ifdef CUST_SENS_SERV 00713 ,isgyro_lsm6ds0Present, 00714 isgyro_lsm6ds3Present, 00715 ENV_TIMER/1000, 00716 MEMS_TIMER/1000*ACC_GYRO_MAG_UPDATE_MUL_10MS 00717 #endif 00718 #ifdef CUST_SW_SERV 00719 ,MEMS_TIMER/1000*SEND_N_QUATERNIONS,SEND_N_QUATERNIONS, 00720 MEMS_TIMER/1000*QUAT_FLOAT_UPDATE_MUL_10MS 00721 #endif 00722 ); 00723 00724 00725 if (NUCLEO_I2C_SHIELDS_SPEED < MAX_I2C_SPEED) 00726 printf ("Warning I2C speed set to: %d (max: %d)\n\r", NUCLEO_I2C_SHIELDS_SPEED, MAX_I2C_SPEED); 00727 00728 p_BLEdev = new BLE; 00729 if (!p_BLEdev) { 00730 printf("\r\nBLE Device creation failed\r\n"); 00731 return 0; 00732 } 00733 p_BLEdev->init(); 00734 00735 greenled = 0;//Switch OFF LED1 00736 00737 /* set the BLE CB functions */ 00738 p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback); 00739 p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback); 00740 p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback); 00741 p_BLEdev->gattServer().onDataSent(onDataSentCallback); 00742 p_BLEdev->gattServer().onDataRead(onDataReadCallback); 00743 p_BLEdev->gattServer().onDataWritten(onDataWriteCallback); 00744 p_BLEdev->gap().onConnection(onConnectionCallback); 00745 p_BLEdev->gap().onDisconnection(onDisconnectionCallback); 00746 p_BLEdev->gap().onTimeout(onTimeoutCallback); 00747 00748 00749 /* Initialize MotionFX library */ 00750 #ifdef USE_SENSOR_FUSION_LIB 00751 bool DS3_OnBoard = DS3_ONBOARD; 00752 if (MotionFX_manager_init(DS3_OnBoard, magOffset)) { 00753 MotionFX_manager_start_9X(); 00754 SensorFusionOK = true; 00755 printf("SW Service sensor fusion library added successfully\r\n"); 00756 } else { /* sensor fusion lib init failed: likely because of wrong license */ 00757 printf("sensor fusion lib init failed: likely because of wrong license \n\r"); 00758 SensorFusionOK = false; 00759 } 00760 #else 00761 printf("sensor fusion lib disabled \n\r"); 00762 SensorFusionOK = false; 00763 #endif 00764 00765 #ifdef CUST_CONFIG_SERV 00766 p_customconfigservice = new CustomConfigService(*p_BLEdev); 00767 if (!p_customconfigservice) { 00768 printf("SW Service W2ST calibration add FAILED!\n\r"); 00769 return 0; 00770 } 00771 printf("SW Service W2ST calibration added successfully\r\n"); 00772 #endif 00773 #ifdef CUST_CONS_SERV 00774 p_customconsoleservice = new CustomConsoleService(*p_BLEdev); 00775 if (!p_customconsoleservice) { 00776 printf("\n\rHW Service W2ST console add FAILED!\n\r"); 00777 return 0; 00778 } 00779 printf("\rHW Service W2ST console added successfully\r\n"); 00780 #endif 00781 #ifdef CUST_SENS_SERV 00782 p_customsensorservice = new CustomSensorService(*p_BLEdev); 00783 if (!p_customsensorservice) { 00784 printf("\n\rHW Service W2ST sensors add FAILED!\n\r"); 00785 return 0; 00786 } 00787 printf("\rHW Service W2ST sensors added successfully\r\n"); 00788 #endif 00789 #ifdef CUST_SW_SERV 00790 AxesRaw_TypeDef quat_axes[SEND_N_QUATERNIONS]; 00791 float QuatFloat[SEND_N_QUATERNIONS]; 00792 if (SensorFusionOK) { 00793 p_customsoftwareservice = new CustomSoftwareService(*p_BLEdev); 00794 if (!p_customsoftwareservice) { 00795 printf("SW Service W2ST quaternions add FAILED!\n\r"); 00796 return 0; 00797 } 00798 printf("SW Service W2ST quaternions added successfully\r\n"); 00799 } 00800 #endif 00801 00802 const static char DEVICE_NAME[] = BLE_DEV_NAME; 00803 /* Setup advertising. */ 00804 p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); 00805 // p_BLEdev->gap().accumulateAdvertisingPayloadTxPower(txPower); 00806 #ifdef USE_SENSOR_FUSION_LIB 00807 uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x01,0x80}; 00808 #else 00809 uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x00,0x00}; 00810 #endif 00811 p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6); 00812 p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN); 00813 p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); 00814 p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); 00815 p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL); 00816 00817 printf("SERVER: BLE Stack Initialized\r\n"); 00818 printf("Starting Advertising...\n\r"); 00819 p_BLEdev->gap().startAdvertising(); 00820 00821 TimeStamp =0; 00822 EnvTimer.attach_us(&Ticker_Env, ENV_TIMER); 00823 CalibTimer.attach_us(&Ticker_Calib, CALIB_TIMER ); 00824 MemsTimer.attach_us(&Ticker_Mems, MEMS_TIMER); 00825 00826 /* Control if the calibration is already available in memory */ 00827 if (SensorFusionOK) { 00828 ReCallCalibrationFromMemory(); 00829 00830 /* Switch on/off the LED according to calibration */ 00831 if(isCal) { 00832 greenled =1; 00833 } else { 00834 greenled =0; 00835 } 00836 } 00837 #ifdef CUST_SW_SERV 00838 uint32_t QuaternionNumber =0; 00839 uint32_t CounterFloat =0; 00840 #endif 00841 00842 while(1) { 00843 00844 if (BLEConnectionStatus) { 00845 if ((isBlueButtonSequence || ForceReCalibration) && SensorFusionOK ) { 00846 printf("ForceReCalibration\r\n"); 00847 /* Reset the Compass Calibration */ 00848 isCal=0; 00849 ForceReCalibration =0; 00850 isBlueButtonSequence = false; 00851 /* Notifications of Compass Calibration */ 00852 #ifdef CUST_CONFIG_SERV 00853 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp); 00854 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp); 00855 #endif 00856 #ifdef USE_SENSOR_FUSION_LIB 00857 /* Reset the Calibration */ 00858 osx_MotionFX_compass_forceReCalibration(); 00859 #endif 00860 ResetCalibrationInMemory(); 00861 00862 /* Reset Calibration offset */ 00863 magOffset.magOffX = magOffset.magOffY= magOffset.magOffZ=0; 00864 greenled =0; 00865 } 00866 } 00867 // Update MEMS @ 100Hz and calculate quaternions 00868 if (timeoutMems && BLEConnectionStatus) { 00869 timeoutMems =0; 00870 00871 #ifdef CUST_SENS_SERV 00872 int32_t err; 00873 err = p_mems_expansion_board->GetAccelerometer()->Get_X_Axes((int32_t *)&Acc); 00874 if (err != IMU_6AXES_OK) { 00875 printf ("= * ERROR %d GetAccelerometer\n\r", err); 00876 } 00877 err = p_mems_expansion_board->GetGyroscope()->Get_G_Axes((int32_t *)&Gyro); 00878 if (err != IMU_6AXES_OK) { 00879 printf ("= * ERROR %d GetGyroscope\n\r", err); 00880 } 00881 err = p_mems_expansion_board->magnetometer->Get_M_Axes((int32_t *)&Magn); 00882 if (err != MAGNETO_OK) { 00883 printf ("= * ERROR %d Get_M_Axes\n\r", err); 00884 } 00885 if(CounterAGM >= ACC_GYRO_MAG_UPDATE_MUL_10MS) { 00886 CounterAGM=0; 00887 p_customsensorservice->updateEnvAccelerometer (&Acc, TimeStamp); 00888 p_customsensorservice->updateEnvGyroscope(&Gyro, TimeStamp); 00889 p_customsensorservice->updateEnvMagnetometer(&Magn, TimeStamp, magOffset); 00890 p_customsensorservice->updateEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, magOffset); 00891 } 00892 #endif 00893 00894 #ifdef USE_SENSOR_FUSION_LIB 00895 if (SensorFusionOK) { 00896 MotionFX_manager_run((AxesRaw_t*)&Acc, (AxesRaw_t*)&Gyro, (AxesRaw_t*)&Magn, magOffset); 00897 p_MotionFX_Engine_Out = MotionFX_manager_getDataOUT(); 00898 } 00899 #endif 00900 #ifdef CUST_SW_SERV 00901 if (SensorFusionOK) { 00902 /* Update quaternions */ 00903 if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) { 00904 quat_axes[QuaternionNumber].AXIS_X = (int)(p_MotionFX_Engine_Out->quaternion_9X[0] * (-10000)); 00905 quat_axes[QuaternionNumber].AXIS_Y = (int)(p_MotionFX_Engine_Out->quaternion_9X[1] * (-10000)); 00906 quat_axes[QuaternionNumber].AXIS_Z = (int)(p_MotionFX_Engine_Out->quaternion_9X[2] * (-10000)); 00907 } else { 00908 quat_axes[QuaternionNumber].AXIS_X = (int)(p_MotionFX_Engine_Out->quaternion_9X[0] * 10000); 00909 quat_axes[QuaternionNumber].AXIS_Y = (int)(p_MotionFX_Engine_Out->quaternion_9X[1] * 10000); 00910 quat_axes[QuaternionNumber].AXIS_Z = (int)(p_MotionFX_Engine_Out->quaternion_9X[2] * 10000); 00911 } 00912 00913 if (QuaternionNumber == SEND_N_QUATERNIONS-1) { 00914 p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp); 00915 QuaternionNumber =0; 00916 } else { 00917 QuaternionNumber++; 00918 } 00919 00920 /* Update Float Quaternions */ 00921 /* Every QUAT_FLOAT_UPDATE_MUL_10MS*10 mSeconds Send Float Quaternion informations via bluetooth */ 00922 if(CounterFloat==QUAT_FLOAT_UPDATE_MUL_10MS) { 00923 if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) { 00924 QuatFloat[0] = - p_MotionFX_Engine_Out->quaternion_9X[0]; 00925 QuatFloat[1] = - p_MotionFX_Engine_Out->quaternion_9X[1]; 00926 QuatFloat[2] = - p_MotionFX_Engine_Out->quaternion_9X[2]; 00927 } else { 00928 QuatFloat[0] = p_MotionFX_Engine_Out->quaternion_9X[0]; 00929 QuatFloat[1] = p_MotionFX_Engine_Out->quaternion_9X[1]; 00930 QuatFloat[2] = p_MotionFX_Engine_Out->quaternion_9X[2]; 00931 } 00932 p_customsoftwareservice->updateFloatQuaternions(QuatFloat, TimeStamp); 00933 CounterFloat=0; 00934 } else { 00935 CounterFloat++; 00936 } 00937 } 00938 #endif 00939 } 00940 00941 /* Check if is calibrated @ 25Hz */ 00942 if(isCal!=0x01 && timeoutCalib && BLEConnectionStatus && SensorFusionOK ) { 00943 timeoutCalib =0; 00944 /* Run Compass Calibration @ 25Hz */ 00945 #ifdef USE_SENSOR_FUSION_LIB 00946 osx_MotionFX_compass_saveAcc(Acc.AXIS_X, Acc.AXIS_Y, Acc.AXIS_Z); /* Accelerometer data ENU systems coordinate */ 00947 osx_MotionFX_compass_saveMag(Magn.AXIS_X, Magn.AXIS_Y, Magn.AXIS_Z); /* Magnetometer data ENU systems coordinate */ 00948 osx_MotionFX_compass_run(); 00949 00950 /* Control the calibration status */ 00951 isCal = osx_MotionFX_compass_isCalibrated(); 00952 #endif 00953 if(isCal == 0x01) { 00954 00955 printf("Compass Calibrated\n\r"); 00956 /* Get new magnetometer offset */ 00957 #ifdef USE_SENSOR_FUSION_LIB 00958 osx_MotionFX_getCalibrationData(&magOffset); 00959 #endif 00960 /* Save the calibration in Memory */ 00961 SaveCalibrationToMemory(); 00962 00963 /* Switch on the Led */ 00964 greenled = 1; 00965 00966 /* Notifications of Compass Calibration */ 00967 #ifdef CUST_CONFIG_SERV 00968 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp); 00969 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp); 00970 #endif 00971 } 00972 } 00973 00974 // Update environmental sensors @ 2Hz 00975 if (timeoutEnv && BLEConnectionStatus) { 00976 timeoutEnv =0; 00977 int32_t err; 00978 #ifdef CUST_SENS_SERV 00979 if (p_customsensorservice->isTempNotificationEn()) { 00980 err = p_mems_expansion_board->ht_sensor->GetTemperature(&temp); 00981 if ( err != HUM_TEMP_OK) { 00982 printf ("= * ERROR %d GetTemperature\n\r", err); 00983 } else { 00984 floatToInt(temp, &intPart, &decPart, 1); 00985 TempToSend = intPart*10+decPart; 00986 p_customsensorservice->updateEnvTemperature(TempToSend, TimeStamp); 00987 } 00988 } 00989 00990 if (p_customsensorservice->isHumNotificationEn()) { 00991 err = p_mems_expansion_board->ht_sensor->GetHumidity(&hum); 00992 if ( err != HUM_TEMP_OK) { 00993 printf ("= * ERROR %d GetHumidity\n\r", err); 00994 } else { 00995 floatToInt(hum, &intPart, &decPart, 1); 00996 HumToSend = intPart*10+decPart; 00997 p_customsensorservice->updateEnvHumidity(HumToSend, TimeStamp); 00998 } 00999 } 01000 01001 if (p_customsensorservice->isPresNotificationEn()) { 01002 err = p_mems_expansion_board->pt_sensor->GetPressure(&pres); 01003 if ( err != PRESSURE_OK) { 01004 printf ("= * ERROR GetPressure\n\r"); 01005 } else { 01006 floatToInt(pres, &intPart, &decPart, 1); 01007 PresToSend = intPart*100+decPart; 01008 p_customsensorservice->updateEnvPressure(PresToSend, TimeStamp); 01009 } 01010 } 01011 #endif 01012 } 01013 p_BLEdev->waitForEvent(); 01014 } 01015 }
Generated on Sun Jul 17 2022 23:51:22 by
1.7.2