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 Jun 17 14:04:14 2016 +0000
Revision:
11:ff82699c696e
Parent:
10:d19d8a60d3e5
Child:
12:baf8b5294320
Updated BLE (with mac addr self generated) and Mbed

Who changed what in which revision?

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