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:
Wed Dec 23 11:13:08 2015 +0000
Revision:
9:2693f9ef8ff7
Parent:
8:b042fe719f51
Child:
10:d19d8a60d3e5
Update BLE API and X-NUCLEO-IDB0XA1

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