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

Dependencies:   HTS221 LIS3MDL LPS22HB LSM303AGR LSM6DSL

Fork of MOTENV_Mbed by ST Expansion SW Team

This application supports three different sets of ST hardware boards:

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

    and runs over four different target configurations:

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

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

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

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

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

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

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

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

to

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

and compile.

Committer:
mapellil
Date:
Wed Sep 20 17:08:56 2017 +0200
Revision:
20:b97e14ade434
Parent:
18:e2360c944484
Child:
24:b374c444d8ad
Updated mbed-os to 5.5 (w SPI3W), updated BLE, updated IKS (now allocating directly sensors),
added sensor fusion support, added env cumulative BLE characteristic.

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