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

Dependencies:   HTS221 LIS3MDL LPS22HB LSM303AGR LSM6DSL

Fork of MOTENV_Mbed by ST Expansion SW Team

This application supports three different sets of ST hardware boards:

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

    and runs over four different target configurations:

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

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

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

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

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

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

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

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

to

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

and compile.

Committer:
mapellil
Date:
Fri Oct 13 16:49:50 2017 +0200
Revision:
32:f47005b845b4
Parent:
29:95acd2d877bc
Child:
33:dec779879624
Added support for Nucleo_401RE + IKS01A2 (added HTS221)
Changed BLE name to MotEnvMbedOS

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