Motion and Environmental sensor reader application connected via BLE to ST BlueMS iOS/Android application.

Dependencies:   HTS221 LIS3MDL LPS22HB LSM303AGR LSM6DSL

Fork of MOTENV_Mbed by ST Expansion SW Team

This application supports three different sets of ST hardware boards:

  • STEVAL-STLKT01V1 (aka SensorTile)
  • X-NUCLEO-IDB05A1 and X-NUCLEO-IKS01A2 expansion boards
  • B-L475E-IOT01A IoT Discovery board

    and runs over four different target configurations:

  • Nucleo F401RE + X-NUCLEO-IDB05A1 + X-NUCLEO-IKS01A2 (set target NUCLEO_F401RE)
  • DISCO_L475VG_IOT01A (set target DISCO_L475VG_IOT01A)
  • Nucleo L476RG + CRADLE + SENSORTILE (set compile target NUCLEO_L476RG)
  • Nucleo L476RG + CRADLE_EXPANSION_BOARD + SENSORTILE (set compile target NUCLEO_L476RG, remove macro MINI_CRADLE from mbed_app.json)

The first 2 configurations do not require any HW modifications (just use the above indicated targets).

Third configuration (CRADLE ) only requires to remove the two ST-LINK jumpers and JP6 from the Nucleo L476RG board in order to allow flashing the SensorTile through the Nucleo Jtag controller. Once flashed, if the battery is properly plugged and charged, the SensorTile could be mounted in the plastic enclosure being able to run as a small stand alone wearable device. Please note that this configuration do not provide a serial console for printf.

To enable last configuration (CRADLE_EXPANSION_BOARD), follow the steps below:

  • On Nucleo L476RG
    • open the two "ST-LINK" jumpers
    • open the MCU power supply jumper JP6
    • close the solder bridges SB63 and SB62 (to enable the serial console)
  • On SensorTile Arduino Cradle close the solder bridges SB21 and SB10 (to enable the serial console) and move the jumper J2 to the 5V position
  • Plug the Sensor Tile on the Arduino Cradle
  • Plug the Cradle on the Nucleo Arduino connector and connect the debug flat cable between Cradle and Nucleo Jtag connector (the cradle pin1 -identified by a dot- must be connected to the Nucleo pin1 (dot) of SWD CN4 jtag connector)
  • Plug the Nucleo USB cable on PC (a new COM port should appear); no need to plug the micro USB cable on the cradle.
  • Open a PC terminal to see the messages
  • Compile from mbed CLI or on-line compiler removing macro MINI_CRADLE from mbed_app.json file and selecting NUCLEO_ L476RG target
  • Flash the board with the binary

For all configurations on an Android or iOS device download and open the ST BlueMS application and connect to "MotEnvMbedOS" BLE device to see the sensor data.

For all configurations is also possible to add a 9 axis MotionFX sensor fusion library, which is part of the X-CUBE-MEMS package at this link.
The library comes in three flavours, choose your preferred according to the toolchain used (IAR, Keil or GC, Keil version should be used for the online compiler) and copy it in the Middlewares\ST\STM32_MotionFX_Library\Lib directory changing the file extension according to the toolchain used (.a for GCC, .ar for Keil).
In the file mbed_app.json add the macro definition "USE_SENSOR_FUSION_LIB" to the chosen target.
If compiling from CLI and using GCC_ARM toolchain option, in the file \mbed-os\tools\toolchains\gcc.py change the compiling option from

        if target.core == "Cortex-M4F":
            self.cpu.append("-mfpu=fpv4-sp-d16")
            self.cpu.append("-mfloat-abi=softfp")

to

        if target.core == "Cortex-M4F":
            self.cpu.append("-mfpu=fpv4-sp-d16")
            self.cpu.append("-mfloat-abi=hard")

and compile.

Committer:
mapellil
Date:
Fri Nov 20 09:55:58 2015 +0000
Revision:
4:007539036889
Parent:
2:d61a6f4d9682
Child:
6:c1b8fb74072e
Updated BLE API, IDB and MBED libs

Who changed what in which revision?

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