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 06 14:37:46 2017 +0000
Revision:
25:9e720d968dc0
Parent:
24:b374c444d8ad
Child:
26:1bc571e52ad9
Added HTS221 sensor

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