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

BlueMicrosystem application

This application is the mbed equivalent of ST BlueMicrosystem1 and provides an example of motion and environmental data exported via Bluetooth Low Energy to an Android or IOS device.
It runs on a ST NUCLEO-F401RE board connected with a X-NUCLEO-IKS01A1 and a X-NUCLEO-IDB04A1 expansion boards and is compatible with Android and iOS ST BlueMS smartphone applications (based on Android and iOS BlueST SDKs).
By default the application is not providing sensor fusion and activity recognition features. However sensor fusion can be enabled following the steps below:

  • Download and install osxMotionFX library on your PC.
  • Obtain the free license for your board following the instructions
  • Copy the correct license into Middlewares/ST/STM32_OSX_MotionFX_Library/osx_license.h of your mbed program folder
  • Copy Middlewares/ST/STM32_OSX_MotionFX_Library/Inc/osx_motion_fx.h file
  • Rename the provided .lib Keil binary library giving it a .ar extension, then copy it into Middlewares/ST/STM32_OSX_MotionFX_Library/Lib of your mbed program folder
  • Enable USE_SENSOR_FUSION_LIB macro into MotionFX_Manager.h file and recompile.
Committer:
mapellil
Date:
Thu Dec 17 15:58:35 2015 +0000
Revision:
7:34014895dda8
Parent:
6:c1b8fb74072e
Child:
8:b042fe719f51
Updated BLE stack/API (now BigEndian UUID), set max tx power

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mapellil 6:c1b8fb74072e 1 /**
mapellil 6:c1b8fb74072e 2 ******************************************************************************
mapellil 6:c1b8fb74072e 3 * @file main.cpp
mapellil 6:c1b8fb74072e 4 * @author AST / EST
mapellil 6:c1b8fb74072e 5 * @version V0.0.1
mapellil 6:c1b8fb74072e 6 * @date 16-Dec-2015
mapellil 6:c1b8fb74072e 7 * @brief main of Bluemicrosystem1 application
mapellil 6:c1b8fb74072e 8 ******************************************************************************
mapellil 6:c1b8fb74072e 9 * @attention
mapellil 6:c1b8fb74072e 10 *
mapellil 6:c1b8fb74072e 11 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
mapellil 6:c1b8fb74072e 12 *
mapellil 6:c1b8fb74072e 13 * Redistribution and use in source and binary forms, with or without modification,
mapellil 6:c1b8fb74072e 14 * are permitted provided that the following conditions are met:
mapellil 6:c1b8fb74072e 15 * 1. Redistributions of source code must retain the above copyright notice,
mapellil 6:c1b8fb74072e 16 * this list of conditions and the following disclaimer.
mapellil 6:c1b8fb74072e 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
mapellil 6:c1b8fb74072e 18 * this list of conditions and the following disclaimer in the documentation
mapellil 6:c1b8fb74072e 19 * and/or other materials provided with the distribution.
mapellil 6:c1b8fb74072e 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
mapellil 6:c1b8fb74072e 21 * may be used to endorse or promote products derived from this software
mapellil 6:c1b8fb74072e 22 * without specific prior written permission.
mapellil 6:c1b8fb74072e 23 *
mapellil 6:c1b8fb74072e 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
mapellil 6:c1b8fb74072e 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
mapellil 6:c1b8fb74072e 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
mapellil 6:c1b8fb74072e 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
mapellil 6:c1b8fb74072e 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
mapellil 6:c1b8fb74072e 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
mapellil 6:c1b8fb74072e 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
mapellil 6:c1b8fb74072e 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
mapellil 6:c1b8fb74072e 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
mapellil 6:c1b8fb74072e 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
mapellil 6:c1b8fb74072e 34 *
mapellil 6:c1b8fb74072e 35 ******************************************************************************
mapellil 6:c1b8fb74072e 36 */
mapellil 0:e93a11b4e044 37 #include "mbed.h"
mapellil 0:e93a11b4e044 38 #include <cmath>
mapellil 0:e93a11b4e044 39 #include <math.h>
mapellil 0:e93a11b4e044 40 #include "mbed.h"
mapellil 0:e93a11b4e044 41 #include "Gap.h"
mapellil 0:e93a11b4e044 42 #include "debug.h"
mapellil 0:e93a11b4e044 43 #include "btle.h"
mapellil 0:e93a11b4e044 44 #include "blecommon.h"
mapellil 0:e93a11b4e044 45 #include "BLE.h"
mapellil 0:e93a11b4e044 46 #include "UUID.h"
mapellil 0:e93a11b4e044 47 #include "Utils.h"
mapellil 0:e93a11b4e044 48 #include "MotionFX_Manager.h"
mapellil 0:e93a11b4e044 49 #include "InterruptManager.h"
mapellil 0:e93a11b4e044 50 #include "DevI2C.h"
mapellil 0:e93a11b4e044 51 #include "BlueNRGGattServer.h"
mapellil 0:e93a11b4e044 52 #include "GattCharacteristic.h"
mapellil 0:e93a11b4e044 53 #include "main.h" // contains the condt compiling configuration
mapellil 0:e93a11b4e044 54
mapellil 0:e93a11b4e044 55 typedef struct {
mapellil 0:e93a11b4e044 56 int32_t AXIS_X;
mapellil 0:e93a11b4e044 57 int32_t AXIS_Y;
mapellil 0:e93a11b4e044 58 int32_t AXIS_Z;
mapellil 0:e93a11b4e044 59 } AxesRaw_TypeDef;
mapellil 0:e93a11b4e044 60
mapellil 6:c1b8fb74072e 61 #ifdef CUST_BLEERR_MNGT
mapellil 6:c1b8fb74072e 62 #include "CustomBleErrManagement.h"
mapellil 6:c1b8fb74072e 63 #endif
mapellil 6:c1b8fb74072e 64
mapellil 0:e93a11b4e044 65 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 66 #include "CustomConsoleService.h"
mapellil 0:e93a11b4e044 67 #endif
mapellil 0:e93a11b4e044 68
mapellil 0:e93a11b4e044 69 #ifdef CUST_BATT_SERV
mapellil 0:e93a11b4e044 70 #include "CustomBatteryService.h"
mapellil 0:e93a11b4e044 71 #endif
mapellil 0:e93a11b4e044 72
mapellil 0:e93a11b4e044 73 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 74 #include "x_nucleo_iks01a1.h"
mapellil 0:e93a11b4e044 75 #include "CustomSensorsService.h"
mapellil 0:e93a11b4e044 76 #endif
mapellil 0:e93a11b4e044 77
mapellil 0:e93a11b4e044 78 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 79 #include "CustomConfigService.h"
mapellil 0:e93a11b4e044 80 #endif
mapellil 0:e93a11b4e044 81
mapellil 0:e93a11b4e044 82 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 83 #include "CustomSoftwareService.h"
mapellil 0:e93a11b4e044 84 #endif
mapellil 0:e93a11b4e044 85
mapellil 0:e93a11b4e044 86 static bool isBlueButtonSequence = false;
mapellil 0:e93a11b4e044 87 static Ticker BlueButtonTimer;
mapellil 0:e93a11b4e044 88 static DigitalOut greenled(LED1);
mapellil 2:d61a6f4d9682 89 //static int8_t txPower=-3;
mapellil 0:e93a11b4e044 90 static unsigned char isCal = 0;
mapellil 0:e93a11b4e044 91 static uint16_t TimeStamp=0;
mapellil 0:e93a11b4e044 92 static int BLEConnectionStatus =0;
mapellil 0:e93a11b4e044 93 static bool ForceReCalibration =0;
mapellil 0:e93a11b4e044 94 static int HowManyButtonPress =0;
mapellil 0:e93a11b4e044 95 static uint32_t timeoutEnv =0;
mapellil 0:e93a11b4e044 96 static uint32_t timeoutCalib =0;
mapellil 0:e93a11b4e044 97 static uint32_t timeoutMems =0;
mapellil 0:e93a11b4e044 98 static BLE * p_BLEdev = NULL;
mapellil 0:e93a11b4e044 99 static int32_t CounterAGM =0;
mapellil 0:e93a11b4e044 100 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 101 static CustomConsoleService * p_customconsoleservice = NULL;
mapellil 0:e93a11b4e044 102 #endif
mapellil 0:e93a11b4e044 103
mapellil 0:e93a11b4e044 104 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 105 static CustomSensorService * p_customsensorservice = NULL;
mapellil 0:e93a11b4e044 106 static X_NUCLEO_IKS01A1 * p_mems_expansion_board = NULL;
mapellil 0:e93a11b4e044 107 #endif
mapellil 0:e93a11b4e044 108
mapellil 0:e93a11b4e044 109 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 110 static CustomSoftwareService * p_customsoftwareservice = NULL;
mapellil 2:d61a6f4d9682 111 static osxMFX_output * p_MotionFX_Engine_Out = NULL;
mapellil 0:e93a11b4e044 112 #endif
mapellil 2:d61a6f4d9682 113 static osxMFX_calibFactor magOffset;
mapellil 0:e93a11b4e044 114
mapellil 0:e93a11b4e044 115 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 116 static CustomConfigService * p_customconfigservice = NULL;
mapellil 0:e93a11b4e044 117 #endif
mapellil 0:e93a11b4e044 118
mapellil 6:c1b8fb74072e 119 #ifdef CUST_BLEERR_MNGT
mapellil 6:c1b8fb74072e 120 static CustomBleErrManagement * p_customBleErrManagement = NULL;
mapellil 6:c1b8fb74072e 121 #endif
mapellil 0:e93a11b4e044 122
mapellil 0:e93a11b4e044 123 /*************************** Calibration functions **************************/
mapellil 0:e93a11b4e044 124
mapellil 0:e93a11b4e044 125 /**
mapellil 0:e93a11b4e044 126 * @brief Check if there are a valid Calibration Values in Memory and read them
mapellil 0:e93a11b4e044 127 * @param None
mapellil 0:e93a11b4e044 128 * @retval unsigned char Success/Not Success
mapellil 0:e93a11b4e044 129 */
mapellil 0:e93a11b4e044 130 static unsigned char ReCallCalibrationFromMemory(void)
mapellil 0:e93a11b4e044 131 #ifdef BLUEMSYS_STORE_CALIB_FLASH
mapellil 0:e93a11b4e044 132 {
mapellil 0:e93a11b4e044 133 /* ReLoad the Calibration Values from FLASH */
mapellil 0:e93a11b4e044 134 unsigned char Success=1;
mapellil 0:e93a11b4e044 135 uint32_t Address = BLUEMSYS_FLASH_ADD;
mapellil 0:e93a11b4e044 136 __IO uint32_t data32 = *(__IO uint32_t*) Address;
mapellil 0:e93a11b4e044 137 if(data32== BLUEMSYS_CHECK_CALIBRATION) {
mapellil 0:e93a11b4e044 138 int32_t ReadIndex;
mapellil 0:e93a11b4e044 139 uint32_t ReadmagOffset[7];
mapellil 0:e93a11b4e044 140
mapellil 0:e93a11b4e044 141 for(ReadIndex=0; ReadIndex<7; ReadIndex++) {
mapellil 0:e93a11b4e044 142 Address +=4;
mapellil 0:e93a11b4e044 143 data32 = *(__IO uint32_t*) Address;
mapellil 0:e93a11b4e044 144 ReadmagOffset[ReadIndex]=data32;
mapellil 0:e93a11b4e044 145 }
mapellil 0:e93a11b4e044 146
mapellil 0:e93a11b4e044 147 magOffset.magOffX = (signed short) ReadmagOffset[0];
mapellil 0:e93a11b4e044 148 magOffset.magOffY = (signed short) ReadmagOffset[1];
mapellil 0:e93a11b4e044 149 magOffset.magOffZ = (signed short) ReadmagOffset[2];
mapellil 0:e93a11b4e044 150 magOffset.magGainX = *((float *) &(ReadmagOffset[3]));
mapellil 0:e93a11b4e044 151 magOffset.magGainY = *((float *) &(ReadmagOffset[4]));
mapellil 0:e93a11b4e044 152 magOffset.magGainZ = *((float *) &(ReadmagOffset[5]));
mapellil 0:e93a11b4e044 153 magOffset.expMagVect = *((float *) &(ReadmagOffset[6]));
mapellil 0:e93a11b4e044 154
mapellil 0:e93a11b4e044 155 #ifdef BLUEMSYS_DEBUG_CALIB
mapellil 0:e93a11b4e044 156 /* Debug */
mapellil 0:e93a11b4e044 157 printf("magOffX ->%d\r\n",magOffset.magOffX);
mapellil 0:e93a11b4e044 158 printf("magOffY ->%d\r\n",magOffset.magOffY);
mapellil 0:e93a11b4e044 159 printf("magOffZ ->%d\r\n",magOffset.magOffZ);
mapellil 0:e93a11b4e044 160 printf("magGainX ->%X\r\n",magOffset.magGainX);
mapellil 0:e93a11b4e044 161 printf("magGainY ->%X\r\n",magOffset.magGainY);
mapellil 0:e93a11b4e044 162 printf("magGainZ ->%X\r\n",magOffset.magGainZ);
mapellil 0:e93a11b4e044 163 printf("expMagVect->%X\r\n",magOffset.expMagVect);
mapellil 0:e93a11b4e044 164 #endif /* BLUEMSYS_DEBUG_CALIB */
mapellil 0:e93a11b4e044 165
mapellil 0:e93a11b4e044 166 /* Set the Calibration Structure */
mapellil 0:e93a11b4e044 167 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 168 osx_MotionFX_setCalibrationData(&magOffset);
mapellil 1:5fb861c45605 169
mapellil 0:e93a11b4e044 170 printf("Calibration Read from Flash\r\n");
mapellil 0:e93a11b4e044 171 /* Control the calibration status */
mapellil 0:e93a11b4e044 172 isCal = osx_MotionFX_compass_isCalibrated();
mapellil 0:e93a11b4e044 173 printf("Check the Calibration =%d\r\n",isCal);
mapellil 0:e93a11b4e044 174 #endif
mapellil 0:e93a11b4e044 175 } else {
mapellil 0:e93a11b4e044 176 printf("Calibration Not present in FLASH\r\n");
mapellil 0:e93a11b4e044 177 isCal=0;
mapellil 0:e93a11b4e044 178 }
mapellil 0:e93a11b4e044 179
mapellil 0:e93a11b4e044 180 return Success;
mapellil 0:e93a11b4e044 181 }
mapellil 0:e93a11b4e044 182 #else /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 183 {
mapellil 0:e93a11b4e044 184 /* ReLoad the Calibration Values from RAM */
mapellil 0:e93a11b4e044 185 unsigned char Success=1;
mapellil 0:e93a11b4e044 186
mapellil 0:e93a11b4e044 187 if(CalibrationStructureRAM[0]== BLUEMSYS_CHECK_CALIBRATION) {
mapellil 0:e93a11b4e044 188 magOffset.magOffX = (signed short) CalibrationStructureRAM[1];
mapellil 0:e93a11b4e044 189 magOffset.magOffY = (signed short) CalibrationStructureRAM[2];
mapellil 0:e93a11b4e044 190 magOffset.magOffZ = (signed short) CalibrationStructureRAM[3];
mapellil 0:e93a11b4e044 191 magOffset.magGainX = *((float *) &(CalibrationStructureRAM[4]));
mapellil 0:e93a11b4e044 192 magOffset.magGainY = *((float *) &(CalibrationStructureRAM[5]));
mapellil 0:e93a11b4e044 193 magOffset.magGainZ = *((float *) &(CalibrationStructureRAM[6]));
mapellil 0:e93a11b4e044 194 magOffset.expMagVect = *((float *) &(CalibrationStructureRAM[7]));
mapellil 0:e93a11b4e044 195
mapellil 0:e93a11b4e044 196 #ifdef BLUEMSYS_DEBUG_CALIB
mapellil 0:e93a11b4e044 197 /* Debug */
mapellil 0:e93a11b4e044 198 printf("magOffX ->%d\r\n",magOffset.magOffX);
mapellil 0:e93a11b4e044 199 printf("magOffY ->%d\r\n",magOffset.magOffY);
mapellil 0:e93a11b4e044 200 printf("magOffZ ->%d\r\n",magOffset.magOffZ);
mapellil 0:e93a11b4e044 201 printf("magGainX ->%X\r\n",magOffset.magGainX);
mapellil 0:e93a11b4e044 202 printf("magGainY ->%X\r\n",magOffset.magGainY);
mapellil 0:e93a11b4e044 203 printf("magGainZ ->%X\r\n",magOffset.magGainZ);
mapellil 0:e93a11b4e044 204 printf("expMagVect->%X\r\n",magOffset.expMagVect);
mapellil 0:e93a11b4e044 205 #endif /* BLUEMSYS_DEBUG_CALIB */
mapellil 0:e93a11b4e044 206
mapellil 1:5fb861c45605 207 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 208 /* Set the Calibration Structure */
mapellil 0:e93a11b4e044 209 osx_MotionFX_setCalibrationData(&magOffset);
mapellil 0:e93a11b4e044 210 printf("Calibration Read from RAM\r\n");
mapellil 0:e93a11b4e044 211
mapellil 0:e93a11b4e044 212 /* Control the calibration status */
mapellil 0:e93a11b4e044 213 isCal = osx_MotionFX_compass_isCalibrated();
mapellil 0:e93a11b4e044 214 printf("Check the Calibration =%d\r\n",isCal);
mapellil 0:e93a11b4e044 215 #endif
mapellil 0:e93a11b4e044 216 } else {
mapellil 0:e93a11b4e044 217 printf("Calibration Not present in RAM\r\n");
mapellil 0:e93a11b4e044 218 isCal=0;
mapellil 0:e93a11b4e044 219 }
mapellil 0:e93a11b4e044 220
mapellil 0:e93a11b4e044 221 return Success;
mapellil 0:e93a11b4e044 222 }
mapellil 0:e93a11b4e044 223 #endif /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 224
mapellil 0:e93a11b4e044 225 static unsigned char ResetCalibrationInMemory(void)
mapellil 0:e93a11b4e044 226 #ifdef BLUEMSYS_STORE_CALIB_FLASH
mapellil 0:e93a11b4e044 227 {
mapellil 0:e93a11b4e044 228 /* Reset Calibration Values in FLASH */
mapellil 0:e93a11b4e044 229 unsigned char Success=1;
mapellil 0:e93a11b4e044 230
mapellil 0:e93a11b4e044 231 /* Erase First Flash sector */
mapellil 0:e93a11b4e044 232 FLASH_EraseInitTypeDef EraseInitStruct;
mapellil 0:e93a11b4e044 233 uint32_t SectorError = 0;
mapellil 0:e93a11b4e044 234 EraseInitStruct.TypeErase = TYPEERASE_SECTORS;
mapellil 0:e93a11b4e044 235 EraseInitStruct.VoltageRange = VOLTAGE_RANGE_3;
mapellil 0:e93a11b4e044 236 EraseInitStruct.Sector = BLUEMSYS_FLASH_SECTOR;
mapellil 0:e93a11b4e044 237 EraseInitStruct.NbSectors = 1;
mapellil 0:e93a11b4e044 238
mapellil 0:e93a11b4e044 239 /* Unlock the Flash to enable the flash control register access *************/
mapellil 0:e93a11b4e044 240 HAL_FLASH_Unlock();
mapellil 0:e93a11b4e044 241
mapellil 0:e93a11b4e044 242 if (HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError) != HAL_OK) {
mapellil 0:e93a11b4e044 243 /*
mapellil 0:e93a11b4e044 244 Error occurred while sector erase.
mapellil 0:e93a11b4e044 245 User can add here some code to deal with this error.
mapellil 0:e93a11b4e044 246 SectorError will contain the faulty sector and then to know the code error on this sector,
mapellil 0:e93a11b4e044 247 user can call function 'HAL_FLASH_GetError()'
mapellil 0:e93a11b4e044 248 */
mapellil 0:e93a11b4e044 249 /*
mapellil 0:e93a11b4e044 250 FLASH_ErrorTypeDef errorcode = HAL_FLASH_GetError();
mapellil 0:e93a11b4e044 251 */
mapellil 0:e93a11b4e044 252
mapellil 0:e93a11b4e044 253 // Error_Handler();
mapellil 0:e93a11b4e044 254 Success=0;
mapellil 0:e93a11b4e044 255 } else {
mapellil 0:e93a11b4e044 256 printf("Erase calibration in FLASH Memory\r\n");
mapellil 0:e93a11b4e044 257 }
mapellil 0:e93a11b4e044 258
mapellil 0:e93a11b4e044 259 /* Lock the Flash to disable the flash control register access (recommended
mapellil 0:e93a11b4e044 260 to protect the FLASH memory against possible unwanted operation) *********/
mapellil 0:e93a11b4e044 261 HAL_FLASH_Lock();
mapellil 0:e93a11b4e044 262
mapellil 0:e93a11b4e044 263 return Success;
mapellil 0:e93a11b4e044 264 }
mapellil 0:e93a11b4e044 265 #else /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 266 {
mapellil 0:e93a11b4e044 267 /* Reset Calibration Values in RAM */
mapellil 0:e93a11b4e044 268 unsigned char Success=1;
mapellil 0:e93a11b4e044 269 int32_t Counter;
mapellil 0:e93a11b4e044 270
mapellil 0:e93a11b4e044 271 for(Counter=0; Counter<8; Counter++)
mapellil 0:e93a11b4e044 272 CalibrationStructureRAM[Counter]=0xFFFFFFFF;
mapellil 0:e93a11b4e044 273
mapellil 0:e93a11b4e044 274 printf("Erase Calibration in RAM Memory\r\n");
mapellil 0:e93a11b4e044 275 return Success;
mapellil 0:e93a11b4e044 276 }
mapellil 0:e93a11b4e044 277 #endif /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 278
mapellil 0:e93a11b4e044 279 /**
mapellil 0:e93a11b4e044 280 * @brief Save the Magnetometer Calibration Values to Memory
mapellil 0:e93a11b4e044 281 * @param None
mapellil 0:e93a11b4e044 282 * @retval unsigned char Success/Not Success
mapellil 0:e93a11b4e044 283 */
mapellil 0:e93a11b4e044 284 static unsigned char SaveCalibrationToMemory(void)
mapellil 0:e93a11b4e044 285 {
mapellil 0:e93a11b4e044 286 unsigned char Success=1;
mapellil 0:e93a11b4e044 287
mapellil 0:e93a11b4e044 288 /* Reset Before The data in Memory */
mapellil 0:e93a11b4e044 289 Success = ResetCalibrationInMemory();
mapellil 0:e93a11b4e044 290
mapellil 0:e93a11b4e044 291 if(Success)
mapellil 0:e93a11b4e044 292 #ifdef BLUEMSYS_STORE_CALIB_FLASH
mapellil 0:e93a11b4e044 293 {
mapellil 0:e93a11b4e044 294 /* Store in Flash Memory */
mapellil 0:e93a11b4e044 295 uint32_t Address = BLUEMSYS_FLASH_ADD;
mapellil 0:e93a11b4e044 296 uint32_t WritemagOffset[8];
mapellil 0:e93a11b4e044 297 int32_t WriteIndex;
mapellil 0:e93a11b4e044 298 WritemagOffset[0] = BLUEMSYS_CHECK_CALIBRATION;
mapellil 0:e93a11b4e044 299 WritemagOffset[1] = (uint32_t) magOffset.magOffX;
mapellil 0:e93a11b4e044 300 WritemagOffset[2] = (uint32_t) magOffset.magOffY;
mapellil 0:e93a11b4e044 301 WritemagOffset[3] = (uint32_t) magOffset.magOffZ;
mapellil 0:e93a11b4e044 302 WritemagOffset[4] = *((uint32_t *) &(magOffset.magGainX));
mapellil 0:e93a11b4e044 303 WritemagOffset[5] = *((uint32_t *) &(magOffset.magGainY));
mapellil 0:e93a11b4e044 304 WritemagOffset[6] = *((uint32_t *) &(magOffset.magGainZ));
mapellil 0:e93a11b4e044 305 WritemagOffset[7] = *((uint32_t *) &(magOffset.expMagVect));
mapellil 0:e93a11b4e044 306
mapellil 0:e93a11b4e044 307 /* Unlock the Flash to enable the flash control register access *************/
mapellil 0:e93a11b4e044 308 HAL_FLASH_Unlock();
mapellil 0:e93a11b4e044 309
mapellil 0:e93a11b4e044 310 for(WriteIndex=0; WriteIndex<8; WriteIndex++) {
mapellil 0:e93a11b4e044 311 if (HAL_FLASH_Program(TYPEPROGRAM_WORD, Address,WritemagOffset[WriteIndex]) == HAL_OK) {
mapellil 0:e93a11b4e044 312 Address = Address + 4;
mapellil 0:e93a11b4e044 313 } else {
mapellil 0:e93a11b4e044 314 /* Error occurred while writing data in Flash memory.
mapellil 0:e93a11b4e044 315 User can add here some code to deal with this error */
mapellil 0:e93a11b4e044 316 /*
mapellil 0:e93a11b4e044 317 FLASH_ErrorTypeDef errorcode = HAL_FLASH_GetError();
mapellil 0:e93a11b4e044 318 */
mapellil 0:e93a11b4e044 319 // Error_Handler();
mapellil 0:e93a11b4e044 320 }
mapellil 0:e93a11b4e044 321 }
mapellil 0:e93a11b4e044 322 #ifdef BLUEMSYS_DEBUG_CALIB
mapellil 0:e93a11b4e044 323 /* Debug */
mapellil 0:e93a11b4e044 324 printf("magOffX ->%d\r\n",magOffset.magOffX);
mapellil 0:e93a11b4e044 325 printf("magOffY ->%d\r\n",magOffset.magOffY);
mapellil 0:e93a11b4e044 326 printf("magOffZ ->%d\r\n",magOffset.magOffZ);
mapellil 0:e93a11b4e044 327 printf("magGainX ->%X\r\n",magOffset.magGainX);
mapellil 0:e93a11b4e044 328 printf("magGainY ->%X\r\n",magOffset.magGainY);
mapellil 0:e93a11b4e044 329 printf("magGainZ ->%X\r\n",magOffset.magGainZ);
mapellil 0:e93a11b4e044 330 printf("expMagVect->%X\r\n",magOffset.expMagVect);
mapellil 0:e93a11b4e044 331 #endif /* BLUEMSYS_DEBUG_CALIB */
mapellil 0:e93a11b4e044 332
mapellil 0:e93a11b4e044 333 printf("New Calibration Values Saved in FLASH\r\n");
mapellil 0:e93a11b4e044 334
mapellil 0:e93a11b4e044 335 /* Lock the Flash to disable the flash control register access (recommended
mapellil 0:e93a11b4e044 336 to protect the FLASH memory against possible unwanted operation) *********/
mapellil 0:e93a11b4e044 337 HAL_FLASH_Lock();
mapellil 0:e93a11b4e044 338 }
mapellil 0:e93a11b4e044 339 #else /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 340 {
mapellil 0:e93a11b4e044 341 /* Store in RAM */
mapellil 0:e93a11b4e044 342 CalibrationStructureRAM[0] = BLUEMSYS_CHECK_CALIBRATION;
mapellil 0:e93a11b4e044 343 CalibrationStructureRAM[1] = (uint32_t) magOffset.magOffX;
mapellil 0:e93a11b4e044 344 CalibrationStructureRAM[2] = (uint32_t) magOffset.magOffY;
mapellil 0:e93a11b4e044 345 CalibrationStructureRAM[3] = (uint32_t) magOffset.magOffZ;
mapellil 0:e93a11b4e044 346 CalibrationStructureRAM[4] = *((uint32_t *) &(magOffset.magGainX));
mapellil 0:e93a11b4e044 347 CalibrationStructureRAM[5] = *((uint32_t *) &(magOffset.magGainY));
mapellil 0:e93a11b4e044 348 CalibrationStructureRAM[6] = *((uint32_t *) &(magOffset.magGainZ));
mapellil 0:e93a11b4e044 349 CalibrationStructureRAM[7] = *((uint32_t *) &(magOffset.expMagVect));
mapellil 0:e93a11b4e044 350
mapellil 0:e93a11b4e044 351 #ifdef BLUEMSYS_DEBUG_CALIB
mapellil 0:e93a11b4e044 352 /* Debug */
mapellil 0:e93a11b4e044 353 printf("magOffX ->%d\r\n",magOffset.magOffX);
mapellil 0:e93a11b4e044 354 printf("magOffY ->%d\r\n",magOffset.magOffY);
mapellil 0:e93a11b4e044 355 printf("magOffZ ->%d\r\n",magOffset.magOffZ);
mapellil 0:e93a11b4e044 356 printf("magGainX ->%X\r\n",magOffset.magGainX);
mapellil 0:e93a11b4e044 357 printf("magGainY ->%X\r\n",magOffset.magGainY);
mapellil 0:e93a11b4e044 358 printf("magGainZ ->%X\r\n",magOffset.magGainZ);
mapellil 0:e93a11b4e044 359 printf("expMagVect->%X\r\n",magOffset.expMagVect);
mapellil 0:e93a11b4e044 360 #endif /* BLUEMSYS_DEBUG_CALIB */
mapellil 0:e93a11b4e044 361
mapellil 0:e93a11b4e044 362 printf("New Calibration Values Saved in RAM\r\n");
mapellil 0:e93a11b4e044 363 }
mapellil 0:e93a11b4e044 364 #endif /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 365
mapellil 0:e93a11b4e044 366 return Success;
mapellil 0:e93a11b4e044 367 }
mapellil 0:e93a11b4e044 368
mapellil 0:e93a11b4e044 369 /*************************** End Calibration functions **************************/
mapellil 0:e93a11b4e044 370
mapellil 0:e93a11b4e044 371 static void floatToInt(float in, int32_t *out_int, int32_t *out_dec, int32_t dec_prec)
mapellil 0:e93a11b4e044 372 {
mapellil 0:e93a11b4e044 373 *out_int = (int32_t)in;
mapellil 0:e93a11b4e044 374 in = in - (float)(*out_int);
mapellil 0:e93a11b4e044 375 // *out_dec = (int32_t)trunc(in*pow((float)10,(int)dec_prec));
mapellil 0:e93a11b4e044 376 *out_dec = (int32_t)(float)(int)(in*pow((float)10,(int)dec_prec));
mapellil 0:e93a11b4e044 377
mapellil 0:e93a11b4e044 378 }
mapellil 0:e93a11b4e044 379
mapellil 0:e93a11b4e044 380 static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
mapellil 0:e93a11b4e044 381 {
mapellil 4:007539036889 382 PRINTF("onUpdatesEnabledCallback! handle: %x\n\r", handle);
mapellil 0:e93a11b4e044 383 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 384 if (p_customsensorservice)
mapellil 0:e93a11b4e044 385 p_customsensorservice->enNotify (handle);
mapellil 0:e93a11b4e044 386 #endif
mapellil 0:e93a11b4e044 387 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 388 if (p_customsoftwareservice)
mapellil 0:e93a11b4e044 389 p_customsoftwareservice->enNotify (handle);
mapellil 0:e93a11b4e044 390 #endif
mapellil 0:e93a11b4e044 391 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 392 if (p_customconfigservice) {
mapellil 0:e93a11b4e044 393 p_customconfigservice->enNotify (handle);
mapellil 0:e93a11b4e044 394 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 395 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 396 }
mapellil 0:e93a11b4e044 397 #endif
mapellil 0:e93a11b4e044 398 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 399 if (p_customconsoleservice)
mapellil 0:e93a11b4e044 400 p_customconsoleservice->enNotify (handle);
mapellil 0:e93a11b4e044 401 #endif
mapellil 6:c1b8fb74072e 402 #ifdef CUST_BLEERR_MNGT
mapellil 6:c1b8fb74072e 403 if (p_customBleErrManagement)
mapellil 6:c1b8fb74072e 404 p_customBleErrManagement->clearTxErr();
mapellil 6:c1b8fb74072e 405 #endif
mapellil 6:c1b8fb74072e 406 /* TODO inform other obj implementing BLE services if any other service is added */
mapellil 0:e93a11b4e044 407 }
mapellil 0:e93a11b4e044 408
mapellil 0:e93a11b4e044 409 static void onUpdatesDisabledCallback(Gap::Handle_t handle)
mapellil 0:e93a11b4e044 410 {
mapellil 4:007539036889 411 PRINTF("onUpdatesDisabledCallback! handle: %x\n\r", handle);
mapellil 0:e93a11b4e044 412 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 413 if (p_customsensorservice)
mapellil 0:e93a11b4e044 414 p_customsensorservice->disNotify (handle);
mapellil 0:e93a11b4e044 415 #endif
mapellil 0:e93a11b4e044 416 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 417 if (p_customsoftwareservice)
mapellil 0:e93a11b4e044 418 p_customsoftwareservice->disNotify (handle);
mapellil 0:e93a11b4e044 419 #endif
mapellil 0:e93a11b4e044 420 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 421 if (p_customconfigservice)
mapellil 0:e93a11b4e044 422 p_customconfigservice->disNotify (handle);
mapellil 0:e93a11b4e044 423 #endif
mapellil 0:e93a11b4e044 424 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 425 if (p_customconsoleservice)
mapellil 0:e93a11b4e044 426 p_customconsoleservice->disNotify (handle);
mapellil 0:e93a11b4e044 427 #endif
mapellil 6:c1b8fb74072e 428 #ifdef CUST_BLEERR_MNGT
mapellil 6:c1b8fb74072e 429 if (p_customBleErrManagement)
mapellil 6:c1b8fb74072e 430 p_customBleErrManagement->clearTxErr();
mapellil 6:c1b8fb74072e 431 #endif
mapellil 6:c1b8fb74072e 432 /* TODO inform other obj implementing BLE services if any other service is added */
mapellil 0:e93a11b4e044 433 }
mapellil 0:e93a11b4e044 434
mapellil 0:e93a11b4e044 435 //static void onDisconnectionCallback(const Gap::Handle_t handle, const Gap::DisconnectionReason_t disConnectionReason)
mapellil 0:e93a11b4e044 436 static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
mapellil 0:e93a11b4e044 437 {
mapellil 0:e93a11b4e044 438 printf("Disconnected\n\r");
mapellil 0:e93a11b4e044 439 printf("Restarting the advertising process\n\r");
mapellil 0:e93a11b4e044 440 TimeStamp =0;
mapellil 0:e93a11b4e044 441 BLEConnectionStatus =0;
mapellil 0:e93a11b4e044 442 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 443 if (p_customsensorservice)
mapellil 0:e93a11b4e044 444 p_customsensorservice->updateConnectionStatus(DISCONNECTED);
mapellil 0:e93a11b4e044 445 #endif
mapellil 0:e93a11b4e044 446 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 447 if (p_customsoftwareservice)
mapellil 0:e93a11b4e044 448 p_customsoftwareservice->updateConnectionStatus(DISCONNECTED);
mapellil 0:e93a11b4e044 449 #endif
mapellil 0:e93a11b4e044 450 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 451 if (p_customconfigservice)
mapellil 0:e93a11b4e044 452 p_customconfigservice->updateConnectionStatus(DISCONNECTED);
mapellil 0:e93a11b4e044 453 #endif
mapellil 0:e93a11b4e044 454 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 455 if (p_customconsoleservice)
mapellil 0:e93a11b4e044 456 p_customconsoleservice->updateConnectionStatus(DISCONNECTED);
mapellil 6:c1b8fb74072e 457 #endif
mapellil 6:c1b8fb74072e 458 #ifdef CUST_BLEERR_MNGT
mapellil 6:c1b8fb74072e 459 if (p_customBleErrManagement)
mapellil 6:c1b8fb74072e 460 p_customBleErrManagement->updateConnectionStatus(DISCONNECTED);
mapellil 0:e93a11b4e044 461 #endif
mapellil 6:c1b8fb74072e 462 /* TODO inform other obj implementing BLE services if any other service is added */
mapellil 0:e93a11b4e044 463 if (p_BLEdev)
mapellil 0:e93a11b4e044 464 p_BLEdev->startAdvertising();
mapellil 0:e93a11b4e044 465 }
mapellil 0:e93a11b4e044 466
mapellil 0:e93a11b4e044 467 static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
mapellil 0:e93a11b4e044 468 {
mapellil 0:e93a11b4e044 469 printf("\rConnected: %x", connectionParams->peerAddr[Gap::ADDR_LEN-1]);
mapellil 0:e93a11b4e044 470 for(int i=Gap::ADDR_LEN-2; i>=0; i--) printf(":%x", connectionParams->peerAddr[i]);
mapellil 0:e93a11b4e044 471 printf("\n\r");
mapellil 0:e93a11b4e044 472
mapellil 0:e93a11b4e044 473 TimeStamp =0;
mapellil 0:e93a11b4e044 474 BLEConnectionStatus =1;
mapellil 0:e93a11b4e044 475 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 476 if (p_customsensorservice)
mapellil 0:e93a11b4e044 477 p_customsensorservice->updateConnectionStatus(CONNECTED);
mapellil 0:e93a11b4e044 478 #endif
mapellil 0:e93a11b4e044 479 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 480 if (p_customsoftwareservice)
mapellil 0:e93a11b4e044 481 p_customsoftwareservice->updateConnectionStatus(CONNECTED);
mapellil 0:e93a11b4e044 482 #endif
mapellil 0:e93a11b4e044 483 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 484 if (p_customconfigservice)
mapellil 0:e93a11b4e044 485 p_customconfigservice->updateConnectionStatus(CONNECTED);
mapellil 0:e93a11b4e044 486 #endif
mapellil 0:e93a11b4e044 487 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 488 if (p_customconsoleservice)
mapellil 0:e93a11b4e044 489 p_customconsoleservice->updateConnectionStatus(CONNECTED);
mapellil 0:e93a11b4e044 490 #endif
mapellil 6:c1b8fb74072e 491 #ifdef CUST_BLEERR_MNGT
mapellil 6:c1b8fb74072e 492 if (p_customBleErrManagement)
mapellil 6:c1b8fb74072e 493 p_customBleErrManagement->updateConnectionStatus(CONNECTED);
mapellil 6:c1b8fb74072e 494 #endif
mapellil 6:c1b8fb74072e 495 /* TODO inform other obj implementing BLE services if any other service is added */
mapellil 0:e93a11b4e044 496 }
mapellil 0:e93a11b4e044 497
mapellil 0:e93a11b4e044 498 static void onDataReadCallback(const GattReadCallbackParams *eventDataP)
mapellil 0:e93a11b4e044 499 {
mapellil 0:e93a11b4e044 500 float temp, hum, pres;
mapellil 0:e93a11b4e044 501 int16_t TempToSend, HumToSend;
mapellil 0:e93a11b4e044 502 uint32_t PresToSend;
mapellil 0:e93a11b4e044 503 int32_t decPart, intPart;
mapellil 0:e93a11b4e044 504 AxesRaw_TypeDef Magn, Acc, Gyro;
mapellil 0:e93a11b4e044 505
mapellil 0:e93a11b4e044 506 printf ("onDataReadCallback\n\r");
mapellil 0:e93a11b4e044 507 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 508 if (p_customsensorservice->isTempHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 509 p_mems_expansion_board->ht_sensor->GetTemperature(&temp);
mapellil 0:e93a11b4e044 510 floatToInt(temp, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 511 TempToSend = intPart*10+decPart;
mapellil 0:e93a11b4e044 512 p_customsensorservice->sendEnvTemperature(TempToSend, TimeStamp);
mapellil 0:e93a11b4e044 513
mapellil 0:e93a11b4e044 514 } else if (p_customsensorservice->isHumHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 515 p_mems_expansion_board->ht_sensor->GetHumidity(&hum);
mapellil 0:e93a11b4e044 516 floatToInt(hum, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 517 HumToSend = intPart*10+decPart;
mapellil 0:e93a11b4e044 518 p_customsensorservice->sendEnvHumidity(HumToSend, TimeStamp);
mapellil 0:e93a11b4e044 519
mapellil 0:e93a11b4e044 520 } else if (p_customsensorservice->isPresHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 521 p_mems_expansion_board->pt_sensor->GetPressure(&pres);
mapellil 0:e93a11b4e044 522 floatToInt(pres, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 523 PresToSend = intPart*100+decPart;
mapellil 0:e93a11b4e044 524 p_customsensorservice->sendEnvPressure(PresToSend, TimeStamp);
mapellil 0:e93a11b4e044 525
mapellil 0:e93a11b4e044 526 } else if (p_customsensorservice->isGyroHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 527 p_mems_expansion_board->GetGyroscope()->Get_G_Axes((int32_t *)&Gyro);
mapellil 0:e93a11b4e044 528 p_customsensorservice->sendEnvGyroscope (&Gyro, TimeStamp);
mapellil 0:e93a11b4e044 529
mapellil 0:e93a11b4e044 530 } else if (p_customsensorservice->isAccHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 531 p_mems_expansion_board->GetAccelerometer()->Get_X_Axes((int32_t *)&Acc);
mapellil 0:e93a11b4e044 532 p_customsensorservice->sendEnvAccelerometer (&Acc, TimeStamp);
mapellil 0:e93a11b4e044 533
mapellil 0:e93a11b4e044 534 } else if (p_customsensorservice->isMagHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 535 p_mems_expansion_board->magnetometer->Get_M_Axes((int32_t *)&Magn);
mapellil 0:e93a11b4e044 536 p_customsensorservice->sendEnvMagnetometer(&Magn, TimeStamp, magOffset);
mapellil 0:e93a11b4e044 537
mapellil 0:e93a11b4e044 538 } else if (p_customsensorservice->isAccGyroMagHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 539 p_customsensorservice->sendEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, magOffset);
mapellil 0:e93a11b4e044 540 }
mapellil 0:e93a11b4e044 541 #endif
mapellil 0:e93a11b4e044 542
mapellil 0:e93a11b4e044 543 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 544 if (p_customsoftwareservice->isQuatHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 545 // p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp); // dont need to update because already calculated/update into main loop
mapellil 0:e93a11b4e044 546
mapellil 0:e93a11b4e044 547 } else if (p_customsoftwareservice->isFloatQuatHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 548 // p_customsoftwareservice->updateFloatQuaternions(QuatFloat, TimeStamp); // dont need to update because already calculated/update into main loop
mapellil 0:e93a11b4e044 549 }
mapellil 0:e93a11b4e044 550 #endif
mapellil 0:e93a11b4e044 551
mapellil 0:e93a11b4e044 552 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 553 if (p_customconfigservice->isConfHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 554 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 555 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 556 }
mapellil 0:e93a11b4e044 557 #endif
mapellil 0:e93a11b4e044 558 }
mapellil 0:e93a11b4e044 559
mapellil 0:e93a11b4e044 560 static void myonDataWriteCallback(const GattWriteCallbackParams *eventDataP)
mapellil 0:e93a11b4e044 561 {
mapellil 4:007539036889 562 PRINTF("onEventCallback!!\n\r");
mapellil 0:e93a11b4e044 563 printf (" myonDataWriteCallback attr_handle: %x att_data[0]: %d data_length: %d\n\r", eventDataP->handle, eventDataP->data[0], eventDataP->len );
mapellil 0:e93a11b4e044 564
mapellil 0:e93a11b4e044 565
mapellil 0:e93a11b4e044 566 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 567 if (p_customconfigservice->isConfHandle(eventDataP->handle)) {
mapellil 0:e93a11b4e044 568 /* Received one write command from Client on calibration characteristc */
mapellil 0:e93a11b4e044 569 uint32_t FeatureMask = (eventDataP->data[3]) | (eventDataP->data[2]<<8) | (eventDataP->data[1]<<16) | (eventDataP->data[0]<<24);
mapellil 0:e93a11b4e044 570 uint8_t Command = eventDataP->data[4];
mapellil 0:e93a11b4e044 571 switch (Command) {
mapellil 0:e93a11b4e044 572 case W2ST_COMMAND_CAL_STATUS:
mapellil 0:e93a11b4e044 573
mapellil 0:e93a11b4e044 574 /* Replay with the calibration status for the feature */
mapellil 0:e93a11b4e044 575 // Config_Notify(FeatureMask,Command,isCal);
mapellil 0:e93a11b4e044 576 p_customconfigservice->sendConfigState(FeatureMask, Command, isCal, TimeStamp);
mapellil 0:e93a11b4e044 577 break;
mapellil 0:e93a11b4e044 578 case W2ST_COMMAND_CAL_RESET:
mapellil 0:e93a11b4e044 579
mapellil 0:e93a11b4e044 580 /* Reset the calibration */
mapellil 0:e93a11b4e044 581 ForceReCalibration=1;
mapellil 0:e93a11b4e044 582 break;
mapellil 0:e93a11b4e044 583 case W2ST_COMMAND_CAL_STOP:
mapellil 0:e93a11b4e044 584
mapellil 0:e93a11b4e044 585 /* Do nothing in this case */
mapellil 0:e93a11b4e044 586 break;
mapellil 0:e93a11b4e044 587 default:
mapellil 0:e93a11b4e044 588 // if(StdErrNotification){
mapellil 0:e93a11b4e044 589 // BytesToWrite = sprintf((char *)BufferToWrite, "Calibration UNKNOW Signal For Features=%x\n\r",FeatureMask);
mapellil 0:e93a11b4e044 590 // Stderr_Update(BufferToWrite,BytesToWrite);
mapellil 0:e93a11b4e044 591 // } else {
mapellil 0:e93a11b4e044 592 // printf("Calibration UNKNOW Signal For Features=%x\n\r",FeatureMask);
mapellil 0:e93a11b4e044 593 // }
mapellil 0:e93a11b4e044 594 break;
mapellil 0:e93a11b4e044 595 }
mapellil 0:e93a11b4e044 596
mapellil 0:e93a11b4e044 597 }
mapellil 0:e93a11b4e044 598 #endif
mapellil 0:e93a11b4e044 599 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 600 // printf ("-------------->>> attr_handle: %x\n\r", attr_handle);
mapellil 0:e93a11b4e044 601 // if (p_customconsoleservice->isTermHandle(attr_handle -1)) {
mapellil 0:e93a11b4e044 602 // p_customconsoleservice->updateTerm(att_data,data_length);
mapellil 0:e93a11b4e044 603 // }
mapellil 0:e93a11b4e044 604 #endif
mapellil 0:e93a11b4e044 605
mapellil 0:e93a11b4e044 606
mapellil 0:e93a11b4e044 607 }
mapellil 0:e93a11b4e044 608
mapellil 0:e93a11b4e044 609 static void onDataSentCallback(unsigned count)
mapellil 0:e93a11b4e044 610 {
mapellil 4:007539036889 611 PRINTF("onDataSentCallback!!\n\r");
mapellil 0:e93a11b4e044 612 }
mapellil 0:e93a11b4e044 613
mapellil 0:e93a11b4e044 614
mapellil 0:e93a11b4e044 615 static void onTimeoutCallback(Gap::TimeoutSource_t source)
mapellil 0:e93a11b4e044 616 {
mapellil 4:007539036889 617 PRINTF("onTimeoutCallback!!\n\r");
mapellil 0:e93a11b4e044 618 }
mapellil 0:e93a11b4e044 619
mapellil 0:e93a11b4e044 620 static void onConfirmationReceivedCallback(uint16_t attributeHandle)
mapellil 0:e93a11b4e044 621 {
mapellil 4:007539036889 622 PRINTF("onConfirmationReceivedCallback!!\n\r");
mapellil 0:e93a11b4e044 623 }
mapellil 0:e93a11b4e044 624
mapellil 0:e93a11b4e044 625
mapellil 0:e93a11b4e044 626 static void Ticker_BlueButton(void)
mapellil 0:e93a11b4e044 627 {
mapellil 0:e93a11b4e044 628 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 629 static uint8_t BufferToWrite[W2ST_CONSOLE_MAX_CHAR_LEN];
mapellil 0:e93a11b4e044 630 static uint8_t BytesToWrite;
mapellil 0:e93a11b4e044 631 #endif
mapellil 0:e93a11b4e044 632
mapellil 0:e93a11b4e044 633 BlueButtonTimer.detach();
mapellil 0:e93a11b4e044 634 printf (" butt time expired \n\r");
mapellil 0:e93a11b4e044 635 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 636 BytesToWrite = sprintf((char *)BufferToWrite, "nr%d push in %1.1fs\r\n",HowManyButtonPress, BUTTON_TIME);
mapellil 0:e93a11b4e044 637 p_customconsoleservice->updateTerm(BufferToWrite,BytesToWrite);
mapellil 0:e93a11b4e044 638 #endif
mapellil 0:e93a11b4e044 639 isBlueButtonSequence = false;
mapellil 0:e93a11b4e044 640 HowManyButtonPress =0;
mapellil 0:e93a11b4e044 641
mapellil 0:e93a11b4e044 642 }
mapellil 0:e93a11b4e044 643
mapellil 0:e93a11b4e044 644 /**
mapellil 0:e93a11b4e044 645 * CB Triggered by the user blue button press;
mapellil 0:e93a11b4e044 646 */
mapellil 0:e93a11b4e044 647 static void BlueButtonPressed ()
mapellil 0:e93a11b4e044 648 {
mapellil 0:e93a11b4e044 649 //printf (" CB BlueButtonPressed PRESSED!!!!!!!!! %d\n\r", HowManyButtonPress);
mapellil 0:e93a11b4e044 650 if (HowManyButtonPress == 0) { // first push
mapellil 0:e93a11b4e044 651 BlueButtonTimer.attach(&Ticker_BlueButton, BUTTON_TIME);
mapellil 0:e93a11b4e044 652 HowManyButtonPress++;
mapellil 0:e93a11b4e044 653 isBlueButtonSequence = false;
mapellil 0:e93a11b4e044 654 } else {
mapellil 0:e93a11b4e044 655 HowManyButtonPress++;
mapellil 0:e93a11b4e044 656 if (HowManyButtonPress == BLUEMSYS_N_BUTTON_PRESS ) {
mapellil 0:e93a11b4e044 657 BlueButtonTimer.detach();
mapellil 0:e93a11b4e044 658 printf (" CB BlueButtoon SEQUENCE!!!!!!!!! \n\r");
mapellil 0:e93a11b4e044 659 HowManyButtonPress =0;
mapellil 0:e93a11b4e044 660 if (BLEConnectionStatus) isBlueButtonSequence = true;
mapellil 0:e93a11b4e044 661 }
mapellil 0:e93a11b4e044 662 }
mapellil 0:e93a11b4e044 663 }
mapellil 0:e93a11b4e044 664
mapellil 0:e93a11b4e044 665 /**
mapellil 0:e93a11b4e044 666 * CB Triggered periodically by the 'ticker' interrupt;
mapellil 0:e93a11b4e044 667 */
mapellil 0:e93a11b4e044 668 static void Ticker_Env(void)
mapellil 0:e93a11b4e044 669 {
mapellil 0:e93a11b4e044 670 timeoutEnv = 1;
mapellil 0:e93a11b4e044 671 }
mapellil 0:e93a11b4e044 672
mapellil 0:e93a11b4e044 673 static void Ticker_Calib(void)
mapellil 0:e93a11b4e044 674 {
mapellil 0:e93a11b4e044 675 timeoutCalib = 1;
mapellil 0:e93a11b4e044 676 }
mapellil 0:e93a11b4e044 677
mapellil 0:e93a11b4e044 678 static void Ticker_Mems(void)
mapellil 0:e93a11b4e044 679 {
mapellil 0:e93a11b4e044 680 TimeStamp++;
mapellil 0:e93a11b4e044 681 CounterAGM++;
mapellil 0:e93a11b4e044 682 timeoutMems = 1;
mapellil 0:e93a11b4e044 683 }
mapellil 0:e93a11b4e044 684
mapellil 0:e93a11b4e044 685
mapellil 0:e93a11b4e044 686 int main()
mapellil 0:e93a11b4e044 687 {
mapellil 0:e93a11b4e044 688 Ticker EnvTimer;
mapellil 0:e93a11b4e044 689 Ticker CalibTimer;
mapellil 0:e93a11b4e044 690 Ticker MemsTimer;
mapellil 0:e93a11b4e044 691 int32_t decPart, intPart;
mapellil 0:e93a11b4e044 692 int16_t TempToSend,HumToSend;
mapellil 0:e93a11b4e044 693 uint32_t PresToSend;
mapellil 0:e93a11b4e044 694 float temp,hum,pres;
mapellil 0:e93a11b4e044 695 AxesRaw_TypeDef Magn;
mapellil 0:e93a11b4e044 696 AxesRaw_TypeDef Acc;
mapellil 0:e93a11b4e044 697 AxesRaw_TypeDef Gyro;
mapellil 0:e93a11b4e044 698 bool isgyro_lsm6ds0Present=false;
mapellil 0:e93a11b4e044 699 bool isgyro_lsm6ds3Present=false;
mapellil 6:c1b8fb74072e 700
mapellil 0:e93a11b4e044 701 static bool SensorFusionOK = false;
mapellil 0:e93a11b4e044 702
mapellil 0:e93a11b4e044 703 DevI2C *i2c = new DevI2C(I2C_SDA, I2C_SCL);
mapellil 0:e93a11b4e044 704 i2c->frequency(NUCLEO_I2C_SHIELDS_SPEED);
mapellil 0:e93a11b4e044 705
mapellil 6:c1b8fb74072e 706 #ifdef CUST_BLEERR_MNGT
mapellil 6:c1b8fb74072e 707 p_customBleErrManagement = CustomBleErrManagement::Instance();
mapellil 6:c1b8fb74072e 708 #endif
mapellil 6:c1b8fb74072e 709
mapellil 0:e93a11b4e044 710 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 711 p_mems_expansion_board = X_NUCLEO_IKS01A1::Instance(i2c);
mapellil 0:e93a11b4e044 712 if (p_mems_expansion_board->gyro_lsm6ds0) isgyro_lsm6ds0Present=true;
mapellil 0:e93a11b4e044 713 if (p_mems_expansion_board->gyro_lsm6ds3) isgyro_lsm6ds3Present=true;
mapellil 0:e93a11b4e044 714 #endif
mapellil 0:e93a11b4e044 715 //#ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 716 InterruptIn BlueButton(USER_BUTTON);
mapellil 0:e93a11b4e044 717 BlueButton.fall(&BlueButtonPressed);
mapellil 0:e93a11b4e044 718 //#endif
mapellil 0:e93a11b4e044 719 printf("\r\nSTMicroelectronics BlueMicrosystem1 W2ST:\r\n"
mapellil 0:e93a11b4e044 720 #ifdef CUST_SENS_SERV
mapellil 2:d61a6f4d9682 721 "\tGyro lsmds0 present: %d, lsmds3 present: %d\n\r"
mapellil 0:e93a11b4e044 722 "\tSend Every %dmS Temperature/Humidity/Pressure\r\n"
mapellil 0:e93a11b4e044 723 "\tSend Every %dmS Acc/Gyro/Magneto\r\n\n"
mapellil 0:e93a11b4e044 724 #endif
mapellil 0:e93a11b4e044 725 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 726 ,"\tSend Every %dmS %d Quaternions\r\n"
mapellil 0:e93a11b4e044 727 "\tSend Every %dmS Float precision Quaternion\r\n"
mapellil 0:e93a11b4e044 728 #endif
mapellil 0:e93a11b4e044 729 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 730 ,isgyro_lsm6ds0Present,
mapellil 0:e93a11b4e044 731 isgyro_lsm6ds3Present,
mapellil 0:e93a11b4e044 732 ENV_TIMER/1000,
mapellil 0:e93a11b4e044 733 MEMS_TIMER/1000*ACC_GYRO_MAG_UPDATE_MUL_10MS
mapellil 0:e93a11b4e044 734 #endif
mapellil 0:e93a11b4e044 735 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 736 ,MEMS_TIMER/1000*SEND_N_QUATERNIONS,SEND_N_QUATERNIONS,
mapellil 0:e93a11b4e044 737 MEMS_TIMER/1000*QUAT_FLOAT_UPDATE_MUL_10MS
mapellil 0:e93a11b4e044 738 #endif
mapellil 0:e93a11b4e044 739 );
mapellil 0:e93a11b4e044 740
mapellil 0:e93a11b4e044 741
mapellil 0:e93a11b4e044 742 if (NUCLEO_I2C_SHIELDS_SPEED < MAX_I2C_SPEED)
mapellil 0:e93a11b4e044 743 printf ("Warning I2C speed set to: %d (max: %d)\n\r", NUCLEO_I2C_SHIELDS_SPEED, MAX_I2C_SPEED);
mapellil 0:e93a11b4e044 744
mapellil 0:e93a11b4e044 745 p_BLEdev = new BLE;
mapellil 0:e93a11b4e044 746 if (!p_BLEdev) {
mapellil 0:e93a11b4e044 747 printf("\r\nBLE Device creation failed\r\n");
mapellil 0:e93a11b4e044 748 return 0;
mapellil 0:e93a11b4e044 749 }
mapellil 0:e93a11b4e044 750 const Gap::Address_t BLE_address_BE = {BLE_DEV_MAC};
mapellil 0:e93a11b4e044 751 p_BLEdev->gap().setAddress(Gap::ADDR_TYPE_PUBLIC, BLE_address_BE);
mapellil 0:e93a11b4e044 752
mapellil 0:e93a11b4e044 753 p_BLEdev->init();
mapellil 0:e93a11b4e044 754
mapellil 0:e93a11b4e044 755 greenled = 0;//Switch OFF LED1
mapellil 0:e93a11b4e044 756
mapellil 0:e93a11b4e044 757 /* set the BLE CB functions */
mapellil 0:e93a11b4e044 758 p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
mapellil 0:e93a11b4e044 759 p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
mapellil 0:e93a11b4e044 760 p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback);
mapellil 0:e93a11b4e044 761 p_BLEdev->gattServer().onDataSent(onDataSentCallback);
mapellil 0:e93a11b4e044 762 p_BLEdev->gattServer().onDataRead(onDataReadCallback);
mapellil 6:c1b8fb74072e 763 p_BLEdev->gattServer().onDataWritten(myonDataWriteCallback);
mapellil 0:e93a11b4e044 764 p_BLEdev->gap().onConnection(onConnectionCallback);
mapellil 0:e93a11b4e044 765 p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
mapellil 0:e93a11b4e044 766 p_BLEdev->gap().onTimeout(onTimeoutCallback);
mapellil 0:e93a11b4e044 767
mapellil 0:e93a11b4e044 768
mapellil 0:e93a11b4e044 769 /* Initialize MotionFX library */
mapellil 0:e93a11b4e044 770 bool DS3_OnBoard = DS3_ONBOARD;
mapellil 0:e93a11b4e044 771 if (MotionFX_manager_init(DS3_OnBoard, magOffset)) {
mapellil 0:e93a11b4e044 772 MotionFX_manager_start_9X();
mapellil 0:e93a11b4e044 773 SensorFusionOK = true;
mapellil 0:e93a11b4e044 774 printf("SW Service sensor fusion library added successfully\r\n");
mapellil 0:e93a11b4e044 775 } else { /* sensor fusion lib init failed: likely because of wrong license */
mapellil 0:e93a11b4e044 776 printf("sensor fusion lib init failed: likely because of wrong license \n\r");
mapellil 0:e93a11b4e044 777 SensorFusionOK = false;
mapellil 0:e93a11b4e044 778 }
mapellil 0:e93a11b4e044 779
mapellil 0:e93a11b4e044 780 #ifdef CUST_CONFIG_SERV
mapellil 6:c1b8fb74072e 781 p_customconfigservice = new CustomConfigService(*p_BLEdev, p_customBleErrManagement);
mapellil 0:e93a11b4e044 782 if (!p_customconfigservice) {
mapellil 0:e93a11b4e044 783 printf("SW Service W2ST calibration add FAILED!\n\r");
mapellil 0:e93a11b4e044 784 return 0;
mapellil 0:e93a11b4e044 785 }
mapellil 0:e93a11b4e044 786 printf("SW Service W2ST calibration added successfully\r\n");
mapellil 0:e93a11b4e044 787 #endif
mapellil 0:e93a11b4e044 788 #ifdef CUST_CONS_SERV
mapellil 6:c1b8fb74072e 789 p_customconsoleservice = new CustomConsoleService(*p_BLEdev, p_customBleErrManagement);
mapellil 0:e93a11b4e044 790 if (!p_customconsoleservice) {
mapellil 0:e93a11b4e044 791 printf("\n\rHW Service W2ST console add FAILED!\n\r");
mapellil 0:e93a11b4e044 792 return 0;
mapellil 0:e93a11b4e044 793 }
mapellil 0:e93a11b4e044 794 printf("\rHW Service W2ST console added successfully\r\n");
mapellil 0:e93a11b4e044 795 #endif
mapellil 0:e93a11b4e044 796 #ifdef CUST_SENS_SERV
mapellil 6:c1b8fb74072e 797 p_customsensorservice = new CustomSensorService(*p_BLEdev, p_customBleErrManagement);
mapellil 0:e93a11b4e044 798 if (!p_customsensorservice) {
mapellil 0:e93a11b4e044 799 printf("\n\rHW Service W2ST sensors add FAILED!\n\r");
mapellil 0:e93a11b4e044 800 return 0;
mapellil 0:e93a11b4e044 801 }
mapellil 0:e93a11b4e044 802 printf("\rHW Service W2ST sensors added successfully\r\n");
mapellil 0:e93a11b4e044 803 #endif
mapellil 0:e93a11b4e044 804 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 805 AxesRaw_TypeDef quat_axes[SEND_N_QUATERNIONS];
mapellil 0:e93a11b4e044 806 float QuatFloat[SEND_N_QUATERNIONS];
mapellil 0:e93a11b4e044 807 if (SensorFusionOK) {
mapellil 6:c1b8fb74072e 808 p_customsoftwareservice = new CustomSoftwareService(*p_BLEdev, p_customBleErrManagement);
mapellil 0:e93a11b4e044 809 if (!p_customsoftwareservice) {
mapellil 0:e93a11b4e044 810 printf("SW Service W2ST quaternions add FAILED!\n\r");
mapellil 0:e93a11b4e044 811 return 0;
mapellil 0:e93a11b4e044 812 }
mapellil 0:e93a11b4e044 813 printf("SW Service W2ST quaternions added successfully\r\n");
mapellil 0:e93a11b4e044 814 }
mapellil 0:e93a11b4e044 815 #endif
mapellil 0:e93a11b4e044 816
mapellil 0:e93a11b4e044 817 const static char DEVICE_NAME[] = BLE_DEV_NAME;
mapellil 0:e93a11b4e044 818 /* Setup advertising. */
mapellil 0:e93a11b4e044 819 p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
mapellil 7:34014895dda8 820 p_BLEdev->gap().accumulateAdvertisingPayloadTxPower(8);
mapellil 7:34014895dda8 821 p_BLEdev->gap().setTxPower(8);
mapellil 7:34014895dda8 822
mapellil 0:e93a11b4e044 823 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 824 uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x01,0x80};
mapellil 0:e93a11b4e044 825 #else
mapellil 0:e93a11b4e044 826 uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x00,0x00};
mapellil 0:e93a11b4e044 827 #endif
mapellil 0:e93a11b4e044 828 p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);
mapellil 0:e93a11b4e044 829 p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
mapellil 0:e93a11b4e044 830 p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
mapellil 0:e93a11b4e044 831 p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
mapellil 0:e93a11b4e044 832 p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
mapellil 0:e93a11b4e044 833
mapellil 0:e93a11b4e044 834 printf("SERVER: BLE Stack Initialized\r\n");
mapellil 0:e93a11b4e044 835 printf("Starting Advertising...\n\r");
mapellil 0:e93a11b4e044 836 p_BLEdev->gap().startAdvertising();
mapellil 0:e93a11b4e044 837
mapellil 0:e93a11b4e044 838 TimeStamp =0;
mapellil 0:e93a11b4e044 839 EnvTimer.attach_us(&Ticker_Env, ENV_TIMER);
mapellil 0:e93a11b4e044 840 CalibTimer.attach_us(&Ticker_Calib, CALIB_TIMER );
mapellil 0:e93a11b4e044 841 MemsTimer.attach_us(&Ticker_Mems, MEMS_TIMER);
mapellil 0:e93a11b4e044 842
mapellil 0:e93a11b4e044 843 /* Control if the calibration is already available in memory */
mapellil 0:e93a11b4e044 844 if (SensorFusionOK) {
mapellil 0:e93a11b4e044 845 ReCallCalibrationFromMemory();
mapellil 0:e93a11b4e044 846
mapellil 0:e93a11b4e044 847 /* Switch on/off the LED according to calibration */
mapellil 0:e93a11b4e044 848 if(isCal) {
mapellil 0:e93a11b4e044 849 greenled =1;
mapellil 0:e93a11b4e044 850 } else {
mapellil 0:e93a11b4e044 851 greenled =0;
mapellil 0:e93a11b4e044 852 }
mapellil 0:e93a11b4e044 853 }
mapellil 2:d61a6f4d9682 854 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 855 uint32_t QuaternionNumber =0;
mapellil 0:e93a11b4e044 856 uint32_t CounterFloat =0;
mapellil 2:d61a6f4d9682 857 #endif
mapellil 0:e93a11b4e044 858
mapellil 0:e93a11b4e044 859 while(1) {
mapellil 0:e93a11b4e044 860
mapellil 0:e93a11b4e044 861 if (BLEConnectionStatus) {
mapellil 0:e93a11b4e044 862 if ((isBlueButtonSequence || ForceReCalibration) && SensorFusionOK ) {
mapellil 0:e93a11b4e044 863 printf("ForceReCalibration\r\n");
mapellil 0:e93a11b4e044 864 /* Reset the Compass Calibration */
mapellil 0:e93a11b4e044 865 isCal=0;
mapellil 0:e93a11b4e044 866 ForceReCalibration =0;
mapellil 0:e93a11b4e044 867 isBlueButtonSequence = false;
mapellil 0:e93a11b4e044 868 /* Notifications of Compass Calibration */
mapellil 0:e93a11b4e044 869 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 870 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 871 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 872 #endif
mapellil 0:e93a11b4e044 873 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 874 /* Reset the Calibration */
mapellil 0:e93a11b4e044 875 osx_MotionFX_compass_forceReCalibration();
mapellil 0:e93a11b4e044 876 #endif
mapellil 0:e93a11b4e044 877 ResetCalibrationInMemory();
mapellil 0:e93a11b4e044 878
mapellil 0:e93a11b4e044 879 /* Reset Calibration offset */
mapellil 0:e93a11b4e044 880 magOffset.magOffX = magOffset.magOffY= magOffset.magOffZ=0;
mapellil 0:e93a11b4e044 881 greenled =0;
mapellil 0:e93a11b4e044 882 }
mapellil 0:e93a11b4e044 883 }
mapellil 0:e93a11b4e044 884 // Update MEMS @ 100Hz and calculate quaternions
mapellil 0:e93a11b4e044 885 if (timeoutMems && BLEConnectionStatus) {
mapellil 0:e93a11b4e044 886 timeoutMems =0;
mapellil 0:e93a11b4e044 887
mapellil 0:e93a11b4e044 888 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 889 int32_t err;
mapellil 0:e93a11b4e044 890 err = p_mems_expansion_board->GetAccelerometer()->Get_X_Axes((int32_t *)&Acc);
mapellil 0:e93a11b4e044 891 if (err != IMU_6AXES_OK) {
mapellil 0:e93a11b4e044 892 printf ("= * ERROR %d GetAccelerometer\n\r", err);
mapellil 0:e93a11b4e044 893 }
mapellil 0:e93a11b4e044 894 err = p_mems_expansion_board->GetGyroscope()->Get_G_Axes((int32_t *)&Gyro);
mapellil 0:e93a11b4e044 895 if (err != IMU_6AXES_OK) {
mapellil 0:e93a11b4e044 896 printf ("= * ERROR %d GetGyroscope\n\r", err);
mapellil 0:e93a11b4e044 897 }
mapellil 0:e93a11b4e044 898 err = p_mems_expansion_board->magnetometer->Get_M_Axes((int32_t *)&Magn);
mapellil 0:e93a11b4e044 899 if (err != MAGNETO_OK) {
mapellil 0:e93a11b4e044 900 printf ("= * ERROR %d Get_M_Axes\n\r", err);
mapellil 0:e93a11b4e044 901 }
mapellil 0:e93a11b4e044 902 if(CounterAGM >= ACC_GYRO_MAG_UPDATE_MUL_10MS) {
mapellil 0:e93a11b4e044 903 CounterAGM=0;
mapellil 0:e93a11b4e044 904 p_customsensorservice->updateEnvAccelerometer (&Acc, TimeStamp);
mapellil 0:e93a11b4e044 905 p_customsensorservice->updateEnvGyroscope(&Gyro, TimeStamp);
mapellil 0:e93a11b4e044 906 p_customsensorservice->updateEnvMagnetometer(&Magn, TimeStamp, magOffset);
mapellil 0:e93a11b4e044 907 p_customsensorservice->updateEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, magOffset);
mapellil 0:e93a11b4e044 908 }
mapellil 0:e93a11b4e044 909 #endif
mapellil 0:e93a11b4e044 910
mapellil 0:e93a11b4e044 911 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 912 if (SensorFusionOK) {
mapellil 0:e93a11b4e044 913 MotionFX_manager_run((AxesRaw_t*)&Acc, (AxesRaw_t*)&Gyro, (AxesRaw_t*)&Magn, magOffset);
mapellil 0:e93a11b4e044 914 p_MotionFX_Engine_Out = MotionFX_manager_getDataOUT();
mapellil 0:e93a11b4e044 915 }
mapellil 0:e93a11b4e044 916 #endif
mapellil 0:e93a11b4e044 917 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 918 if (SensorFusionOK) {
mapellil 0:e93a11b4e044 919 /* Update quaternions */
mapellil 0:e93a11b4e044 920 if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) {
mapellil 0:e93a11b4e044 921 quat_axes[QuaternionNumber].AXIS_X = (int)(p_MotionFX_Engine_Out->quaternion_9X[0] * (-10000));
mapellil 0:e93a11b4e044 922 quat_axes[QuaternionNumber].AXIS_Y = (int)(p_MotionFX_Engine_Out->quaternion_9X[1] * (-10000));
mapellil 0:e93a11b4e044 923 quat_axes[QuaternionNumber].AXIS_Z = (int)(p_MotionFX_Engine_Out->quaternion_9X[2] * (-10000));
mapellil 0:e93a11b4e044 924 } else {
mapellil 0:e93a11b4e044 925 quat_axes[QuaternionNumber].AXIS_X = (int)(p_MotionFX_Engine_Out->quaternion_9X[0] * 10000);
mapellil 0:e93a11b4e044 926 quat_axes[QuaternionNumber].AXIS_Y = (int)(p_MotionFX_Engine_Out->quaternion_9X[1] * 10000);
mapellil 0:e93a11b4e044 927 quat_axes[QuaternionNumber].AXIS_Z = (int)(p_MotionFX_Engine_Out->quaternion_9X[2] * 10000);
mapellil 0:e93a11b4e044 928 }
mapellil 0:e93a11b4e044 929
mapellil 0:e93a11b4e044 930 if (QuaternionNumber == SEND_N_QUATERNIONS-1) {
mapellil 0:e93a11b4e044 931 p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp);
mapellil 0:e93a11b4e044 932 QuaternionNumber =0;
mapellil 0:e93a11b4e044 933 } else {
mapellil 0:e93a11b4e044 934 QuaternionNumber++;
mapellil 0:e93a11b4e044 935 }
mapellil 0:e93a11b4e044 936
mapellil 0:e93a11b4e044 937 /* Update Float Quaternions */
mapellil 0:e93a11b4e044 938 /* Every QUAT_FLOAT_UPDATE_MUL_10MS*10 mSeconds Send Float Quaternion informations via bluetooth */
mapellil 0:e93a11b4e044 939 if(CounterFloat==QUAT_FLOAT_UPDATE_MUL_10MS) {
mapellil 0:e93a11b4e044 940 if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) {
mapellil 0:e93a11b4e044 941 QuatFloat[0] = - p_MotionFX_Engine_Out->quaternion_9X[0];
mapellil 0:e93a11b4e044 942 QuatFloat[1] = - p_MotionFX_Engine_Out->quaternion_9X[1];
mapellil 0:e93a11b4e044 943 QuatFloat[2] = - p_MotionFX_Engine_Out->quaternion_9X[2];
mapellil 0:e93a11b4e044 944 } else {
mapellil 0:e93a11b4e044 945 QuatFloat[0] = p_MotionFX_Engine_Out->quaternion_9X[0];
mapellil 0:e93a11b4e044 946 QuatFloat[1] = p_MotionFX_Engine_Out->quaternion_9X[1];
mapellil 0:e93a11b4e044 947 QuatFloat[2] = p_MotionFX_Engine_Out->quaternion_9X[2];
mapellil 0:e93a11b4e044 948 }
mapellil 0:e93a11b4e044 949 p_customsoftwareservice->updateFloatQuaternions(QuatFloat, TimeStamp);
mapellil 0:e93a11b4e044 950 CounterFloat=0;
mapellil 0:e93a11b4e044 951 } else {
mapellil 0:e93a11b4e044 952 CounterFloat++;
mapellil 0:e93a11b4e044 953 }
mapellil 0:e93a11b4e044 954 }
mapellil 0:e93a11b4e044 955 #endif
mapellil 0:e93a11b4e044 956 }
mapellil 0:e93a11b4e044 957
mapellil 0:e93a11b4e044 958 /* Check if is calibrated @ 25Hz */
mapellil 0:e93a11b4e044 959 if(isCal!=0x01 && timeoutCalib && BLEConnectionStatus && SensorFusionOK ) {
mapellil 0:e93a11b4e044 960 timeoutCalib =0;
mapellil 0:e93a11b4e044 961 /* Run Compass Calibration @ 25Hz */
mapellil 0:e93a11b4e044 962 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 963 osx_MotionFX_compass_saveAcc(Acc.AXIS_X, Acc.AXIS_Y, Acc.AXIS_Z); /* Accelerometer data ENU systems coordinate */
mapellil 0:e93a11b4e044 964 osx_MotionFX_compass_saveMag(Magn.AXIS_X, Magn.AXIS_Y, Magn.AXIS_Z); /* Magnetometer data ENU systems coordinate */
mapellil 0:e93a11b4e044 965 osx_MotionFX_compass_run();
mapellil 0:e93a11b4e044 966
mapellil 0:e93a11b4e044 967 /* Control the calibration status */
mapellil 0:e93a11b4e044 968 isCal = osx_MotionFX_compass_isCalibrated();
mapellil 0:e93a11b4e044 969 #endif
mapellil 0:e93a11b4e044 970 if(isCal == 0x01) {
mapellil 0:e93a11b4e044 971
mapellil 0:e93a11b4e044 972 printf("Compass Calibrated\n\r");
mapellil 0:e93a11b4e044 973 /* Get new magnetometer offset */
mapellil 0:e93a11b4e044 974 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 975 osx_MotionFX_getCalibrationData(&magOffset);
mapellil 0:e93a11b4e044 976 #endif
mapellil 0:e93a11b4e044 977 /* Save the calibration in Memory */
mapellil 0:e93a11b4e044 978 SaveCalibrationToMemory();
mapellil 0:e93a11b4e044 979
mapellil 0:e93a11b4e044 980 /* Switch on the Led */
mapellil 0:e93a11b4e044 981 greenled = 1;
mapellil 0:e93a11b4e044 982
mapellil 0:e93a11b4e044 983 /* Notifications of Compass Calibration */
mapellil 0:e93a11b4e044 984 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 985 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 986 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 987 #endif
mapellil 0:e93a11b4e044 988 }
mapellil 0:e93a11b4e044 989 }
mapellil 0:e93a11b4e044 990
mapellil 0:e93a11b4e044 991 // Update environmental sensors @ 2Hz
mapellil 0:e93a11b4e044 992 if (timeoutEnv && BLEConnectionStatus) {
mapellil 0:e93a11b4e044 993 timeoutEnv =0;
mapellil 0:e93a11b4e044 994 int32_t err;
mapellil 0:e93a11b4e044 995 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 996 if (p_customsensorservice->isTempNotificationEn()) {
mapellil 0:e93a11b4e044 997 err = p_mems_expansion_board->ht_sensor->GetTemperature(&temp);
mapellil 0:e93a11b4e044 998 if ( err != HUM_TEMP_OK) {
mapellil 0:e93a11b4e044 999 printf ("= * ERROR %d GetTemperature\n\r", err);
mapellil 0:e93a11b4e044 1000 } else {
mapellil 0:e93a11b4e044 1001 floatToInt(temp, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 1002 TempToSend = intPart*10+decPart;
mapellil 0:e93a11b4e044 1003 p_customsensorservice->updateEnvTemperature(TempToSend, TimeStamp);
mapellil 0:e93a11b4e044 1004 }
mapellil 0:e93a11b4e044 1005 }
mapellil 0:e93a11b4e044 1006
mapellil 0:e93a11b4e044 1007 if (p_customsensorservice->isHumNotificationEn()) {
mapellil 0:e93a11b4e044 1008 err = p_mems_expansion_board->ht_sensor->GetHumidity(&hum);
mapellil 0:e93a11b4e044 1009 if ( err != HUM_TEMP_OK) {
mapellil 0:e93a11b4e044 1010 printf ("= * ERROR %d GetHumidity\n\r", err);
mapellil 0:e93a11b4e044 1011 } else {
mapellil 0:e93a11b4e044 1012 floatToInt(hum, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 1013 HumToSend = intPart*10+decPart;
mapellil 0:e93a11b4e044 1014 p_customsensorservice->updateEnvHumidity(HumToSend, TimeStamp);
mapellil 0:e93a11b4e044 1015 }
mapellil 0:e93a11b4e044 1016 }
mapellil 0:e93a11b4e044 1017
mapellil 0:e93a11b4e044 1018 if (p_customsensorservice->isPresNotificationEn()) {
mapellil 0:e93a11b4e044 1019 err = p_mems_expansion_board->pt_sensor->GetPressure(&pres);
mapellil 0:e93a11b4e044 1020 if ( err != PRESSURE_OK) {
mapellil 0:e93a11b4e044 1021 printf ("= * ERROR GetPressure\n\r");
mapellil 0:e93a11b4e044 1022 } else {
mapellil 0:e93a11b4e044 1023 floatToInt(pres, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 1024 PresToSend = intPart*100+decPart;
mapellil 0:e93a11b4e044 1025 p_customsensorservice->updateEnvPressure(PresToSend, TimeStamp);
mapellil 0:e93a11b4e044 1026 }
mapellil 0:e93a11b4e044 1027 }
mapellil 0:e93a11b4e044 1028 #endif
mapellil 0:e93a11b4e044 1029 }
mapellil 0:e93a11b4e044 1030 p_BLEdev->waitForEvent();
mapellil 0:e93a11b4e044 1031 }
mapellil 0:e93a11b4e044 1032 }