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:
Thu Oct 12 13:53:52 2017 +0200
Revision:
29:95acd2d877bc
Parent:
27:dccb3b0fae57
Child:
32:f47005b845b4
Led flashing while BLE is disconnected. Led on if mag calibrated.

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