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

Dependencies:   HTS221 LIS3MDL LPS22HB LSM303AGR LSM6DSL

Fork of MOTENV_Mbed by ST Expansion SW Team

This application supports three different sets of ST hardware boards:

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

    and runs over four different target configurations:

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

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

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

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

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

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

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

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

to

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

and compile.

Committer:
mapellil
Date:
Thu Jun 08 09:25:00 2017 +0200
Revision:
16:b794ff225d9d
Parent:
13:d00147d01d4e
Child:
18:e2360c944484
Thread implementation of sensors (temp, press, acc, gyro, magn, hum HTS221 on mini cradle not managed ) reading and sending to BLE.
To be added: Calib, LED, Sens Fusion lib

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 8:b042fe719f51 39
mapellil 8:b042fe719f51 40 /*L0_BlueNRG_xx version main*/
mapellil 8:b042fe719f51 41
mapellil 0:e93a11b4e044 42 #include <cmath>
mapellil 0:e93a11b4e044 43 #include <math.h>
mapellil 0:e93a11b4e044 44 #include "mbed.h"
mapellil 0:e93a11b4e044 45 #include "Gap.h"
mapellil 0:e93a11b4e044 46 #include "debug.h"
mapellil 0:e93a11b4e044 47 #include "btle.h"
mapellil 0:e93a11b4e044 48 #include "blecommon.h"
mapellil 0:e93a11b4e044 49 #include "BLE.h"
mapellil 0:e93a11b4e044 50 #include "UUID.h"
mapellil 16:b794ff225d9d 51 //#include "Utils.h"
mapellil 0:e93a11b4e044 52 #include "MotionFX_Manager.h"
mapellil 0:e93a11b4e044 53 #include "InterruptManager.h"
mapellil 0:e93a11b4e044 54 #include "DevI2C.h"
mapellil 0:e93a11b4e044 55 #include "BlueNRGGattServer.h"
mapellil 0:e93a11b4e044 56 #include "GattCharacteristic.h"
mapellil 0:e93a11b4e044 57 #include "main.h" // contains the condt compiling configuration
mapellil 16:b794ff225d9d 58 #include "ble_utils.h"
mapellil 16:b794ff225d9d 59
mapellil 16:b794ff225d9d 60 #ifdef DEBUG
mapellil 16:b794ff225d9d 61 #include <stdio.h>
mapellil 16:b794ff225d9d 62 #define PRINTF(...) printf(__VA_ARGS__)
mapellil 16:b794ff225d9d 63 #else
mapellil 16:b794ff225d9d 64 #define PRINTF(...)
mapellil 16:b794ff225d9d 65 #endif
mapellil 0:e93a11b4e044 66
mapellil 0:e93a11b4e044 67 typedef struct {
mapellil 0:e93a11b4e044 68 int32_t AXIS_X;
mapellil 0:e93a11b4e044 69 int32_t AXIS_Y;
mapellil 0:e93a11b4e044 70 int32_t AXIS_Z;
mapellil 0:e93a11b4e044 71 } AxesRaw_TypeDef;
mapellil 0:e93a11b4e044 72
mapellil 0:e93a11b4e044 73 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 74 #include "CustomConsoleService.h"
mapellil 0:e93a11b4e044 75 #endif
mapellil 0:e93a11b4e044 76
mapellil 0:e93a11b4e044 77 #ifdef CUST_BATT_SERV
mapellil 0:e93a11b4e044 78 #include "CustomBatteryService.h"
mapellil 0:e93a11b4e044 79 #endif
mapellil 0:e93a11b4e044 80
mapellil 0:e93a11b4e044 81 #ifdef CUST_SENS_SERV
mapellil 16:b794ff225d9d 82 #include "XNucleoIKS01A2.h"
mapellil 0:e93a11b4e044 83 #include "CustomSensorsService.h"
mapellil 0:e93a11b4e044 84 #endif
mapellil 0:e93a11b4e044 85
mapellil 0:e93a11b4e044 86 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 87 #include "CustomConfigService.h"
mapellil 0:e93a11b4e044 88 #endif
mapellil 0:e93a11b4e044 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 0:e93a11b4e044 98 static unsigned char isCal = 0;
mapellil 0:e93a11b4e044 99 static uint16_t TimeStamp=0;
mapellil 0:e93a11b4e044 100 static int BLEConnectionStatus =0;
mapellil 0:e93a11b4e044 101 static bool ForceReCalibration =0;
mapellil 0:e93a11b4e044 102 static int HowManyButtonPress =0;
mapellil 0:e93a11b4e044 103 static uint32_t timeoutEnv =0;
mapellil 0:e93a11b4e044 104 static uint32_t timeoutCalib =0;
mapellil 0:e93a11b4e044 105 static uint32_t timeoutMems =0;
mapellil 0:e93a11b4e044 106 static BLE * p_BLEdev = NULL;
mapellil 0:e93a11b4e044 107 static int32_t CounterAGM =0;
mapellil 16:b794ff225d9d 108 static bool SensorFusionOK = false;
mapellil 16:b794ff225d9d 109
mapellil 16:b794ff225d9d 110 static EventQueue eventQueue(/* event count */ 16 * /* event size */ 32);
mapellil 16:b794ff225d9d 111 static Queue <char, 5>QueueMainActivity;
mapellil 16:b794ff225d9d 112
mapellil 16:b794ff225d9d 113 #define TMO_MEMS 1
mapellil 16:b794ff225d9d 114 #define TMO_CALIB 2
mapellil 16:b794ff225d9d 115 #define TMO_SENSOR_FUSION 3
mapellil 16:b794ff225d9d 116 #define TMO_ENV 4
mapellil 16:b794ff225d9d 117
mapellil 8:b042fe719f51 118
mapellil 0:e93a11b4e044 119 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 120 static CustomConsoleService * p_customconsoleservice = NULL;
mapellil 0:e93a11b4e044 121 #endif
mapellil 0:e93a11b4e044 122
mapellil 0:e93a11b4e044 123 #ifdef CUST_SENS_SERV
mapellil 16:b794ff225d9d 124 #define IMU_6AXES_OK 0
mapellil 16:b794ff225d9d 125 #define MAGNETO_OK 0
mapellil 16:b794ff225d9d 126 #define HUM_TEMP_OK 0
mapellil 16:b794ff225d9d 127 #define PRESSURE_OK 0
mapellil 0:e93a11b4e044 128 static CustomSensorService * p_customsensorservice = NULL;
mapellil 16:b794ff225d9d 129 //static X_NUCLEO_IKS01A1 * p_mems_expansion_board = NULL;
mapellil 16:b794ff225d9d 130 SPI3W sens_intf(PB_15, NC, PB_13); // 3W mosi, sclk on Nucleo L476 same as BTLE
mapellil 16:b794ff225d9d 131
mapellil 16:b794ff225d9d 132 LPS22HBSensor press_temp(&sens_intf, PA_3); // on SensorTile L476JG
mapellil 16:b794ff225d9d 133 LSM6DSLSensor acc_gyro(&sens_intf, PB_12, NC, PA_2 ); // on SensorTile L476JG
mapellil 16:b794ff225d9d 134 LSM303AGRMagSensor mag(&sens_intf, PB_1 ); //on SensorTile L476JG
mapellil 16:b794ff225d9d 135 LSM303AGRAccSensor acc(&sens_intf, PC_4 ); //on SensorTile L476JG
mapellil 16:b794ff225d9d 136
mapellil 0:e93a11b4e044 137 #endif
mapellil 0:e93a11b4e044 138
mapellil 0:e93a11b4e044 139 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 140 static CustomSoftwareService * p_customsoftwareservice = NULL;
mapellil 2:d61a6f4d9682 141 static osxMFX_output * p_MotionFX_Engine_Out = NULL;
mapellil 0:e93a11b4e044 142 #endif
mapellil 2:d61a6f4d9682 143 static osxMFX_calibFactor magOffset;
mapellil 0:e93a11b4e044 144
mapellil 0:e93a11b4e044 145 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 146 static CustomConfigService * p_customconfigservice = NULL;
mapellil 0:e93a11b4e044 147 #endif
mapellil 0:e93a11b4e044 148
mapellil 0:e93a11b4e044 149
mapellil 0:e93a11b4e044 150 /*************************** Calibration functions **************************/
mapellil 0:e93a11b4e044 151
mapellil 0:e93a11b4e044 152 /**
mapellil 0:e93a11b4e044 153 * @brief Check if there are a valid Calibration Values in Memory and read them
mapellil 0:e93a11b4e044 154 * @param None
mapellil 0:e93a11b4e044 155 * @retval unsigned char Success/Not Success
mapellil 0:e93a11b4e044 156 */
mapellil 0:e93a11b4e044 157 static unsigned char ReCallCalibrationFromMemory(void)
mapellil 0:e93a11b4e044 158 #ifdef BLUEMSYS_STORE_CALIB_FLASH
mapellil 0:e93a11b4e044 159 {
mapellil 0:e93a11b4e044 160 /* ReLoad the Calibration Values from FLASH */
mapellil 0:e93a11b4e044 161 unsigned char Success=1;
mapellil 0:e93a11b4e044 162 uint32_t Address = BLUEMSYS_FLASH_ADD;
mapellil 0:e93a11b4e044 163 __IO uint32_t data32 = *(__IO uint32_t*) Address;
mapellil 0:e93a11b4e044 164 if(data32== BLUEMSYS_CHECK_CALIBRATION) {
mapellil 0:e93a11b4e044 165 int32_t ReadIndex;
mapellil 0:e93a11b4e044 166 uint32_t ReadmagOffset[7];
mapellil 0:e93a11b4e044 167
mapellil 0:e93a11b4e044 168 for(ReadIndex=0; ReadIndex<7; ReadIndex++) {
mapellil 0:e93a11b4e044 169 Address +=4;
mapellil 0:e93a11b4e044 170 data32 = *(__IO uint32_t*) Address;
mapellil 0:e93a11b4e044 171 ReadmagOffset[ReadIndex]=data32;
mapellil 0:e93a11b4e044 172 }
mapellil 0:e93a11b4e044 173
mapellil 0:e93a11b4e044 174 magOffset.magOffX = (signed short) ReadmagOffset[0];
mapellil 0:e93a11b4e044 175 magOffset.magOffY = (signed short) ReadmagOffset[1];
mapellil 0:e93a11b4e044 176 magOffset.magOffZ = (signed short) ReadmagOffset[2];
mapellil 0:e93a11b4e044 177 magOffset.magGainX = *((float *) &(ReadmagOffset[3]));
mapellil 0:e93a11b4e044 178 magOffset.magGainY = *((float *) &(ReadmagOffset[4]));
mapellil 0:e93a11b4e044 179 magOffset.magGainZ = *((float *) &(ReadmagOffset[5]));
mapellil 0:e93a11b4e044 180 magOffset.expMagVect = *((float *) &(ReadmagOffset[6]));
mapellil 0:e93a11b4e044 181
mapellil 0:e93a11b4e044 182 #ifdef BLUEMSYS_DEBUG_CALIB
mapellil 0:e93a11b4e044 183 /* Debug */
mapellil 0:e93a11b4e044 184 printf("magOffX ->%d\r\n",magOffset.magOffX);
mapellil 0:e93a11b4e044 185 printf("magOffY ->%d\r\n",magOffset.magOffY);
mapellil 0:e93a11b4e044 186 printf("magOffZ ->%d\r\n",magOffset.magOffZ);
mapellil 0:e93a11b4e044 187 printf("magGainX ->%X\r\n",magOffset.magGainX);
mapellil 0:e93a11b4e044 188 printf("magGainY ->%X\r\n",magOffset.magGainY);
mapellil 0:e93a11b4e044 189 printf("magGainZ ->%X\r\n",magOffset.magGainZ);
mapellil 0:e93a11b4e044 190 printf("expMagVect->%X\r\n",magOffset.expMagVect);
mapellil 0:e93a11b4e044 191 #endif /* BLUEMSYS_DEBUG_CALIB */
mapellil 0:e93a11b4e044 192
mapellil 0:e93a11b4e044 193 /* Set the Calibration Structure */
mapellil 0:e93a11b4e044 194 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 195 osx_MotionFX_setCalibrationData(&magOffset);
mapellil 1:5fb861c45605 196
mapellil 0:e93a11b4e044 197 printf("Calibration Read from Flash\r\n");
mapellil 0:e93a11b4e044 198 /* Control the calibration status */
mapellil 0:e93a11b4e044 199 isCal = osx_MotionFX_compass_isCalibrated();
mapellil 0:e93a11b4e044 200 printf("Check the Calibration =%d\r\n",isCal);
mapellil 0:e93a11b4e044 201 #endif
mapellil 0:e93a11b4e044 202 } else {
mapellil 0:e93a11b4e044 203 printf("Calibration Not present in FLASH\r\n");
mapellil 0:e93a11b4e044 204 isCal=0;
mapellil 0:e93a11b4e044 205 }
mapellil 0:e93a11b4e044 206
mapellil 0:e93a11b4e044 207 return Success;
mapellil 0:e93a11b4e044 208 }
mapellil 0:e93a11b4e044 209 #else /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 210 {
mapellil 0:e93a11b4e044 211 /* ReLoad the Calibration Values from RAM */
mapellil 0:e93a11b4e044 212 unsigned char Success=1;
mapellil 0:e93a11b4e044 213
mapellil 0:e93a11b4e044 214 if(CalibrationStructureRAM[0]== BLUEMSYS_CHECK_CALIBRATION) {
mapellil 0:e93a11b4e044 215 magOffset.magOffX = (signed short) CalibrationStructureRAM[1];
mapellil 0:e93a11b4e044 216 magOffset.magOffY = (signed short) CalibrationStructureRAM[2];
mapellil 0:e93a11b4e044 217 magOffset.magOffZ = (signed short) CalibrationStructureRAM[3];
mapellil 0:e93a11b4e044 218 magOffset.magGainX = *((float *) &(CalibrationStructureRAM[4]));
mapellil 0:e93a11b4e044 219 magOffset.magGainY = *((float *) &(CalibrationStructureRAM[5]));
mapellil 0:e93a11b4e044 220 magOffset.magGainZ = *((float *) &(CalibrationStructureRAM[6]));
mapellil 0:e93a11b4e044 221 magOffset.expMagVect = *((float *) &(CalibrationStructureRAM[7]));
mapellil 0:e93a11b4e044 222
mapellil 0:e93a11b4e044 223 #ifdef BLUEMSYS_DEBUG_CALIB
mapellil 0:e93a11b4e044 224 /* Debug */
mapellil 0:e93a11b4e044 225 printf("magOffX ->%d\r\n",magOffset.magOffX);
mapellil 0:e93a11b4e044 226 printf("magOffY ->%d\r\n",magOffset.magOffY);
mapellil 0:e93a11b4e044 227 printf("magOffZ ->%d\r\n",magOffset.magOffZ);
mapellil 0:e93a11b4e044 228 printf("magGainX ->%X\r\n",magOffset.magGainX);
mapellil 0:e93a11b4e044 229 printf("magGainY ->%X\r\n",magOffset.magGainY);
mapellil 0:e93a11b4e044 230 printf("magGainZ ->%X\r\n",magOffset.magGainZ);
mapellil 0:e93a11b4e044 231 printf("expMagVect->%X\r\n",magOffset.expMagVect);
mapellil 0:e93a11b4e044 232 #endif /* BLUEMSYS_DEBUG_CALIB */
mapellil 0:e93a11b4e044 233
mapellil 1:5fb861c45605 234 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 235 /* Set the Calibration Structure */
mapellil 0:e93a11b4e044 236 osx_MotionFX_setCalibrationData(&magOffset);
mapellil 0:e93a11b4e044 237 printf("Calibration Read from RAM\r\n");
mapellil 0:e93a11b4e044 238
mapellil 0:e93a11b4e044 239 /* Control the calibration status */
mapellil 0:e93a11b4e044 240 isCal = osx_MotionFX_compass_isCalibrated();
mapellil 0:e93a11b4e044 241 printf("Check the Calibration =%d\r\n",isCal);
mapellil 0:e93a11b4e044 242 #endif
mapellil 0:e93a11b4e044 243 } else {
mapellil 0:e93a11b4e044 244 printf("Calibration Not present in RAM\r\n");
mapellil 0:e93a11b4e044 245 isCal=0;
mapellil 0:e93a11b4e044 246 }
mapellil 0:e93a11b4e044 247
mapellil 0:e93a11b4e044 248 return Success;
mapellil 0:e93a11b4e044 249 }
mapellil 0:e93a11b4e044 250 #endif /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 251
mapellil 0:e93a11b4e044 252 static unsigned char ResetCalibrationInMemory(void)
mapellil 0:e93a11b4e044 253 #ifdef BLUEMSYS_STORE_CALIB_FLASH
mapellil 0:e93a11b4e044 254 {
mapellil 0:e93a11b4e044 255 /* Reset Calibration Values in FLASH */
mapellil 0:e93a11b4e044 256 unsigned char Success=1;
mapellil 0:e93a11b4e044 257
mapellil 0:e93a11b4e044 258 /* Erase First Flash sector */
mapellil 0:e93a11b4e044 259 FLASH_EraseInitTypeDef EraseInitStruct;
mapellil 0:e93a11b4e044 260 uint32_t SectorError = 0;
mapellil 0:e93a11b4e044 261 EraseInitStruct.TypeErase = TYPEERASE_SECTORS;
mapellil 0:e93a11b4e044 262 EraseInitStruct.VoltageRange = VOLTAGE_RANGE_3;
mapellil 0:e93a11b4e044 263 EraseInitStruct.Sector = BLUEMSYS_FLASH_SECTOR;
mapellil 0:e93a11b4e044 264 EraseInitStruct.NbSectors = 1;
mapellil 0:e93a11b4e044 265
mapellil 0:e93a11b4e044 266 /* Unlock the Flash to enable the flash control register access *************/
mapellil 0:e93a11b4e044 267 HAL_FLASH_Unlock();
mapellil 0:e93a11b4e044 268
mapellil 0:e93a11b4e044 269 if (HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError) != HAL_OK) {
mapellil 0:e93a11b4e044 270 /*
mapellil 0:e93a11b4e044 271 Error occurred while sector erase.
mapellil 0:e93a11b4e044 272 User can add here some code to deal with this error.
mapellil 0:e93a11b4e044 273 SectorError will contain the faulty sector and then to know the code error on this sector,
mapellil 0:e93a11b4e044 274 user can call function 'HAL_FLASH_GetError()'
mapellil 0:e93a11b4e044 275 */
mapellil 0:e93a11b4e044 276 /*
mapellil 0:e93a11b4e044 277 FLASH_ErrorTypeDef errorcode = HAL_FLASH_GetError();
mapellil 0:e93a11b4e044 278 */
mapellil 0:e93a11b4e044 279
mapellil 0:e93a11b4e044 280 // Error_Handler();
mapellil 0:e93a11b4e044 281 Success=0;
mapellil 0:e93a11b4e044 282 } else {
mapellil 0:e93a11b4e044 283 printf("Erase calibration in FLASH Memory\r\n");
mapellil 0:e93a11b4e044 284 }
mapellil 0:e93a11b4e044 285
mapellil 0:e93a11b4e044 286 /* Lock the Flash to disable the flash control register access (recommended
mapellil 0:e93a11b4e044 287 to protect the FLASH memory against possible unwanted operation) *********/
mapellil 0:e93a11b4e044 288 HAL_FLASH_Lock();
mapellil 0:e93a11b4e044 289
mapellil 0:e93a11b4e044 290 return Success;
mapellil 0:e93a11b4e044 291 }
mapellil 0:e93a11b4e044 292 #else /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 293 {
mapellil 0:e93a11b4e044 294 /* Reset Calibration Values in RAM */
mapellil 0:e93a11b4e044 295 unsigned char Success=1;
mapellil 0:e93a11b4e044 296 int32_t Counter;
mapellil 0:e93a11b4e044 297
mapellil 0:e93a11b4e044 298 for(Counter=0; Counter<8; Counter++)
mapellil 0:e93a11b4e044 299 CalibrationStructureRAM[Counter]=0xFFFFFFFF;
mapellil 0:e93a11b4e044 300
mapellil 0:e93a11b4e044 301 printf("Erase Calibration in RAM Memory\r\n");
mapellil 0:e93a11b4e044 302 return Success;
mapellil 0:e93a11b4e044 303 }
mapellil 0:e93a11b4e044 304 #endif /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 305
mapellil 0:e93a11b4e044 306 /**
mapellil 0:e93a11b4e044 307 * @brief Save the Magnetometer Calibration Values to Memory
mapellil 0:e93a11b4e044 308 * @param None
mapellil 0:e93a11b4e044 309 * @retval unsigned char Success/Not Success
mapellil 0:e93a11b4e044 310 */
mapellil 0:e93a11b4e044 311 static unsigned char SaveCalibrationToMemory(void)
mapellil 0:e93a11b4e044 312 {
mapellil 0:e93a11b4e044 313 unsigned char Success=1;
mapellil 0:e93a11b4e044 314
mapellil 0:e93a11b4e044 315 /* Reset Before The data in Memory */
mapellil 0:e93a11b4e044 316 Success = ResetCalibrationInMemory();
mapellil 0:e93a11b4e044 317
mapellil 0:e93a11b4e044 318 if(Success)
mapellil 0:e93a11b4e044 319 #ifdef BLUEMSYS_STORE_CALIB_FLASH
mapellil 0:e93a11b4e044 320 {
mapellil 0:e93a11b4e044 321 /* Store in Flash Memory */
mapellil 0:e93a11b4e044 322 uint32_t Address = BLUEMSYS_FLASH_ADD;
mapellil 0:e93a11b4e044 323 uint32_t WritemagOffset[8];
mapellil 0:e93a11b4e044 324 int32_t WriteIndex;
mapellil 0:e93a11b4e044 325 WritemagOffset[0] = BLUEMSYS_CHECK_CALIBRATION;
mapellil 0:e93a11b4e044 326 WritemagOffset[1] = (uint32_t) magOffset.magOffX;
mapellil 0:e93a11b4e044 327 WritemagOffset[2] = (uint32_t) magOffset.magOffY;
mapellil 0:e93a11b4e044 328 WritemagOffset[3] = (uint32_t) magOffset.magOffZ;
mapellil 0:e93a11b4e044 329 WritemagOffset[4] = *((uint32_t *) &(magOffset.magGainX));
mapellil 0:e93a11b4e044 330 WritemagOffset[5] = *((uint32_t *) &(magOffset.magGainY));
mapellil 0:e93a11b4e044 331 WritemagOffset[6] = *((uint32_t *) &(magOffset.magGainZ));
mapellil 0:e93a11b4e044 332 WritemagOffset[7] = *((uint32_t *) &(magOffset.expMagVect));
mapellil 0:e93a11b4e044 333
mapellil 0:e93a11b4e044 334 /* Unlock the Flash to enable the flash control register access *************/
mapellil 0:e93a11b4e044 335 HAL_FLASH_Unlock();
mapellil 0:e93a11b4e044 336
mapellil 0:e93a11b4e044 337 for(WriteIndex=0; WriteIndex<8; WriteIndex++) {
mapellil 0:e93a11b4e044 338 if (HAL_FLASH_Program(TYPEPROGRAM_WORD, Address,WritemagOffset[WriteIndex]) == HAL_OK) {
mapellil 0:e93a11b4e044 339 Address = Address + 4;
mapellil 0:e93a11b4e044 340 } else {
mapellil 0:e93a11b4e044 341 /* Error occurred while writing data in Flash memory.
mapellil 0:e93a11b4e044 342 User can add here some code to deal with this error */
mapellil 0:e93a11b4e044 343 /*
mapellil 0:e93a11b4e044 344 FLASH_ErrorTypeDef errorcode = HAL_FLASH_GetError();
mapellil 0:e93a11b4e044 345 */
mapellil 0:e93a11b4e044 346 // Error_Handler();
mapellil 0:e93a11b4e044 347 }
mapellil 0:e93a11b4e044 348 }
mapellil 0:e93a11b4e044 349 #ifdef BLUEMSYS_DEBUG_CALIB
mapellil 0:e93a11b4e044 350 /* Debug */
mapellil 0:e93a11b4e044 351 printf("magOffX ->%d\r\n",magOffset.magOffX);
mapellil 0:e93a11b4e044 352 printf("magOffY ->%d\r\n",magOffset.magOffY);
mapellil 0:e93a11b4e044 353 printf("magOffZ ->%d\r\n",magOffset.magOffZ);
mapellil 0:e93a11b4e044 354 printf("magGainX ->%X\r\n",magOffset.magGainX);
mapellil 0:e93a11b4e044 355 printf("magGainY ->%X\r\n",magOffset.magGainY);
mapellil 0:e93a11b4e044 356 printf("magGainZ ->%X\r\n",magOffset.magGainZ);
mapellil 0:e93a11b4e044 357 printf("expMagVect->%X\r\n",magOffset.expMagVect);
mapellil 0:e93a11b4e044 358 #endif /* BLUEMSYS_DEBUG_CALIB */
mapellil 0:e93a11b4e044 359
mapellil 0:e93a11b4e044 360 printf("New Calibration Values Saved in FLASH\r\n");
mapellil 0:e93a11b4e044 361
mapellil 0:e93a11b4e044 362 /* Lock the Flash to disable the flash control register access (recommended
mapellil 0:e93a11b4e044 363 to protect the FLASH memory against possible unwanted operation) *********/
mapellil 0:e93a11b4e044 364 HAL_FLASH_Lock();
mapellil 0:e93a11b4e044 365 }
mapellil 0:e93a11b4e044 366 #else /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 367 {
mapellil 0:e93a11b4e044 368 /* Store in RAM */
mapellil 0:e93a11b4e044 369 CalibrationStructureRAM[0] = BLUEMSYS_CHECK_CALIBRATION;
mapellil 0:e93a11b4e044 370 CalibrationStructureRAM[1] = (uint32_t) magOffset.magOffX;
mapellil 0:e93a11b4e044 371 CalibrationStructureRAM[2] = (uint32_t) magOffset.magOffY;
mapellil 0:e93a11b4e044 372 CalibrationStructureRAM[3] = (uint32_t) magOffset.magOffZ;
mapellil 0:e93a11b4e044 373 CalibrationStructureRAM[4] = *((uint32_t *) &(magOffset.magGainX));
mapellil 0:e93a11b4e044 374 CalibrationStructureRAM[5] = *((uint32_t *) &(magOffset.magGainY));
mapellil 0:e93a11b4e044 375 CalibrationStructureRAM[6] = *((uint32_t *) &(magOffset.magGainZ));
mapellil 0:e93a11b4e044 376 CalibrationStructureRAM[7] = *((uint32_t *) &(magOffset.expMagVect));
mapellil 0:e93a11b4e044 377
mapellil 0:e93a11b4e044 378 #ifdef BLUEMSYS_DEBUG_CALIB
mapellil 0:e93a11b4e044 379 /* Debug */
mapellil 0:e93a11b4e044 380 printf("magOffX ->%d\r\n",magOffset.magOffX);
mapellil 0:e93a11b4e044 381 printf("magOffY ->%d\r\n",magOffset.magOffY);
mapellil 0:e93a11b4e044 382 printf("magOffZ ->%d\r\n",magOffset.magOffZ);
mapellil 0:e93a11b4e044 383 printf("magGainX ->%X\r\n",magOffset.magGainX);
mapellil 0:e93a11b4e044 384 printf("magGainY ->%X\r\n",magOffset.magGainY);
mapellil 0:e93a11b4e044 385 printf("magGainZ ->%X\r\n",magOffset.magGainZ);
mapellil 0:e93a11b4e044 386 printf("expMagVect->%X\r\n",magOffset.expMagVect);
mapellil 0:e93a11b4e044 387 #endif /* BLUEMSYS_DEBUG_CALIB */
mapellil 0:e93a11b4e044 388
mapellil 0:e93a11b4e044 389 printf("New Calibration Values Saved in RAM\r\n");
mapellil 0:e93a11b4e044 390 }
mapellil 0:e93a11b4e044 391 #endif /* BLUEMSYS_STORE_CALIB_FLASH */
mapellil 0:e93a11b4e044 392
mapellil 0:e93a11b4e044 393 return Success;
mapellil 0:e93a11b4e044 394 }
mapellil 0:e93a11b4e044 395
mapellil 0:e93a11b4e044 396 /*************************** End Calibration functions **************************/
mapellil 0:e93a11b4e044 397
mapellil 0:e93a11b4e044 398 static void floatToInt(float in, int32_t *out_int, int32_t *out_dec, int32_t dec_prec)
mapellil 0:e93a11b4e044 399 {
mapellil 0:e93a11b4e044 400 *out_int = (int32_t)in;
mapellil 0:e93a11b4e044 401 in = in - (float)(*out_int);
mapellil 0:e93a11b4e044 402 // *out_dec = (int32_t)trunc(in*pow((float)10,(int)dec_prec));
mapellil 0:e93a11b4e044 403 *out_dec = (int32_t)(float)(int)(in*pow((float)10,(int)dec_prec));
mapellil 0:e93a11b4e044 404
mapellil 0:e93a11b4e044 405 }
mapellil 0:e93a11b4e044 406
mapellil 0:e93a11b4e044 407 static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
mapellil 0:e93a11b4e044 408 {
mapellil 4:007539036889 409 PRINTF("onUpdatesEnabledCallback! handle: %x\n\r", handle);
mapellil 16:b794ff225d9d 410 printf("onUpdatesEnabledCallback! handle: %x\n\r", handle);
mapellil 0:e93a11b4e044 411 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 412 if (p_customsensorservice)
mapellil 0:e93a11b4e044 413 p_customsensorservice->enNotify (handle);
mapellil 0:e93a11b4e044 414 #endif
mapellil 8:b042fe719f51 415
mapellil 0:e93a11b4e044 416 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 417 if (p_customsoftwareservice)
mapellil 0:e93a11b4e044 418 p_customsoftwareservice->enNotify (handle);
mapellil 0:e93a11b4e044 419 #endif
mapellil 8:b042fe719f51 420
mapellil 0:e93a11b4e044 421 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 422 if (p_customconfigservice) {
mapellil 0:e93a11b4e044 423 p_customconfigservice->enNotify (handle);
mapellil 0:e93a11b4e044 424 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 425 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 426 }
mapellil 0:e93a11b4e044 427 #endif
mapellil 0:e93a11b4e044 428 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 429 if (p_customconsoleservice)
mapellil 0:e93a11b4e044 430 p_customconsoleservice->enNotify (handle);
mapellil 0:e93a11b4e044 431 #endif
mapellil 8:b042fe719f51 432 /* TODO inform other obj implementing BLE services */
mapellil 0:e93a11b4e044 433 }
mapellil 0:e93a11b4e044 434
mapellil 0:e93a11b4e044 435 static void onUpdatesDisabledCallback(Gap::Handle_t handle)
mapellil 0:e93a11b4e044 436 {
mapellil 4:007539036889 437 PRINTF("onUpdatesDisabledCallback! handle: %x\n\r", handle);
mapellil 0:e93a11b4e044 438 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 439 if (p_customsensorservice)
mapellil 0:e93a11b4e044 440 p_customsensorservice->disNotify (handle);
mapellil 0:e93a11b4e044 441 #endif
mapellil 8:b042fe719f51 442
mapellil 0:e93a11b4e044 443 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 444 if (p_customsoftwareservice)
mapellil 0:e93a11b4e044 445 p_customsoftwareservice->disNotify (handle);
mapellil 0:e93a11b4e044 446 #endif
mapellil 8:b042fe719f51 447
mapellil 0:e93a11b4e044 448 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 449 if (p_customconfigservice)
mapellil 0:e93a11b4e044 450 p_customconfigservice->disNotify (handle);
mapellil 0:e93a11b4e044 451 #endif
mapellil 0:e93a11b4e044 452 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 453 if (p_customconsoleservice)
mapellil 0:e93a11b4e044 454 p_customconsoleservice->disNotify (handle);
mapellil 0:e93a11b4e044 455 #endif
mapellil 8:b042fe719f51 456 /* TODO inform other obj implementing BLE services */
mapellil 0:e93a11b4e044 457 }
mapellil 0:e93a11b4e044 458
mapellil 0:e93a11b4e044 459 //static void onDisconnectionCallback(const Gap::Handle_t handle, const Gap::DisconnectionReason_t disConnectionReason)
mapellil 0:e93a11b4e044 460 static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
mapellil 0:e93a11b4e044 461 {
mapellil 0:e93a11b4e044 462 printf("Disconnected\n\r");
mapellil 0:e93a11b4e044 463 printf("Restarting the advertising process\n\r");
mapellil 0:e93a11b4e044 464 TimeStamp =0;
mapellil 0:e93a11b4e044 465 BLEConnectionStatus =0;
mapellil 0:e93a11b4e044 466 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 467 if (p_customsensorservice)
mapellil 0:e93a11b4e044 468 p_customsensorservice->updateConnectionStatus(DISCONNECTED);
mapellil 0:e93a11b4e044 469 #endif
mapellil 0:e93a11b4e044 470 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 471 if (p_customsoftwareservice)
mapellil 0:e93a11b4e044 472 p_customsoftwareservice->updateConnectionStatus(DISCONNECTED);
mapellil 0:e93a11b4e044 473 #endif
mapellil 0:e93a11b4e044 474 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 475 if (p_customconfigservice)
mapellil 0:e93a11b4e044 476 p_customconfigservice->updateConnectionStatus(DISCONNECTED);
mapellil 0:e93a11b4e044 477 #endif
mapellil 0:e93a11b4e044 478 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 479 if (p_customconsoleservice)
mapellil 0:e93a11b4e044 480 p_customconsoleservice->updateConnectionStatus(DISCONNECTED);
mapellil 0:e93a11b4e044 481 #endif
mapellil 0:e93a11b4e044 482 if (p_BLEdev)
mapellil 0:e93a11b4e044 483 p_BLEdev->startAdvertising();
mapellil 0:e93a11b4e044 484 }
mapellil 0:e93a11b4e044 485
mapellil 0:e93a11b4e044 486 static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
mapellil 0:e93a11b4e044 487 {
mapellil 0:e93a11b4e044 488 printf("\rConnected: %x", connectionParams->peerAddr[Gap::ADDR_LEN-1]);
mapellil 0:e93a11b4e044 489 for(int i=Gap::ADDR_LEN-2; i>=0; i--) printf(":%x", connectionParams->peerAddr[i]);
mapellil 0:e93a11b4e044 490 printf("\n\r");
mapellil 0:e93a11b4e044 491
mapellil 0:e93a11b4e044 492 TimeStamp =0;
mapellil 0:e93a11b4e044 493 BLEConnectionStatus =1;
mapellil 0:e93a11b4e044 494 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 495 if (p_customsensorservice)
mapellil 0:e93a11b4e044 496 p_customsensorservice->updateConnectionStatus(CONNECTED);
mapellil 0:e93a11b4e044 497 #endif
mapellil 0:e93a11b4e044 498 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 499 if (p_customsoftwareservice)
mapellil 0:e93a11b4e044 500 p_customsoftwareservice->updateConnectionStatus(CONNECTED);
mapellil 0:e93a11b4e044 501 #endif
mapellil 0:e93a11b4e044 502 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 503 if (p_customconfigservice)
mapellil 0:e93a11b4e044 504 p_customconfigservice->updateConnectionStatus(CONNECTED);
mapellil 0:e93a11b4e044 505 #endif
mapellil 0:e93a11b4e044 506 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 507 if (p_customconsoleservice)
mapellil 0:e93a11b4e044 508 p_customconsoleservice->updateConnectionStatus(CONNECTED);
mapellil 0:e93a11b4e044 509 #endif
mapellil 0:e93a11b4e044 510 }
mapellil 0:e93a11b4e044 511
mapellil 0:e93a11b4e044 512 static void onDataReadCallback(const GattReadCallbackParams *eventDataP)
mapellil 0:e93a11b4e044 513 {
mapellil 0:e93a11b4e044 514 float temp, hum, pres;
mapellil 0:e93a11b4e044 515 int16_t TempToSend, HumToSend;
mapellil 0:e93a11b4e044 516 uint32_t PresToSend;
mapellil 0:e93a11b4e044 517 int32_t decPart, intPart;
mapellil 0:e93a11b4e044 518 AxesRaw_TypeDef Magn, Acc, Gyro;
mapellil 13:d00147d01d4e 519 Gap::Handle_t handle = eventDataP->handle - BLE_HANDLE_VALUE_OFFSET;
mapellil 0:e93a11b4e044 520
mapellil 0:e93a11b4e044 521 #ifdef CUST_SENS_SERV
mapellil 12:baf8b5294320 522 if (p_customsensorservice->isTempHandle(handle)) {
mapellil 16:b794ff225d9d 523 // p_mems_expansion_board->ht_sensor->GetTemperature(&temp);
mapellil 16:b794ff225d9d 524 press_temp.get_temperature(&temp);
mapellil 0:e93a11b4e044 525 floatToInt(temp, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 526 TempToSend = intPart*10+decPart;
mapellil 0:e93a11b4e044 527 p_customsensorservice->sendEnvTemperature(TempToSend, TimeStamp);
mapellil 0:e93a11b4e044 528
mapellil 12:baf8b5294320 529 } else if (p_customsensorservice->isHumHandle(handle)) {
mapellil 16:b794ff225d9d 530 // p_mems_expansion_board->ht_sensor->GetHumidity(&hum); /** FIXME Hum not present */
mapellil 0:e93a11b4e044 531 floatToInt(hum, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 532 HumToSend = intPart*10+decPart;
mapellil 0:e93a11b4e044 533 p_customsensorservice->sendEnvHumidity(HumToSend, TimeStamp);
mapellil 0:e93a11b4e044 534
mapellil 12:baf8b5294320 535 } else if (p_customsensorservice->isPresHandle(handle)) {
mapellil 16:b794ff225d9d 536 // p_mems_expansion_board->pt_sensor->GetPressure(&pres);
mapellil 16:b794ff225d9d 537 press_temp.get_pressure(&pres);
mapellil 0:e93a11b4e044 538 floatToInt(pres, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 539 PresToSend = intPart*100+decPart;
mapellil 0:e93a11b4e044 540 p_customsensorservice->sendEnvPressure(PresToSend, TimeStamp);
mapellil 0:e93a11b4e044 541
mapellil 12:baf8b5294320 542 } else if (p_customsensorservice->isGyroHandle(handle)) {
mapellil 16:b794ff225d9d 543 // p_mems_expansion_board->GetGyroscope()->Get_G_Axes((int32_t *)&Gyro);
mapellil 16:b794ff225d9d 544 acc_gyro.get_g_axes((int32_t *)&Gyro);
mapellil 0:e93a11b4e044 545 p_customsensorservice->sendEnvGyroscope (&Gyro, TimeStamp);
mapellil 0:e93a11b4e044 546
mapellil 12:baf8b5294320 547 } else if (p_customsensorservice->isAccHandle(handle)) {
mapellil 16:b794ff225d9d 548 // p_mems_expansion_board->GetAccelerometer()->Get_X_Axes((int32_t *)&Acc);
mapellil 16:b794ff225d9d 549 acc_gyro.get_x_axes((int32_t *)&Acc);
mapellil 0:e93a11b4e044 550 p_customsensorservice->sendEnvAccelerometer (&Acc, TimeStamp);
mapellil 0:e93a11b4e044 551
mapellil 12:baf8b5294320 552 } else if (p_customsensorservice->isMagHandle(handle)) {
mapellil 16:b794ff225d9d 553 // p_mems_expansion_board->magnetometer->Get_M_Axes((int32_t *)&Magn);
mapellil 16:b794ff225d9d 554 mag.get_m_axes((int32_t *)&Magn);
mapellil 0:e93a11b4e044 555 p_customsensorservice->sendEnvMagnetometer(&Magn, TimeStamp, magOffset);
mapellil 0:e93a11b4e044 556
mapellil 12:baf8b5294320 557 } else if (p_customsensorservice->isAccGyroMagHandle(handle)) {
mapellil 0:e93a11b4e044 558 p_customsensorservice->sendEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, magOffset);
mapellil 0:e93a11b4e044 559 }
mapellil 0:e93a11b4e044 560 #endif
mapellil 0:e93a11b4e044 561
mapellil 0:e93a11b4e044 562 #ifdef CUST_SW_SERV
mapellil 12:baf8b5294320 563 if (p_customsoftwareservice->isQuatHandle(handle)) {
mapellil 0:e93a11b4e044 564 // p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp); // dont need to update because already calculated/update into main loop
mapellil 0:e93a11b4e044 565
mapellil 12:baf8b5294320 566 } else if (p_customsoftwareservice->isFloatQuatHandle(handle)) {
mapellil 0:e93a11b4e044 567 // p_customsoftwareservice->updateFloatQuaternions(QuatFloat, TimeStamp); // dont need to update because already calculated/update into main loop
mapellil 0:e93a11b4e044 568 }
mapellil 0:e93a11b4e044 569 #endif
mapellil 0:e93a11b4e044 570
mapellil 0:e93a11b4e044 571 #ifdef CUST_CONFIG_SERV
mapellil 12:baf8b5294320 572 if (p_customconfigservice->isConfHandle(handle)) {
mapellil 0:e93a11b4e044 573 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 574 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 575 }
mapellil 0:e93a11b4e044 576 #endif
mapellil 0:e93a11b4e044 577 }
mapellil 0:e93a11b4e044 578
mapellil 13:d00147d01d4e 579 static void onDataWriteCallback(const GattWriteCallbackParams *eventDataP)
mapellil 0:e93a11b4e044 580 {
mapellil 13:d00147d01d4e 581 Gap::Handle_t handle = eventDataP->handle - BLE_HANDLE_VALUE_OFFSET;
mapellil 4:007539036889 582 PRINTF("onEventCallback!!\n\r");
mapellil 12:baf8b5294320 583 printf (" myonDataWriteCallback attr_handle: %x att_data[4]: %d data_length: %d\n\r", eventDataP->handle, eventDataP->data[4], eventDataP->len );
mapellil 0:e93a11b4e044 584
mapellil 0:e93a11b4e044 585
mapellil 0:e93a11b4e044 586 #ifdef CUST_CONFIG_SERV
mapellil 12:baf8b5294320 587 if (p_customconfigservice->isConfHandle(handle)) {
mapellil 0:e93a11b4e044 588 /* Received one write command from Client on calibration characteristc */
mapellil 0:e93a11b4e044 589 uint32_t FeatureMask = (eventDataP->data[3]) | (eventDataP->data[2]<<8) | (eventDataP->data[1]<<16) | (eventDataP->data[0]<<24);
mapellil 12:baf8b5294320 590 uint8_t Command = eventDataP->data[4];
mapellil 0:e93a11b4e044 591 switch (Command) {
mapellil 0:e93a11b4e044 592 case W2ST_COMMAND_CAL_STATUS:
mapellil 0:e93a11b4e044 593
mapellil 0:e93a11b4e044 594 /* Replay with the calibration status for the feature */
mapellil 0:e93a11b4e044 595 // Config_Notify(FeatureMask,Command,isCal);
mapellil 0:e93a11b4e044 596 p_customconfigservice->sendConfigState(FeatureMask, Command, isCal, TimeStamp);
mapellil 0:e93a11b4e044 597 break;
mapellil 0:e93a11b4e044 598 case W2ST_COMMAND_CAL_RESET:
mapellil 0:e93a11b4e044 599 /* Reset the calibration */
mapellil 0:e93a11b4e044 600 ForceReCalibration=1;
mapellil 0:e93a11b4e044 601 break;
mapellil 0:e93a11b4e044 602 case W2ST_COMMAND_CAL_STOP:
mapellil 0:e93a11b4e044 603
mapellil 0:e93a11b4e044 604 /* Do nothing in this case */
mapellil 0:e93a11b4e044 605 break;
mapellil 0:e93a11b4e044 606 default:
mapellil 0:e93a11b4e044 607 // if(StdErrNotification){
mapellil 0:e93a11b4e044 608 // BytesToWrite = sprintf((char *)BufferToWrite, "Calibration UNKNOW Signal For Features=%x\n\r",FeatureMask);
mapellil 0:e93a11b4e044 609 // Stderr_Update(BufferToWrite,BytesToWrite);
mapellil 0:e93a11b4e044 610 // } else {
mapellil 0:e93a11b4e044 611 // printf("Calibration UNKNOW Signal For Features=%x\n\r",FeatureMask);
mapellil 0:e93a11b4e044 612 // }
mapellil 0:e93a11b4e044 613 break;
mapellil 0:e93a11b4e044 614 }
mapellil 0:e93a11b4e044 615
mapellil 0:e93a11b4e044 616 }
mapellil 0:e93a11b4e044 617 #endif
mapellil 0:e93a11b4e044 618 #ifdef CUST_CONS_SERV
mapellil 12:baf8b5294320 619 if (p_customconsoleservice->isTermHandle(handle)) {
mapellil 12:baf8b5294320 620 printf ("Console handle: %x data: %s\n\r", handle, eventDataP->data);
mapellil 12:baf8b5294320 621 // p_customconsoleservice->updateTerm((uint8_t*)eventDataP->data,eventDataP->len);
mapellil 12:baf8b5294320 622 }
mapellil 0:e93a11b4e044 623 #endif
mapellil 0:e93a11b4e044 624
mapellil 0:e93a11b4e044 625
mapellil 0:e93a11b4e044 626 }
mapellil 0:e93a11b4e044 627
mapellil 0:e93a11b4e044 628 static void onDataSentCallback(unsigned count)
mapellil 0:e93a11b4e044 629 {
mapellil 4:007539036889 630 PRINTF("onDataSentCallback!!\n\r");
mapellil 0:e93a11b4e044 631 }
mapellil 0:e93a11b4e044 632
mapellil 0:e93a11b4e044 633
mapellil 0:e93a11b4e044 634 static void onTimeoutCallback(Gap::TimeoutSource_t source)
mapellil 0:e93a11b4e044 635 {
mapellil 4:007539036889 636 PRINTF("onTimeoutCallback!!\n\r");
mapellil 0:e93a11b4e044 637 }
mapellil 0:e93a11b4e044 638
mapellil 0:e93a11b4e044 639 static void onConfirmationReceivedCallback(uint16_t attributeHandle)
mapellil 0:e93a11b4e044 640 {
mapellil 4:007539036889 641 PRINTF("onConfirmationReceivedCallback!!\n\r");
mapellil 0:e93a11b4e044 642 }
mapellil 0:e93a11b4e044 643
mapellil 0:e93a11b4e044 644
mapellil 0:e93a11b4e044 645 static void Ticker_BlueButton(void)
mapellil 0:e93a11b4e044 646 {
mapellil 0:e93a11b4e044 647 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 648 static uint8_t BufferToWrite[W2ST_CONSOLE_MAX_CHAR_LEN];
mapellil 0:e93a11b4e044 649 static uint8_t BytesToWrite;
mapellil 0:e93a11b4e044 650 #endif
mapellil 0:e93a11b4e044 651
mapellil 0:e93a11b4e044 652 BlueButtonTimer.detach();
mapellil 0:e93a11b4e044 653 printf (" butt time expired \n\r");
mapellil 0:e93a11b4e044 654 #ifdef CUST_CONS_SERV
mapellil 0:e93a11b4e044 655 BytesToWrite = sprintf((char *)BufferToWrite, "nr%d push in %1.1fs\r\n",HowManyButtonPress, BUTTON_TIME);
mapellil 0:e93a11b4e044 656 p_customconsoleservice->updateTerm(BufferToWrite,BytesToWrite);
mapellil 0:e93a11b4e044 657 #endif
mapellil 0:e93a11b4e044 658 isBlueButtonSequence = false;
mapellil 0:e93a11b4e044 659 HowManyButtonPress =0;
mapellil 0:e93a11b4e044 660
mapellil 0:e93a11b4e044 661 }
mapellil 0:e93a11b4e044 662
mapellil 0:e93a11b4e044 663 /**
mapellil 0:e93a11b4e044 664 * CB Triggered by the user blue button press;
mapellil 0:e93a11b4e044 665 */
mapellil 0:e93a11b4e044 666 static void BlueButtonPressed ()
mapellil 0:e93a11b4e044 667 {
mapellil 0:e93a11b4e044 668 //printf (" CB BlueButtonPressed PRESSED!!!!!!!!! %d\n\r", HowManyButtonPress);
mapellil 0:e93a11b4e044 669 if (HowManyButtonPress == 0) { // first push
mapellil 0:e93a11b4e044 670 BlueButtonTimer.attach(&Ticker_BlueButton, BUTTON_TIME);
mapellil 0:e93a11b4e044 671 HowManyButtonPress++;
mapellil 0:e93a11b4e044 672 isBlueButtonSequence = false;
mapellil 0:e93a11b4e044 673 } else {
mapellil 0:e93a11b4e044 674 HowManyButtonPress++;
mapellil 0:e93a11b4e044 675 if (HowManyButtonPress == BLUEMSYS_N_BUTTON_PRESS ) {
mapellil 0:e93a11b4e044 676 BlueButtonTimer.detach();
mapellil 0:e93a11b4e044 677 printf (" CB BlueButtoon SEQUENCE!!!!!!!!! \n\r");
mapellil 0:e93a11b4e044 678 HowManyButtonPress =0;
mapellil 0:e93a11b4e044 679 if (BLEConnectionStatus) isBlueButtonSequence = true;
mapellil 0:e93a11b4e044 680 }
mapellil 0:e93a11b4e044 681 }
mapellil 0:e93a11b4e044 682 }
mapellil 0:e93a11b4e044 683
mapellil 0:e93a11b4e044 684 /**
mapellil 0:e93a11b4e044 685 * CB Triggered periodically by the 'ticker' interrupt;
mapellil 0:e93a11b4e044 686 */
mapellil 0:e93a11b4e044 687 static void Ticker_Env(void)
mapellil 0:e93a11b4e044 688 {
mapellil 16:b794ff225d9d 689 char msg;
mapellil 16:b794ff225d9d 690 msg = TMO_ENV;
mapellil 16:b794ff225d9d 691 QueueMainActivity.put((char*)msg);
mapellil 0:e93a11b4e044 692 }
mapellil 0:e93a11b4e044 693
mapellil 0:e93a11b4e044 694 static void Ticker_Calib(void)
mapellil 0:e93a11b4e044 695 {
mapellil 16:b794ff225d9d 696 char msg;
mapellil 0:e93a11b4e044 697 timeoutCalib = 1;
mapellil 0:e93a11b4e044 698 }
mapellil 0:e93a11b4e044 699
mapellil 0:e93a11b4e044 700 static void Ticker_Mems(void)
mapellil 0:e93a11b4e044 701 {
mapellil 16:b794ff225d9d 702 char msg;
mapellil 0:e93a11b4e044 703 TimeStamp++;
mapellil 8:b042fe719f51 704 CounterAGM++;
mapellil 16:b794ff225d9d 705 msg = TMO_MEMS;
mapellil 16:b794ff225d9d 706 QueueMainActivity.put((char*)msg);
mapellil 16:b794ff225d9d 707 }
mapellil 16:b794ff225d9d 708
mapellil 16:b794ff225d9d 709 static int ble_started=0;
mapellil 16:b794ff225d9d 710
mapellil 16:b794ff225d9d 711 void onBleInitError(BLE &ble, ble_error_t error)
mapellil 16:b794ff225d9d 712 {
mapellil 16:b794ff225d9d 713 (void)ble;
mapellil 16:b794ff225d9d 714 (void)error;
mapellil 16:b794ff225d9d 715 printf ("--->> onBleInitError\n\r");
mapellil 16:b794ff225d9d 716 /* Initialization error handling should go here */
mapellil 16:b794ff225d9d 717 }
mapellil 16:b794ff225d9d 718
mapellil 16:b794ff225d9d 719 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
mapellil 16:b794ff225d9d 720 {
mapellil 16:b794ff225d9d 721 BLE& ble = params->ble;
mapellil 16:b794ff225d9d 722 ble_error_t error = params->error;
mapellil 16:b794ff225d9d 723 if (error != BLE_ERROR_NONE) {
mapellil 16:b794ff225d9d 724 onBleInitError(ble, error);
mapellil 16:b794ff225d9d 725 printf ("--->> bleInitComplete ERROR\n\r");
mapellil 16:b794ff225d9d 726 return;
mapellil 16:b794ff225d9d 727 }
mapellil 16:b794ff225d9d 728 if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
mapellil 16:b794ff225d9d 729 return;
mapellil 16:b794ff225d9d 730 }
mapellil 16:b794ff225d9d 731 ble_started=1;
mapellil 16:b794ff225d9d 732 #if 0
mapellil 16:b794ff225d9d 733 ble.gap().onDisconnection(disconnectionCallback);
mapellil 16:b794ff225d9d 734
mapellil 16:b794ff225d9d 735 /* Setup primary service. */
mapellil 16:b794ff225d9d 736 hrServicePtr = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
mapellil 16:b794ff225d9d 737
mapellil 16:b794ff225d9d 738 /* Setup advertising. */
mapellil 16:b794ff225d9d 739 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
mapellil 16:b794ff225d9d 740 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
mapellil 16:b794ff225d9d 741 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
mapellil 16:b794ff225d9d 742 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
mapellil 16:b794ff225d9d 743 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
mapellil 16:b794ff225d9d 744 // ble.gap().setAdvertisingInterval(1000); /* 1000ms */
mapellil 16:b794ff225d9d 745 ble.gap().setAdvertisingInterval(500); /* 1000ms */
mapellil 16:b794ff225d9d 746 ble.gap().startAdvertising();
mapellil 16:b794ff225d9d 747 #endif
mapellil 16:b794ff225d9d 748 }
mapellil 16:b794ff225d9d 749
mapellil 16:b794ff225d9d 750 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
mapellil 16:b794ff225d9d 751 BLE &ble = BLE::Instance();
mapellil 16:b794ff225d9d 752 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
mapellil 16:b794ff225d9d 753 }
mapellil 16:b794ff225d9d 754
mapellil 16:b794ff225d9d 755 /* Helper function for printing floats & doubles */
mapellil 16:b794ff225d9d 756 static char *print_double(char* str, double v, int decimalDigits=2)
mapellil 16:b794ff225d9d 757 {
mapellil 16:b794ff225d9d 758 int i = 1;
mapellil 16:b794ff225d9d 759 int intPart, fractPart;
mapellil 16:b794ff225d9d 760 int len;
mapellil 16:b794ff225d9d 761 char *ptr;
mapellil 16:b794ff225d9d 762
mapellil 16:b794ff225d9d 763 /* prepare decimal digits multiplicator */
mapellil 16:b794ff225d9d 764 for (;decimalDigits!=0; i*=10, decimalDigits--);
mapellil 16:b794ff225d9d 765
mapellil 16:b794ff225d9d 766 /* calculate integer & fractinal parts */
mapellil 16:b794ff225d9d 767 intPart = (int)v;
mapellil 16:b794ff225d9d 768 fractPart = (int)((v-(double)(int)v)*i);
mapellil 16:b794ff225d9d 769
mapellil 16:b794ff225d9d 770 /* fill in integer part */
mapellil 16:b794ff225d9d 771 sprintf(str, "%i.", intPart);
mapellil 16:b794ff225d9d 772
mapellil 16:b794ff225d9d 773 /* prepare fill in of fractional part */
mapellil 16:b794ff225d9d 774 len = strlen(str);
mapellil 16:b794ff225d9d 775 ptr = &str[len];
mapellil 16:b794ff225d9d 776
mapellil 16:b794ff225d9d 777 /* fill in leading fractional zeros */
mapellil 16:b794ff225d9d 778 for (i/=10;i>1; i/=10, ptr++) {
mapellil 16:b794ff225d9d 779 if (fractPart >= i) {
mapellil 16:b794ff225d9d 780 break;
mapellil 16:b794ff225d9d 781 }
mapellil 16:b794ff225d9d 782 *ptr = '0';
mapellil 16:b794ff225d9d 783 }
mapellil 16:b794ff225d9d 784
mapellil 16:b794ff225d9d 785 /* fill in (rest of) fractional part */
mapellil 16:b794ff225d9d 786 sprintf(ptr, "%i", fractPart);
mapellil 16:b794ff225d9d 787
mapellil 16:b794ff225d9d 788 return str;
mapellil 0:e93a11b4e044 789 }
mapellil 0:e93a11b4e044 790
mapellil 0:e93a11b4e044 791
mapellil 16:b794ff225d9d 792 void updateSensorValue() {
mapellil 16:b794ff225d9d 793 // Do blocking calls or whatever is necessary for sensor polling.
mapellil 16:b794ff225d9d 794 // In our case, we simply update the HRM measurement.
mapellil 16:b794ff225d9d 795
mapellil 16:b794ff225d9d 796 float value1, value2;
mapellil 16:b794ff225d9d 797 char buffer1[32], buffer2[32];
mapellil 16:b794ff225d9d 798 int32_t axes[3];
mapellil 16:b794ff225d9d 799 #ifdef CUST_SENS_SERV
mapellil 16:b794ff225d9d 800 press_temp.get_temperature(&value1);
mapellil 16:b794ff225d9d 801 press_temp.get_pressure(&value2);
mapellil 16:b794ff225d9d 802 printf("LPS22HB: [temp] %7s C, [press] %s mbar\r\n", print_double(buffer1, value1), print_double(buffer2, value2));
mapellil 0:e93a11b4e044 803
mapellil 16:b794ff225d9d 804 mag.get_m_axes(axes);
mapellil 16:b794ff225d9d 805 printf("LSM303AGR [mag/mgauss]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
mapellil 0:e93a11b4e044 806
mapellil 16:b794ff225d9d 807 acc.get_x_axes(axes);
mapellil 16:b794ff225d9d 808 printf("LSM303AGR [acc/mg]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
mapellil 16:b794ff225d9d 809
mapellil 16:b794ff225d9d 810 acc_gyro.get_x_axes(axes);
mapellil 16:b794ff225d9d 811 printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
mapellil 16:b794ff225d9d 812
mapellil 16:b794ff225d9d 813 acc_gyro.get_g_axes(axes);
mapellil 16:b794ff225d9d 814 printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
mapellil 16:b794ff225d9d 815 #endif
mapellil 16:b794ff225d9d 816 }
mapellil 16:b794ff225d9d 817
mapellil 16:b794ff225d9d 818
mapellil 16:b794ff225d9d 819 void periodicCallback(void)
mapellil 16:b794ff225d9d 820 {
mapellil 16:b794ff225d9d 821 // led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
mapellil 16:b794ff225d9d 822 if (BLE::Instance().getGapState().connected) {
mapellil 16:b794ff225d9d 823 // eventQueue.call(updateSensorValue);
mapellil 0:e93a11b4e044 824 }
mapellil 16:b794ff225d9d 825 }
mapellil 16:b794ff225d9d 826
mapellil 0:e93a11b4e044 827
mapellil 16:b794ff225d9d 828 void ThreadMainActivity (void) {
mapellil 16:b794ff225d9d 829 float temp, hum, pres;
mapellil 16:b794ff225d9d 830 AxesRaw_TypeDef Magn, Acc, Gyro;
mapellil 16:b794ff225d9d 831 int32_t decPart, intPart;
mapellil 16:b794ff225d9d 832 int16_t TempToSend, HumToSend;
mapellil 16:b794ff225d9d 833 uint32_t PresToSend;
mapellil 16:b794ff225d9d 834 osEvent RxEvt;
mapellil 16:b794ff225d9d 835
mapellil 0:e93a11b4e044 836 while(1) {
mapellil 0:e93a11b4e044 837
mapellil 16:b794ff225d9d 838 RxEvt = QueueMainActivity.get();
mapellil 16:b794ff225d9d 839 switch (RxEvt.value.v) {
mapellil 16:b794ff225d9d 840
mapellil 16:b794ff225d9d 841 case TMO_MEMS:
mapellil 16:b794ff225d9d 842 timeoutMems =1;
mapellil 16:b794ff225d9d 843 break;
mapellil 16:b794ff225d9d 844
mapellil 16:b794ff225d9d 845 case TMO_CALIB:
mapellil 16:b794ff225d9d 846 break;
mapellil 16:b794ff225d9d 847
mapellil 16:b794ff225d9d 848 case TMO_SENSOR_FUSION:
mapellil 16:b794ff225d9d 849 break;
mapellil 16:b794ff225d9d 850
mapellil 16:b794ff225d9d 851 case TMO_ENV:
mapellil 16:b794ff225d9d 852 timeoutEnv =1;
mapellil 16:b794ff225d9d 853 break;
mapellil 16:b794ff225d9d 854
mapellil 16:b794ff225d9d 855 default:
mapellil 16:b794ff225d9d 856 printf ("ERROR unrecognized event\n\r");
mapellil 16:b794ff225d9d 857 }
mapellil 16:b794ff225d9d 858
mapellil 0:e93a11b4e044 859 if (BLEConnectionStatus) {
mapellil 0:e93a11b4e044 860 if ((isBlueButtonSequence || ForceReCalibration) && SensorFusionOK ) {
mapellil 0:e93a11b4e044 861 printf("ForceReCalibration\r\n");
mapellil 0:e93a11b4e044 862 /* Reset the Compass Calibration */
mapellil 0:e93a11b4e044 863 isCal=0;
mapellil 0:e93a11b4e044 864 ForceReCalibration =0;
mapellil 16:b794ff225d9d 865 isBlueButtonSequence = false;
mapellil 0:e93a11b4e044 866 /* Notifications of Compass Calibration */
mapellil 0:e93a11b4e044 867 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 868 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 869 p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 870 #endif
mapellil 0:e93a11b4e044 871 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 872 /* Reset the Calibration */
mapellil 0:e93a11b4e044 873 osx_MotionFX_compass_forceReCalibration();
mapellil 0:e93a11b4e044 874 #endif
mapellil 0:e93a11b4e044 875 ResetCalibrationInMemory();
mapellil 0:e93a11b4e044 876
mapellil 0:e93a11b4e044 877 /* Reset Calibration offset */
mapellil 0:e93a11b4e044 878 magOffset.magOffX = magOffset.magOffY= magOffset.magOffZ=0;
mapellil 16:b794ff225d9d 879 // greenled =0;
mapellil 0:e93a11b4e044 880 }
mapellil 0:e93a11b4e044 881 }
mapellil 0:e93a11b4e044 882 // Update MEMS @ 100Hz and calculate quaternions
mapellil 0:e93a11b4e044 883 if (timeoutMems && BLEConnectionStatus) {
mapellil 0:e93a11b4e044 884 timeoutMems =0;
mapellil 0:e93a11b4e044 885
mapellil 0:e93a11b4e044 886 #ifdef CUST_SENS_SERV
mapellil 16:b794ff225d9d 887 int32_t err=IMU_6AXES_OK;
mapellil 16:b794ff225d9d 888 err=acc_gyro.get_x_axes((int32_t *)&Acc);
mapellil 10:d19d8a60d3e5 889 if (err != IMU_6AXES_OK) {
mapellil 10:d19d8a60d3e5 890 printf ("= * ERROR %d GetAccelerometer\n\r", err);
mapellil 10:d19d8a60d3e5 891 }
mapellil 16:b794ff225d9d 892
mapellil 16:b794ff225d9d 893 err=acc_gyro.get_g_axes((int32_t *)&Gyro);
mapellil 10:d19d8a60d3e5 894 if (err != IMU_6AXES_OK) {
mapellil 10:d19d8a60d3e5 895 printf ("= * ERROR %d GetGyroscope\n\r", err);
mapellil 10:d19d8a60d3e5 896 }
mapellil 16:b794ff225d9d 897 err=mag.get_m_axes((int32_t *)&Magn);
mapellil 10:d19d8a60d3e5 898 if (err != MAGNETO_OK) {
mapellil 10:d19d8a60d3e5 899 printf ("= * ERROR %d Get_M_Axes\n\r", err);
mapellil 10:d19d8a60d3e5 900 }
mapellil 16:b794ff225d9d 901
mapellil 0:e93a11b4e044 902 if(CounterAGM >= ACC_GYRO_MAG_UPDATE_MUL_10MS) {
mapellil 16:b794ff225d9d 903 CounterAGM=0;
mapellil 10:d19d8a60d3e5 904 p_customsensorservice->updateEnvAccelerometer (&Acc, TimeStamp);
mapellil 10:d19d8a60d3e5 905 p_customsensorservice->updateEnvGyroscope(&Gyro, TimeStamp);
mapellil 10:d19d8a60d3e5 906 p_customsensorservice->updateEnvMagnetometer(&Magn, TimeStamp, magOffset);
mapellil 10:d19d8a60d3e5 907 p_customsensorservice->updateEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, magOffset);
mapellil 0:e93a11b4e044 908 }
mapellil 0:e93a11b4e044 909 #endif
mapellil 0:e93a11b4e044 910
mapellil 0:e93a11b4e044 911 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 912 if (SensorFusionOK) {
mapellil 0:e93a11b4e044 913 MotionFX_manager_run((AxesRaw_t*)&Acc, (AxesRaw_t*)&Gyro, (AxesRaw_t*)&Magn, magOffset);
mapellil 0:e93a11b4e044 914 p_MotionFX_Engine_Out = MotionFX_manager_getDataOUT();
mapellil 0:e93a11b4e044 915 }
mapellil 0:e93a11b4e044 916 #endif
mapellil 0:e93a11b4e044 917 #ifdef CUST_SW_SERV
mapellil 0:e93a11b4e044 918 if (SensorFusionOK) {
mapellil 0:e93a11b4e044 919 /* Update quaternions */
mapellil 0:e93a11b4e044 920 if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) {
mapellil 0:e93a11b4e044 921 quat_axes[QuaternionNumber].AXIS_X = (int)(p_MotionFX_Engine_Out->quaternion_9X[0] * (-10000));
mapellil 0:e93a11b4e044 922 quat_axes[QuaternionNumber].AXIS_Y = (int)(p_MotionFX_Engine_Out->quaternion_9X[1] * (-10000));
mapellil 0:e93a11b4e044 923 quat_axes[QuaternionNumber].AXIS_Z = (int)(p_MotionFX_Engine_Out->quaternion_9X[2] * (-10000));
mapellil 0:e93a11b4e044 924 } else {
mapellil 0:e93a11b4e044 925 quat_axes[QuaternionNumber].AXIS_X = (int)(p_MotionFX_Engine_Out->quaternion_9X[0] * 10000);
mapellil 0:e93a11b4e044 926 quat_axes[QuaternionNumber].AXIS_Y = (int)(p_MotionFX_Engine_Out->quaternion_9X[1] * 10000);
mapellil 0:e93a11b4e044 927 quat_axes[QuaternionNumber].AXIS_Z = (int)(p_MotionFX_Engine_Out->quaternion_9X[2] * 10000);
mapellil 0:e93a11b4e044 928 }
mapellil 0:e93a11b4e044 929
mapellil 0:e93a11b4e044 930 if (QuaternionNumber == SEND_N_QUATERNIONS-1) {
mapellil 10:d19d8a60d3e5 931 p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp);
mapellil 0:e93a11b4e044 932 QuaternionNumber =0;
mapellil 0:e93a11b4e044 933 } else {
mapellil 0:e93a11b4e044 934 QuaternionNumber++;
mapellil 0:e93a11b4e044 935 }
mapellil 0:e93a11b4e044 936
mapellil 0:e93a11b4e044 937 /* Update Float Quaternions */
mapellil 0:e93a11b4e044 938 /* Every QUAT_FLOAT_UPDATE_MUL_10MS*10 mSeconds Send Float Quaternion informations via bluetooth */
mapellil 0:e93a11b4e044 939 if(CounterFloat==QUAT_FLOAT_UPDATE_MUL_10MS) {
mapellil 0:e93a11b4e044 940 if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) {
mapellil 0:e93a11b4e044 941 QuatFloat[0] = - p_MotionFX_Engine_Out->quaternion_9X[0];
mapellil 0:e93a11b4e044 942 QuatFloat[1] = - p_MotionFX_Engine_Out->quaternion_9X[1];
mapellil 0:e93a11b4e044 943 QuatFloat[2] = - p_MotionFX_Engine_Out->quaternion_9X[2];
mapellil 0:e93a11b4e044 944 } else {
mapellil 0:e93a11b4e044 945 QuatFloat[0] = p_MotionFX_Engine_Out->quaternion_9X[0];
mapellil 0:e93a11b4e044 946 QuatFloat[1] = p_MotionFX_Engine_Out->quaternion_9X[1];
mapellil 0:e93a11b4e044 947 QuatFloat[2] = p_MotionFX_Engine_Out->quaternion_9X[2];
mapellil 0:e93a11b4e044 948 }
mapellil 10:d19d8a60d3e5 949 p_customsoftwareservice->updateFloatQuaternions(QuatFloat, TimeStamp);
mapellil 0:e93a11b4e044 950 CounterFloat=0;
mapellil 0:e93a11b4e044 951 } else {
mapellil 0:e93a11b4e044 952 CounterFloat++;
mapellil 0:e93a11b4e044 953 }
mapellil 10:d19d8a60d3e5 954 }
mapellil 0:e93a11b4e044 955 #endif
mapellil 0:e93a11b4e044 956 }
mapellil 0:e93a11b4e044 957
mapellil 0:e93a11b4e044 958 /* Check if is calibrated @ 25Hz */
mapellil 0:e93a11b4e044 959 if(isCal!=0x01 && timeoutCalib && BLEConnectionStatus && SensorFusionOK ) {
mapellil 0:e93a11b4e044 960 timeoutCalib =0;
mapellil 0:e93a11b4e044 961 /* Run Compass Calibration @ 25Hz */
mapellil 0:e93a11b4e044 962 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 963 osx_MotionFX_compass_saveAcc(Acc.AXIS_X, Acc.AXIS_Y, Acc.AXIS_Z); /* Accelerometer data ENU systems coordinate */
mapellil 0:e93a11b4e044 964 osx_MotionFX_compass_saveMag(Magn.AXIS_X, Magn.AXIS_Y, Magn.AXIS_Z); /* Magnetometer data ENU systems coordinate */
mapellil 0:e93a11b4e044 965 osx_MotionFX_compass_run();
mapellil 0:e93a11b4e044 966
mapellil 0:e93a11b4e044 967 /* Control the calibration status */
mapellil 0:e93a11b4e044 968 isCal = osx_MotionFX_compass_isCalibrated();
mapellil 0:e93a11b4e044 969 #endif
mapellil 0:e93a11b4e044 970 if(isCal == 0x01) {
mapellil 0:e93a11b4e044 971
mapellil 0:e93a11b4e044 972 printf("Compass Calibrated\n\r");
mapellil 0:e93a11b4e044 973 /* Get new magnetometer offset */
mapellil 0:e93a11b4e044 974 #ifdef USE_SENSOR_FUSION_LIB
mapellil 0:e93a11b4e044 975 osx_MotionFX_getCalibrationData(&magOffset);
mapellil 0:e93a11b4e044 976 #endif
mapellil 0:e93a11b4e044 977 /* Save the calibration in Memory */
mapellil 0:e93a11b4e044 978 SaveCalibrationToMemory();
mapellil 0:e93a11b4e044 979
mapellil 0:e93a11b4e044 980 /* Switch on the Led */
mapellil 16:b794ff225d9d 981 // greenled = 1;
mapellil 0:e93a11b4e044 982
mapellil 0:e93a11b4e044 983 /* Notifications of Compass Calibration */
mapellil 0:e93a11b4e044 984 #ifdef CUST_CONFIG_SERV
mapellil 0:e93a11b4e044 985 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 986 p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
mapellil 0:e93a11b4e044 987 #endif
mapellil 0:e93a11b4e044 988 }
mapellil 0:e93a11b4e044 989 }
mapellil 0:e93a11b4e044 990
mapellil 0:e93a11b4e044 991 // Update environmental sensors @ 2Hz
mapellil 0:e93a11b4e044 992 if (timeoutEnv && BLEConnectionStatus) {
mapellil 0:e93a11b4e044 993 timeoutEnv =0;
mapellil 16:b794ff225d9d 994 int32_t err;
mapellil 0:e93a11b4e044 995 #ifdef CUST_SENS_SERV
mapellil 0:e93a11b4e044 996 if (p_customsensorservice->isTempNotificationEn()) {
mapellil 16:b794ff225d9d 997 err=press_temp.get_temperature(&temp);
mapellil 8:b042fe719f51 998 if ( err != HUM_TEMP_OK) {
mapellil 0:e93a11b4e044 999 printf ("= * ERROR %d GetTemperature\n\r", err);
mapellil 0:e93a11b4e044 1000 } else {
mapellil 16:b794ff225d9d 1001 floatToInt(temp, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 1002 TempToSend = intPart*10+decPart;
mapellil 10:d19d8a60d3e5 1003 p_customsensorservice->updateEnvTemperature(TempToSend, TimeStamp);
mapellil 0:e93a11b4e044 1004 }
mapellil 0:e93a11b4e044 1005 }
mapellil 16:b794ff225d9d 1006 #if 0 // Hum sensor not present on SensorTile
mapellil 0:e93a11b4e044 1007 if (p_customsensorservice->isHumNotificationEn()) {
mapellil 16:b794ff225d9d 1008 // err = p_mems_expansion_board->ht_sensor->GetHumidity(&hum); /** FIXME no hum sens on Tile */
mapellil 8:b042fe719f51 1009 if ( err != HUM_TEMP_OK) {
mapellil 0:e93a11b4e044 1010 printf ("= * ERROR %d GetHumidity\n\r", err);
mapellil 0:e93a11b4e044 1011 } else {
mapellil 0:e93a11b4e044 1012 floatToInt(hum, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 1013 HumToSend = intPart*10+decPart;
mapellil 10:d19d8a60d3e5 1014 p_customsensorservice->updateEnvHumidity(HumToSend, TimeStamp);
mapellil 0:e93a11b4e044 1015 }
mapellil 0:e93a11b4e044 1016 }
mapellil 16:b794ff225d9d 1017 #endif
mapellil 0:e93a11b4e044 1018 if (p_customsensorservice->isPresNotificationEn()) {
mapellil 16:b794ff225d9d 1019 err=press_temp.get_pressure(&pres);
mapellil 8:b042fe719f51 1020 if ( err != PRESSURE_OK) {
mapellil 0:e93a11b4e044 1021 printf ("= * ERROR GetPressure\n\r");
mapellil 0:e93a11b4e044 1022 } else {
mapellil 0:e93a11b4e044 1023 floatToInt(pres, &intPart, &decPart, 1);
mapellil 0:e93a11b4e044 1024 PresToSend = intPart*100+decPart;
mapellil 10:d19d8a60d3e5 1025 p_customsensorservice->updateEnvPressure(PresToSend, TimeStamp);
mapellil 0:e93a11b4e044 1026 }
mapellil 0:e93a11b4e044 1027 }
mapellil 0:e93a11b4e044 1028 #endif
mapellil 0:e93a11b4e044 1029 }
mapellil 0:e93a11b4e044 1030 }
mapellil 16:b794ff225d9d 1031 }
mapellil 16:b794ff225d9d 1032
mapellil 16:b794ff225d9d 1033
mapellil 16:b794ff225d9d 1034 int main()
mapellil 16:b794ff225d9d 1035 {
mapellil 16:b794ff225d9d 1036 Ticker EnvTimer;
mapellil 16:b794ff225d9d 1037 Ticker CalibTimer;
mapellil 16:b794ff225d9d 1038 Ticker MemsTimer;
mapellil 16:b794ff225d9d 1039 bool isgyro_lsm6ds0Present=false;
mapellil 16:b794ff225d9d 1040 bool isgyro_lsm6ds3Present=false;
mapellil 16:b794ff225d9d 1041 #ifdef CUST_CONS_SERV
mapellil 16:b794ff225d9d 1042 // static uint8_t BufferToWrite[256];
mapellil 16:b794ff225d9d 1043 // static uint8_t BytesToWrite;
mapellil 16:b794ff225d9d 1044 #endif
mapellil 16:b794ff225d9d 1045
mapellil 16:b794ff225d9d 1046 // DevI2C *i2c = new DevI2C(I2C_SDA, I2C_SCL);
mapellil 16:b794ff225d9d 1047 // i2c->frequency(NUCLEO_I2C_SHIELDS_SPEED);
mapellil 16:b794ff225d9d 1048
mapellil 16:b794ff225d9d 1049 #ifdef CUST_SENS_SERV
mapellil 16:b794ff225d9d 1050 // p_mems_expansion_board = X_NUCLEO_IKS01A1::Instance(i2c);
mapellil 16:b794ff225d9d 1051 // if (p_mems_expansion_board->gyro_lsm6ds0) isgyro_lsm6ds0Present=true;
mapellil 16:b794ff225d9d 1052 // if (p_mems_expansion_board->gyro_lsm6ds3) isgyro_lsm6ds3Present=true;
mapellil 16:b794ff225d9d 1053 #endif
mapellil 16:b794ff225d9d 1054 //#ifdef CUST_CONFIG_SERV
mapellil 16:b794ff225d9d 1055 // InterruptIn BlueButton(USER_BUTTON);
mapellil 16:b794ff225d9d 1056 // BlueButton.fall(&BlueButtonPressed);
mapellil 16:b794ff225d9d 1057 //#endif
mapellil 16:b794ff225d9d 1058
mapellil 16:b794ff225d9d 1059 printf("\r\nSTMicroelectronics BlueMicrosystem1 W2ST:\r\n"
mapellil 16:b794ff225d9d 1060 #ifdef CUST_SENS_SERV
mapellil 16:b794ff225d9d 1061 "\tGyro lsmds0 present: %d, lsmds3 present: %d\n\r"
mapellil 16:b794ff225d9d 1062 "\tSend Every %dmS Temperature/Humidity/Pressure\r\n"
mapellil 16:b794ff225d9d 1063 "\tSend Every %dmS Acc/Gyro/Magneto\r\n\n"
mapellil 16:b794ff225d9d 1064 #endif
mapellil 16:b794ff225d9d 1065 #ifdef CUST_SW_SERV
mapellil 16:b794ff225d9d 1066 ,"\tSend Every %dmS %d Quaternions\r\n"
mapellil 16:b794ff225d9d 1067 "\tSend Every %dmS Float precision Quaternion\r\n"
mapellil 16:b794ff225d9d 1068 #endif
mapellil 16:b794ff225d9d 1069 #ifdef CUST_SENS_SERV
mapellil 16:b794ff225d9d 1070 ,isgyro_lsm6ds0Present,
mapellil 16:b794ff225d9d 1071 isgyro_lsm6ds3Present,
mapellil 16:b794ff225d9d 1072 ENV_TIMER/1000,
mapellil 16:b794ff225d9d 1073 MEMS_TIMER/1000*ACC_GYRO_MAG_UPDATE_MUL_10MS
mapellil 16:b794ff225d9d 1074 #endif
mapellil 16:b794ff225d9d 1075 #ifdef CUST_SW_SERV
mapellil 16:b794ff225d9d 1076 ,MEMS_TIMER/1000*SEND_N_QUATERNIONS,SEND_N_QUATERNIONS,
mapellil 16:b794ff225d9d 1077 MEMS_TIMER/1000*QUAT_FLOAT_UPDATE_MUL_10MS
mapellil 16:b794ff225d9d 1078 #endif
mapellil 16:b794ff225d9d 1079 );
mapellil 16:b794ff225d9d 1080
mapellil 16:b794ff225d9d 1081 /*
mapellil 16:b794ff225d9d 1082 p_BLEdev = new BLE;
mapellil 16:b794ff225d9d 1083 if (!p_BLEdev) {
mapellil 16:b794ff225d9d 1084 printf("\r\nBLE Device creation failed\r\n");
mapellil 16:b794ff225d9d 1085 return 0;
mapellil 16:b794ff225d9d 1086 }
mapellil 16:b794ff225d9d 1087 p_BLEdev->init(bleInitComplete);
mapellil 16:b794ff225d9d 1088 */
mapellil 16:b794ff225d9d 1089 // eventQueue.call_every(500, periodicCallback);
mapellil 16:b794ff225d9d 1090 BLE &ble = BLE::Instance();
mapellil 16:b794ff225d9d 1091 p_BLEdev = &ble;
mapellil 16:b794ff225d9d 1092 p_BLEdev->onEventsToProcess(scheduleBleEventsProcessing);
mapellil 16:b794ff225d9d 1093 p_BLEdev->init(bleInitComplete);
mapellil 16:b794ff225d9d 1094
mapellil 16:b794ff225d9d 1095 for (; !ble_started; wait_ms(10));
mapellil 16:b794ff225d9d 1096 // greenled = 0;//Switch OFF LED1
mapellil 16:b794ff225d9d 1097
mapellil 16:b794ff225d9d 1098 /* set the BLE CB functions */
mapellil 16:b794ff225d9d 1099 p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
mapellil 16:b794ff225d9d 1100 p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
mapellil 16:b794ff225d9d 1101 /* p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback);
mapellil 16:b794ff225d9d 1102 p_BLEdev->gattServer().onDataSent(onDataSentCallback);
mapellil 16:b794ff225d9d 1103 p_BLEdev->gattServer().onDataRead(onDataReadCallback);
mapellil 16:b794ff225d9d 1104 p_BLEdev->gattServer().onDataWritten(onDataWriteCallback);
mapellil 16:b794ff225d9d 1105 */
mapellil 16:b794ff225d9d 1106 p_BLEdev->gap().onConnection(onConnectionCallback);
mapellil 16:b794ff225d9d 1107 p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
mapellil 16:b794ff225d9d 1108 /* p_BLEdev->gap().onTimeout(onTimeoutCallback);
mapellil 16:b794ff225d9d 1109 */
mapellil 16:b794ff225d9d 1110 /* Initialize MotionFX library */
mapellil 16:b794ff225d9d 1111 #ifdef USE_SENSOR_FUSION_LIB
mapellil 16:b794ff225d9d 1112 bool DS3_OnBoard = DS3_ONBOARD;
mapellil 16:b794ff225d9d 1113 if (MotionFX_manager_init(DS3_OnBoard, magOffset)) {
mapellil 16:b794ff225d9d 1114 MotionFX_manager_start_9X();
mapellil 16:b794ff225d9d 1115 SensorFusionOK = true;
mapellil 16:b794ff225d9d 1116 printf("SW Service sensor fusion library added successfully\r\n");
mapellil 16:b794ff225d9d 1117 } else { /* sensor fusion lib init failed: likely because of wrong license */
mapellil 16:b794ff225d9d 1118 printf("sensor fusion lib init failed: likely because of wrong license \n\r");
mapellil 16:b794ff225d9d 1119 SensorFusionOK = false;
mapellil 16:b794ff225d9d 1120 }
mapellil 16:b794ff225d9d 1121 #else
mapellil 16:b794ff225d9d 1122 printf("sensor fusion lib disabled \n\r");
mapellil 16:b794ff225d9d 1123 SensorFusionOK = false;
mapellil 16:b794ff225d9d 1124 #endif
mapellil 16:b794ff225d9d 1125
mapellil 16:b794ff225d9d 1126 #ifdef CUST_CONFIG_SERV
mapellil 16:b794ff225d9d 1127 p_customconfigservice = new CustomConfigService(*p_BLEdev);
mapellil 16:b794ff225d9d 1128 if (!p_customconfigservice) {
mapellil 16:b794ff225d9d 1129 printf("SW Service W2ST calibration add FAILED!\n\r");
mapellil 16:b794ff225d9d 1130 return 0;
mapellil 16:b794ff225d9d 1131 }
mapellil 16:b794ff225d9d 1132 printf("SW Service W2ST calibration added successfully\r\n");
mapellil 16:b794ff225d9d 1133 #endif
mapellil 16:b794ff225d9d 1134 #ifdef CUST_SENS_SERV
mapellil 16:b794ff225d9d 1135 press_temp.enable();
mapellil 16:b794ff225d9d 1136 acc_gyro.enable_x();
mapellil 16:b794ff225d9d 1137 acc_gyro.enable_g();
mapellil 16:b794ff225d9d 1138 mag.enable();
mapellil 16:b794ff225d9d 1139 acc.enable();
mapellil 16:b794ff225d9d 1140
mapellil 16:b794ff225d9d 1141 uint8_t id22=0, idLSM6=0, id303mag=0, id303acc=0;
mapellil 16:b794ff225d9d 1142 press_temp.read_id(&id22);
mapellil 16:b794ff225d9d 1143 acc_gyro.read_id(&idLSM6);
mapellil 16:b794ff225d9d 1144 mag.read_id(&id303mag);
mapellil 16:b794ff225d9d 1145 acc.read_id(&id303acc);
mapellil 16:b794ff225d9d 1146
mapellil 16:b794ff225d9d 1147 printf("LS303acc ID %x LS303mag ID %x LSM6DSL ID %x LPS22HB ID %x \r\n", id303acc, id303mag, idLSM6, id22);
mapellil 16:b794ff225d9d 1148 printf("\r\n");
mapellil 16:b794ff225d9d 1149
mapellil 16:b794ff225d9d 1150 p_customsensorservice = new CustomSensorService(*p_BLEdev);
mapellil 16:b794ff225d9d 1151 if (!p_customsensorservice) {
mapellil 16:b794ff225d9d 1152 printf("\n\rHW Service W2ST sensors add FAILED!\n\r");
mapellil 16:b794ff225d9d 1153 return 0;
mapellil 16:b794ff225d9d 1154 }
mapellil 16:b794ff225d9d 1155 printf("\rHW Service W2ST sensors added successfully\r\n");
mapellil 16:b794ff225d9d 1156 #endif
mapellil 16:b794ff225d9d 1157 #ifdef CUST_CONS_SERV
mapellil 16:b794ff225d9d 1158 p_customconsoleservice = new CustomConsoleService(*p_BLEdev);
mapellil 16:b794ff225d9d 1159 if (!p_customconsoleservice) {
mapellil 16:b794ff225d9d 1160 printf("\n\rHW Service W2ST console add FAILED!\n\r");
mapellil 16:b794ff225d9d 1161 return 0;
mapellil 16:b794ff225d9d 1162 }
mapellil 16:b794ff225d9d 1163 printf("\rHW Service W2ST console added successfully\r\n");
mapellil 16:b794ff225d9d 1164 #endif
mapellil 16:b794ff225d9d 1165 #ifdef CUST_SW_SERV
mapellil 16:b794ff225d9d 1166 AxesRaw_TypeDef quat_axes[SEND_N_QUATERNIONS];
mapellil 16:b794ff225d9d 1167 float QuatFloat[SEND_N_QUATERNIONS];
mapellil 16:b794ff225d9d 1168 if (SensorFusionOK) {
mapellil 16:b794ff225d9d 1169 p_customsoftwareservice = new CustomSoftwareService(*p_BLEdev);
mapellil 16:b794ff225d9d 1170 if (!p_customsoftwareservice) {
mapellil 16:b794ff225d9d 1171 printf("SW Service W2ST quaternions add FAILED!\n\r");
mapellil 16:b794ff225d9d 1172 return 0;
mapellil 16:b794ff225d9d 1173 }
mapellil 16:b794ff225d9d 1174 printf("SW Service W2ST quaternions added successfully\r\n");
mapellil 16:b794ff225d9d 1175 }
mapellil 16:b794ff225d9d 1176 #endif
mapellil 16:b794ff225d9d 1177
mapellil 16:b794ff225d9d 1178 const static char DEVICE_NAME[] = BLE_DEV_NAME;
mapellil 16:b794ff225d9d 1179 /* Setup advertising. */
mapellil 16:b794ff225d9d 1180 p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
mapellil 16:b794ff225d9d 1181 // p_BLEdev->gap().accumulateAdvertisingPayloadTxPower(txPower);
mapellil 16:b794ff225d9d 1182 #ifdef USE_SENSOR_FUSION_LIB
mapellil 16:b794ff225d9d 1183 uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x01,0x80};
mapellil 16:b794ff225d9d 1184 #else
mapellil 16:b794ff225d9d 1185 uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x00,0x00};
mapellil 16:b794ff225d9d 1186 #endif
mapellil 16:b794ff225d9d 1187 p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);
mapellil 16:b794ff225d9d 1188 p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
mapellil 16:b794ff225d9d 1189 p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
mapellil 16:b794ff225d9d 1190
mapellil 16:b794ff225d9d 1191 // p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, SENS_SERVICE_UUID_128, sizeof(SENS_SERVICE_UUID_128));
mapellil 16:b794ff225d9d 1192
mapellil 16:b794ff225d9d 1193 p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
mapellil 16:b794ff225d9d 1194 p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
mapellil 16:b794ff225d9d 1195
mapellil 16:b794ff225d9d 1196
mapellil 16:b794ff225d9d 1197 printf("SERVER: BLE Stack Initialized\r\n");
mapellil 16:b794ff225d9d 1198 printf("Starting Advertising...\n\r");
mapellil 16:b794ff225d9d 1199 p_BLEdev->gap().startAdvertising();
mapellil 16:b794ff225d9d 1200
mapellil 16:b794ff225d9d 1201 TimeStamp =0;
mapellil 16:b794ff225d9d 1202 EnvTimer.attach_us(&Ticker_Env, ENV_TIMER);
mapellil 16:b794ff225d9d 1203 // CalibTimer.attach_us(&Ticker_Calib, CALIB_TIMER );
mapellil 16:b794ff225d9d 1204 MemsTimer.attach_us(&Ticker_Mems, MEMS_TIMER);
mapellil 16:b794ff225d9d 1205
mapellil 16:b794ff225d9d 1206 /* Control if the calibration is already available in memory */
mapellil 16:b794ff225d9d 1207 if (SensorFusionOK) {
mapellil 16:b794ff225d9d 1208 ReCallCalibrationFromMemory();
mapellil 16:b794ff225d9d 1209
mapellil 16:b794ff225d9d 1210 /* Switch on/off the LED according to calibration */
mapellil 16:b794ff225d9d 1211 if(isCal) {
mapellil 16:b794ff225d9d 1212 // greenled =1;
mapellil 16:b794ff225d9d 1213 } else {
mapellil 16:b794ff225d9d 1214 // greenled =0;
mapellil 16:b794ff225d9d 1215 }
mapellil 16:b794ff225d9d 1216 }
mapellil 16:b794ff225d9d 1217 #ifdef CUST_SW_SERV
mapellil 16:b794ff225d9d 1218 uint32_t QuaternionNumber =0;
mapellil 16:b794ff225d9d 1219 uint32_t CounterFloat =0;
mapellil 16:b794ff225d9d 1220 #endif
mapellil 16:b794ff225d9d 1221
mapellil 16:b794ff225d9d 1222 Thread ThObjMainActivity;
mapellil 16:b794ff225d9d 1223 ThObjMainActivity.start(Callback<void()>(ThreadMainActivity));
mapellil 16:b794ff225d9d 1224
mapellil 16:b794ff225d9d 1225 eventQueue.dispatch_forever();
mapellil 0:e93a11b4e044 1226 }
mapellil 16:b794ff225d9d 1227
mapellil 16:b794ff225d9d 1228
mapellil 16:b794ff225d9d 1229