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