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:
Mon Dec 21 13:05:16 2015 +0000
Revision:
8:b042fe719f51
Parent:
7:34014895dda8
Child:
9:2693f9ef8ff7
Reworked ble congestion control

Who changed what in which revision?

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