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:
Tue Oct 17 17:19:42 2017 +0200
Revision:
33:dec779879624
Parent:
32:f47005b845b4
Child:
34:00643b22fb56
Added support for DISCO_L475VG_IOT01A, code cleanup

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