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

Dependencies:   HTS221 LIS3MDL LPS22HB LSM303AGR LSM6DSL

Fork of MOTENV_Mbed by ST Expansion SW Team

This application supports three different sets of ST hardware boards:

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

    and runs over four different target configurations:

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

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

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

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

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

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

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

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

to

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

and compile.

Committer:
mapellil
Date:
Tue Oct 03 16:12:15 2017 +0000
Revision:
24:b374c444d8ad
Parent:
20:b97e14ade434
Child:
25:9e720d968dc0
Removed IKS lib, added separate sensor libs

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