ST
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.
Diff: main.cpp
- Revision:
- 20:b97e14ade434
- Parent:
- 18:e2360c944484
- Child:
- 24:b374c444d8ad
--- a/main.cpp Fri Jun 09 14:00:14 2017 +0200
+++ b/main.cpp Wed Sep 20 17:08:56 2017 +0200
@@ -36,33 +36,21 @@
*/
#include "mbed.h"
-
-/*L0_BlueNRG_xx version main*/
-
+#include "main.h" // contains the condt compiling configuration
#include <cmath>
#include <math.h>
-#include "mbed.h"
#include "Gap.h"
#include "debug.h"
#include "btle.h"
#include "blecommon.h"
#include "BLE.h"
#include "UUID.h"
-//#include "Utils.h"
#include "MotionFX_Manager.h"
#include "InterruptManager.h"
#include "DevI2C.h"
#include "BlueNRGGattServer.h"
#include "GattCharacteristic.h"
-#include "main.h" // contains the condt compiling configuration
-#include "ble_utils.h"
-#ifdef DEBUG
-#include <stdio.h>
-#define PRINTF(...) printf(__VA_ARGS__)
-#else
-#define PRINTF(...)
-#endif
typedef struct {
int32_t AXIS_X;
@@ -81,12 +69,24 @@
#ifdef CUST_SENS_SERV
#include "XNucleoIKS01A2.h"
#include "CustomSensorsService.h"
+//DevI2C i2c(I2C_SDA, I2C_SCL);
+//HTS221Sensor ht_sensor(&i2c); // sensor on the mini cradle only (not on the tile)
+SPI3W sens_intf(PB_15, NC, PB_13); // 3W mosi, sclk on Nucleo L476 same as BTLE
+LPS22HBSensor pt_sensor(&sens_intf, PA_3); // on SensorTile L476JG
+LSM6DSLSensor acc_gyro(&sens_intf, PB_12, NC, PA_2 ); // on SensorTile L476JG
+LSM303AGRMagSensor magnetometer(&sens_intf, PB_1 ); //on SensorTile L476JG
+LSM303AGRAccSensor accelerometer(&sens_intf, PC_4 ); //on SensorTile L476JG
#endif
#ifdef CUST_CONFIG_SERV
#include "CustomConfigService.h"
+//static unsigned char isCal = 0;
+//static bool ForceReCalibration =0;
#endif
+static unsigned char isCal = 0;
+static bool ForceReCalibration =0;
+
#ifdef CUST_SW_SERV
#include "CustomSoftwareService.h"
#endif
@@ -95,55 +95,38 @@
static Ticker BlueButtonTimer;
//static DigitalOut greenled(LED1);
//static int8_t txPower=-3;
-static unsigned char isCal = 0;
-static uint16_t TimeStamp=0;
+static uint32_t TimeStamp=0;
static int BLEConnectionStatus =0;
-static bool ForceReCalibration =0;
static int HowManyButtonPress =0;
-static uint32_t timeoutEnv =0;
-static uint32_t timeoutCalib =0;
-static uint32_t timeoutMems =0;
static BLE * p_BLEdev = NULL;
-static int32_t CounterAGM =0;
-static bool SensorFusionOK = false;
+static bool SensorFusionOK = false;
+static int ble_started=0;
+static Queue <char, 8> QueueMainActivity;
static EventQueue eventQueue(/* event count */ 16 * /* event size */ 32);
-static Queue <char, 5>QueueMainActivity;
-#define TMO_MEMS 1
-#define TMO_CALIB 2
-#define TMO_SENSOR_FUSION 3
-#define TMO_ENV 4
#define BLE_STARTED 5
#ifdef CUST_CONS_SERV
-static CustomConsoleService * p_customconsoleservice = NULL;
+static CustomConsoleService * p_customconsoleservice = NULL;
#endif
#ifdef CUST_SENS_SERV
-#define IMU_6AXES_OK 0
-#define MAGNETO_OK 0
-#define HUM_TEMP_OK 0
-#define PRESSURE_OK 0
-static CustomSensorService * p_customsensorservice = NULL;
-//static X_NUCLEO_IKS01A1 * p_mems_expansion_board = NULL;
-SPI3W sens_intf(PB_15, NC, PB_13); // 3W mosi, sclk on Nucleo L476 same as BTLE
-
-LPS22HBSensor press_temp(&sens_intf, PA_3); // on SensorTile L476JG
-LSM6DSLSensor acc_gyro(&sens_intf, PB_12, NC, PA_2 ); // on SensorTile L476JG
-LSM303AGRMagSensor mag(&sens_intf, PB_1 ); //on SensorTile L476JG
-LSM303AGRAccSensor acc(&sens_intf, PC_4 ); //on SensorTile L476JG
-
-#endif
+static CustomSensorService * p_customsensorservice = NULL;
+#endif
#ifdef CUST_SW_SERV
-static CustomSoftwareService * p_customsoftwareservice = NULL;
-static osxMFX_output * p_MotionFX_Engine_Out = NULL;
+static CustomSoftwareService * p_customsoftwareservice = NULL;
+static MFX_input_t MotionFX_Engine_In;
+static MFX_input_t * p_MotionFX_Engine_In = &MotionFX_Engine_In;
+static MFX_output_t MotionFX_Engine_Out;
+static MFX_output_t * p_MotionFX_Engine_Out = &MotionFX_Engine_Out;
#endif
static osxMFX_calibFactor magOffset;
+
#ifdef CUST_CONFIG_SERV
-static CustomConfigService * p_customconfigservice = NULL;
+static CustomConfigService * p_customconfigservice = NULL;
#endif
@@ -163,88 +146,39 @@
__IO uint32_t data32 = *(__IO uint32_t*) Address;
if(data32== BLUEMSYS_CHECK_CALIBRATION) {
int32_t ReadIndex;
- uint32_t ReadmagOffset[7];
+ float ReadmagOffset[7];
for(ReadIndex=0; ReadIndex<7; ReadIndex++) {
Address +=4;
data32 = *(__IO uint32_t*) Address;
ReadmagOffset[ReadIndex]=data32;
}
-
+ isCal=1;
magOffset.magOffX = (signed short) ReadmagOffset[0];
magOffset.magOffY = (signed short) ReadmagOffset[1];
magOffset.magOffZ = (signed short) ReadmagOffset[2];
- magOffset.magGainX = *((float *) &(ReadmagOffset[3]));
- magOffset.magGainY = *((float *) &(ReadmagOffset[4]));
- magOffset.magGainZ = *((float *) &(ReadmagOffset[5]));
- magOffset.expMagVect = *((float *) &(ReadmagOffset[6]));
-
#ifdef BLUEMSYS_DEBUG_CALIB
/* Debug */
printf("magOffX ->%d\r\n",magOffset.magOffX);
printf("magOffY ->%d\r\n",magOffset.magOffY);
printf("magOffZ ->%d\r\n",magOffset.magOffZ);
- printf("magGainX ->%X\r\n",magOffset.magGainX);
- printf("magGainY ->%X\r\n",magOffset.magGainY);
- printf("magGainZ ->%X\r\n",magOffset.magGainZ);
- printf("expMagVect->%X\r\n",magOffset.expMagVect);
#endif /* BLUEMSYS_DEBUG_CALIB */
-
- /* Set the Calibration Structure */
-#ifdef USE_SENSOR_FUSION_LIB
- osx_MotionFX_setCalibrationData(&magOffset);
-
printf("Calibration Read from Flash\r\n");
- /* Control the calibration status */
- isCal = osx_MotionFX_compass_isCalibrated();
- printf("Check the Calibration =%d\r\n",isCal);
-#endif
} else {
printf("Calibration Not present in FLASH\r\n");
+ magOffset.magOffX = 0;
+ magOffset.magOffY = 0;
+ magOffset.magOffZ = 0;
isCal=0;
}
-
return Success;
}
#else /* BLUEMSYS_STORE_CALIB_FLASH */
{
- /* ReLoad the Calibration Values from RAM */
- unsigned char Success=1;
-
- if(CalibrationStructureRAM[0]== BLUEMSYS_CHECK_CALIBRATION) {
- magOffset.magOffX = (signed short) CalibrationStructureRAM[1];
- magOffset.magOffY = (signed short) CalibrationStructureRAM[2];
- magOffset.magOffZ = (signed short) CalibrationStructureRAM[3];
- magOffset.magGainX = *((float *) &(CalibrationStructureRAM[4]));
- magOffset.magGainY = *((float *) &(CalibrationStructureRAM[5]));
- magOffset.magGainZ = *((float *) &(CalibrationStructureRAM[6]));
- magOffset.expMagVect = *((float *) &(CalibrationStructureRAM[7]));
-
-#ifdef BLUEMSYS_DEBUG_CALIB
- /* Debug */
- printf("magOffX ->%d\r\n",magOffset.magOffX);
- printf("magOffY ->%d\r\n",magOffset.magOffY);
- printf("magOffZ ->%d\r\n",magOffset.magOffZ);
- printf("magGainX ->%X\r\n",magOffset.magGainX);
- printf("magGainY ->%X\r\n",magOffset.magGainY);
- printf("magGainZ ->%X\r\n",magOffset.magGainZ);
- printf("expMagVect->%X\r\n",magOffset.expMagVect);
-#endif /* BLUEMSYS_DEBUG_CALIB */
-
-#ifdef USE_SENSOR_FUSION_LIB
- /* Set the Calibration Structure */
- osx_MotionFX_setCalibrationData(&magOffset);
- printf("Calibration Read from RAM\r\n");
-
- /* Control the calibration status */
- isCal = osx_MotionFX_compass_isCalibrated();
- printf("Check the Calibration =%d\r\n",isCal);
-#endif
- } else {
- printf("Calibration Not present in RAM\r\n");
- isCal=0;
- }
-
+ unsigned char Success= 1;
+ magOffset.magOffX = 0;
+ magOffset.magOffY = 0;
+ magOffset.magOffZ = 0;
return Success;
}
#endif /* BLUEMSYS_STORE_CALIB_FLASH */
@@ -286,19 +220,18 @@
/* Lock the Flash to disable the flash control register access (recommended
to protect the FLASH memory against possible unwanted operation) *********/
HAL_FLASH_Lock();
-
+ magOffset.magOffX = 0;
+ magOffset.magOffY = 0;
+ magOffset.magOffZ = 0;
+
return Success;
}
#else /* BLUEMSYS_STORE_CALIB_FLASH */
{
- /* Reset Calibration Values in RAM */
- unsigned char Success=1;
- int32_t Counter;
-
- for(Counter=0; Counter<8; Counter++)
- CalibrationStructureRAM[Counter]=0xFFFFFFFF;
-
- printf("Erase Calibration in RAM Memory\r\n");
+ unsigned char Success=1;
+ magOffset.magOffX = 0;
+ magOffset.magOffY = 0;
+ magOffset.magOffZ = 0;
return Success;
}
#endif /* BLUEMSYS_STORE_CALIB_FLASH */
@@ -326,10 +259,6 @@
WritemagOffset[1] = (uint32_t) magOffset.magOffX;
WritemagOffset[2] = (uint32_t) magOffset.magOffY;
WritemagOffset[3] = (uint32_t) magOffset.magOffZ;
- WritemagOffset[4] = *((uint32_t *) &(magOffset.magGainX));
- WritemagOffset[5] = *((uint32_t *) &(magOffset.magGainY));
- WritemagOffset[6] = *((uint32_t *) &(magOffset.magGainZ));
- WritemagOffset[7] = *((uint32_t *) &(magOffset.expMagVect));
/* Unlock the Flash to enable the flash control register access *************/
HAL_FLASH_Unlock();
@@ -351,10 +280,6 @@
printf("magOffX ->%d\r\n",magOffset.magOffX);
printf("magOffY ->%d\r\n",magOffset.magOffY);
printf("magOffZ ->%d\r\n",magOffset.magOffZ);
- printf("magGainX ->%X\r\n",magOffset.magGainX);
- printf("magGainY ->%X\r\n",magOffset.magGainY);
- printf("magGainZ ->%X\r\n",magOffset.magGainZ);
- printf("expMagVect->%X\r\n",magOffset.expMagVect);
#endif /* BLUEMSYS_DEBUG_CALIB */
printf("New Calibration Values Saved in FLASH\r\n");
@@ -365,28 +290,12 @@
}
#else /* BLUEMSYS_STORE_CALIB_FLASH */
{
- /* Store in RAM */
- CalibrationStructureRAM[0] = BLUEMSYS_CHECK_CALIBRATION;
- CalibrationStructureRAM[1] = (uint32_t) magOffset.magOffX;
- CalibrationStructureRAM[2] = (uint32_t) magOffset.magOffY;
- CalibrationStructureRAM[3] = (uint32_t) magOffset.magOffZ;
- CalibrationStructureRAM[4] = *((uint32_t *) &(magOffset.magGainX));
- CalibrationStructureRAM[5] = *((uint32_t *) &(magOffset.magGainY));
- CalibrationStructureRAM[6] = *((uint32_t *) &(magOffset.magGainZ));
- CalibrationStructureRAM[7] = *((uint32_t *) &(magOffset.expMagVect));
-
#ifdef BLUEMSYS_DEBUG_CALIB
/* Debug */
printf("magOffX ->%d\r\n",magOffset.magOffX);
printf("magOffY ->%d\r\n",magOffset.magOffY);
printf("magOffZ ->%d\r\n",magOffset.magOffZ);
- printf("magGainX ->%X\r\n",magOffset.magGainX);
- printf("magGainY ->%X\r\n",magOffset.magGainY);
- printf("magGainZ ->%X\r\n",magOffset.magGainZ);
- printf("expMagVect->%X\r\n",magOffset.expMagVect);
#endif /* BLUEMSYS_DEBUG_CALIB */
-
- printf("New Calibration Values Saved in RAM\r\n");
}
#endif /* BLUEMSYS_STORE_CALIB_FLASH */
@@ -399,15 +308,12 @@
{
*out_int = (int32_t)in;
in = in - (float)(*out_int);
-// *out_dec = (int32_t)trunc(in*pow((float)10,(int)dec_prec));
*out_dec = (int32_t)(float)(int)(in*pow((float)10,(int)dec_prec));
-
}
static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
{
PRINTF("onUpdatesEnabledCallback! handle: %x\n\r", handle);
- printf("onUpdatesEnabledCallback! handle: %x\n\r", handle);
#ifdef CUST_SENS_SERV
if (p_customsensorservice)
p_customsensorservice->enNotify (handle);
@@ -421,15 +327,15 @@
#ifdef CUST_CONFIG_SERV
if (p_customconfigservice) {
p_customconfigservice->enNotify (handle);
- p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
- p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
- }
+ p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
+ p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
+ }
#endif
+
#ifdef CUST_CONS_SERV
if (p_customconsoleservice)
p_customconsoleservice->enNotify (handle);
#endif
- /* TODO inform other obj implementing BLE services */
}
static void onUpdatesDisabledCallback(Gap::Handle_t handle)
@@ -449,14 +355,13 @@
if (p_customconfigservice)
p_customconfigservice->disNotify (handle);
#endif
+
#ifdef CUST_CONS_SERV
if (p_customconsoleservice)
p_customconsoleservice->disNotify (handle);
#endif
- /* TODO inform other obj implementing BLE services */
}
-//static void onDisconnectionCallback(const Gap::Handle_t handle, const Gap::DisconnectionReason_t disConnectionReason)
static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
{
printf("Disconnected\n\r");
@@ -517,45 +422,54 @@
int32_t decPart, intPart;
AxesRaw_TypeDef Magn, Acc, Gyro;
Gap::Handle_t handle = eventDataP->handle - BLE_HANDLE_VALUE_OFFSET;
-
#ifdef CUST_SENS_SERV
+#ifdef DISAGGREGATED_ENV_CHAR
if (p_customsensorservice->isTempHandle(handle)) {
-// p_mems_expansion_board->ht_sensor->GetTemperature(&temp);
- press_temp.get_temperature(&temp);
+ pt_sensor.get_temperature(&temp);
floatToInt(temp, &intPart, &decPart, 1);
TempToSend = intPart*10+decPart;
p_customsensorservice->sendEnvTemperature(TempToSend, TimeStamp);
} else if (p_customsensorservice->isHumHandle(handle)) {
-// p_mems_expansion_board->ht_sensor->GetHumidity(&hum); /** FIXME Hum not present */
+// ht_sensor.get_humidity(&hum);
floatToInt(hum, &intPart, &decPart, 1);
HumToSend = intPart*10+decPart;
p_customsensorservice->sendEnvHumidity(HumToSend, TimeStamp);
} else if (p_customsensorservice->isPresHandle(handle)) {
-// p_mems_expansion_board->pt_sensor->GetPressure(&pres);
- press_temp.get_pressure(&pres);
+ pt_sensor.get_pressure(&pres);
floatToInt(pres, &intPart, &decPart, 1);
PresToSend = intPart*100+decPart;
p_customsensorservice->sendEnvPressure(PresToSend, TimeStamp);
- } else if (p_customsensorservice->isGyroHandle(handle)) {
-// p_mems_expansion_board->GetGyroscope()->Get_G_Axes((int32_t *)&Gyro);
+ } else
+#endif
+ if (p_customsensorservice->isGyroHandle(handle)) {
acc_gyro.get_g_axes((int32_t *)&Gyro);
p_customsensorservice->sendEnvGyroscope (&Gyro, TimeStamp);
} else if (p_customsensorservice->isAccHandle(handle)) {
-// p_mems_expansion_board->GetAccelerometer()->Get_X_Axes((int32_t *)&Acc);
- acc_gyro.get_x_axes((int32_t *)&Acc);
+ accelerometer.get_x_axes((int32_t *)&Acc);
p_customsensorservice->sendEnvAccelerometer (&Acc, TimeStamp);
} else if (p_customsensorservice->isMagHandle(handle)) {
-// p_mems_expansion_board->magnetometer->Get_M_Axes((int32_t *)&Magn);
- mag.get_m_axes((int32_t *)&Magn);
- p_customsensorservice->sendEnvMagnetometer(&Magn, TimeStamp, magOffset);
+ magnetometer.get_m_axes((int32_t *)&Magn);
+ p_customsensorservice->sendEnvMagnetometer(&Magn, TimeStamp, &magOffset);
} else if (p_customsensorservice->isAccGyroMagHandle(handle)) {
- p_customsensorservice->sendEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, magOffset);
+ p_customsensorservice->sendEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, &magOffset);
+
+ } else if (p_customsensorservice->isPresHumTempHandle(handle)) {
+ pt_sensor.get_temperature(&temp);
+ floatToInt(temp, &intPart, &decPart, 1);
+ TempToSend = intPart*10+decPart;
+// ht_sensor.get_humidity(&hum);
+ floatToInt(hum, &intPart, &decPart, 1);
+ HumToSend = intPart*10+decPart;
+ pt_sensor.get_pressure(&pres);
+ floatToInt(pres, &intPart, &decPart, 1);
+ PresToSend = intPart*100+decPart;
+ p_customsensorservice->sendEnvPresHumTemp (PresToSend, HumToSend, TempToSend, TimeStamp);
}
#endif
@@ -579,9 +493,7 @@
static void onDataWriteCallback(const GattWriteCallbackParams *eventDataP)
{
Gap::Handle_t handle = eventDataP->handle - BLE_HANDLE_VALUE_OFFSET;
- PRINTF("onEventCallback!!\n\r");
- printf (" myonDataWriteCallback attr_handle: %x att_data[4]: %d data_length: %d\n\r", eventDataP->handle, eventDataP->data[4], eventDataP->len );
-
+ PRINTF("onDataWriteCallback!!\n\r");
#ifdef CUST_CONFIG_SERV
if (p_customconfigservice->isConfHandle(handle)) {
@@ -615,14 +527,6 @@
}
#endif
-#ifdef CUST_CONS_SERV
- if (p_customconsoleservice->isTermHandle(handle)) {
- printf ("Console handle: %x data: %s\n\r", handle, eventDataP->data);
-// p_customconsoleservice->updateTerm((uint8_t*)eventDataP->data,eventDataP->len);
- }
-#endif
-
-
}
static void onDataSentCallback(unsigned count)
@@ -650,14 +554,13 @@
#endif
BlueButtonTimer.detach();
- printf (" butt time expired \n\r");
+// printf (" butt time expired \n\r");
#ifdef CUST_CONS_SERV
BytesToWrite = sprintf((char *)BufferToWrite, "nr%d push in %1.1fs\r\n",HowManyButtonPress, BUTTON_TIME);
p_customconsoleservice->updateTerm(BufferToWrite,BytesToWrite);
#endif
isBlueButtonSequence = false;
HowManyButtonPress =0;
-
}
/**
@@ -665,7 +568,11 @@
*/
static void BlueButtonPressed ()
{
- //printf (" CB BlueButtonPressed PRESSED!!!!!!!!! %d\n\r", HowManyButtonPress);
+#ifdef CUST_CONS_SERV
+static uint8_t BufferToWrite[W2ST_CONSOLE_MAX_CHAR_LEN];
+static uint8_t BytesToWrite;
+#endif
+// printf (" CB BlueButtonPressed PRESSED!!!!!!!!! %d\n\r", HowManyButtonPress);
if (HowManyButtonPress == 0) { // first push
BlueButtonTimer.attach(&Ticker_BlueButton, BUTTON_TIME);
HowManyButtonPress++;
@@ -674,39 +581,17 @@
HowManyButtonPress++;
if (HowManyButtonPress == BLUEMSYS_N_BUTTON_PRESS ) {
BlueButtonTimer.detach();
- printf (" CB BlueButtoon SEQUENCE!!!!!!!!! \n\r");
+// printf (" CB BlueButtoon SEQUENCE!!!!!!!!! \n\r");
HowManyButtonPress =0;
- if (BLEConnectionStatus) isBlueButtonSequence = true;
+ if (BLEConnectionStatus) isBlueButtonSequence = true;
+#ifdef CUST_CONS_SERV
+ BytesToWrite = sprintf((char *)BufferToWrite, "BlueButton SEQ\r\n");
+ p_customconsoleservice->updateTerm(BufferToWrite,BytesToWrite);
+#endif
}
}
}
-/**
- * CB Triggered periodically by the 'ticker' interrupt;
- */
-static void Ticker_Env(void)
-{
- char msg;
- msg = TMO_ENV;
- QueueMainActivity.put((char*)msg);
-}
-
-static void Ticker_Calib(void)
-{
- char msg;
- timeoutCalib = 1;
-}
-
-static void Ticker_Mems(void)
-{
- char msg;
- TimeStamp++;
- CounterAGM++;
- msg = TMO_MEMS;
- QueueMainActivity.put((char*)msg);
-}
-
-static int ble_started=0;
void onBleInitError(BLE &ble, ble_error_t error)
{
@@ -728,9 +613,8 @@
if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
return;
}
- char msg;
- msg = BLE_STARTED;
- QueueMainActivity.put((char*)msg);
+ char msg = BLE_STARTED;
+ QueueMainActivity.put((char*)msg);
}
void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
@@ -738,182 +622,108 @@
eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}
-/* Helper function for printing floats & doubles */
-static char *print_double(char* str, double v, int decimalDigits=2)
+
+void updateSensorValue (void)
{
- int i = 1;
- int intPart, fractPart;
- int len;
- char *ptr;
-
- /* prepare decimal digits multiplicator */
- for (;decimalDigits!=0; i*=10, decimalDigits--);
-
- /* calculate integer & fractinal parts */
- intPart = (int)v;
- fractPart = (int)((v-(double)(int)v)*i);
-
- /* fill in integer part */
- sprintf(str, "%i.", intPart);
-
- /* prepare fill in of fractional part */
- len = strlen(str);
- ptr = &str[len];
-
- /* fill in leading fractional zeros */
- for (i/=10;i>1; i/=10, ptr++) {
- if (fractPart >= i) {
- break;
- }
- *ptr = '0';
- }
-
- /* fill in (rest of) fractional part */
- sprintf(ptr, "%i", fractPart);
-
- return str;
-}
-
-
-void updateSensorValue() {
- // Do blocking calls or whatever is necessary for sensor polling.
- // In our case, we simply update the HRM measurement.
-
- float value1, value2;
- char buffer1[32], buffer2[32];
- int32_t axes[3];
-#ifdef CUST_SENS_SERV
- press_temp.get_temperature(&value1);
- press_temp.get_pressure(&value2);
- printf("LPS22HB: [temp] %7s C, [press] %s mbar\r\n", print_double(buffer1, value1), print_double(buffer2, value2));
-
- mag.get_m_axes(axes);
- printf("LSM303AGR [mag/mgauss]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-
- acc.get_x_axes(axes);
- printf("LSM303AGR [acc/mg]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-
- acc_gyro.get_x_axes(axes);
- printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-
- acc_gyro.get_g_axes(axes);
- printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-#endif
-}
-
-
-void periodicCallback(void)
-{
-// led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
- if (BLE::Instance().getGapState().connected) {
- // eventQueue.call(updateSensorValue);
- }
-}
-
-
-void ThreadMainActivity (void) {
- float temp, hum, pres;
+ float temp, hum, pres;
AxesRaw_TypeDef Magn, Acc, Gyro;
int32_t decPart, intPart;
int16_t TempToSend, HumToSend;
uint32_t PresToSend;
- osEvent RxEvt;
+#ifdef CUST_SW_SERV
+ static AxesRaw_TypeDef quat_axes[SEND_N_QUATERNIONS];
+ static float QuatFloat[SEND_N_QUATERNIONS];
+ static uint32_t QuaternionNumber =0;
+ static uint32_t CounterFloat =0;
+#endif
+ static unsigned int envTime =0;
+ static unsigned int CounterAGM =0;
- while(1) {
-
- RxEvt = QueueMainActivity.get();
- switch (RxEvt.value.v) {
-
- case TMO_MEMS:
- timeoutMems =1;
- break;
-
- case TMO_CALIB:
- break;
-
- case TMO_SENSOR_FUSION:
- break;
-
- case TMO_ENV:
- timeoutEnv =1;
- break;
-
- default:
- printf ("ERROR unrecognized event\n\r");
- }
-
+ TimeStamp++;
+#ifdef USE_SENSOR_FUSION_LIB
+ static unsigned int calibTime =0;
+ // Run Calibration from command
if (BLEConnectionStatus) {
if ((isBlueButtonSequence || ForceReCalibration) && SensorFusionOK ) {
printf("ForceReCalibration\r\n");
/* Reset the Compass Calibration */
isCal=0;
+ calibTime =0;
ForceReCalibration =0;
- isBlueButtonSequence = false;
+ isBlueButtonSequence = false;
+// greenled =0;
/* Notifications of Compass Calibration */
#ifdef CUST_CONFIG_SERV
p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
#endif
-#ifdef USE_SENSOR_FUSION_LIB
/* Reset the Calibration */
- osx_MotionFX_compass_forceReCalibration();
-#endif
ResetCalibrationInMemory();
-
- /* Reset Calibration offset */
- magOffset.magOffX = magOffset.magOffY= magOffset.magOffZ=0;
-// greenled =0;
+ MotionFX_manager_MagCal_start(SAMPLE_PERIOD);
}
- }
+ }
+#endif
+
// Update MEMS @ 100Hz and calculate quaternions
- if (timeoutMems && BLEConnectionStatus) {
- timeoutMems =0;
-
+ if (BLEConnectionStatus) {
#ifdef CUST_SENS_SERV
- int32_t err=IMU_6AXES_OK;
- err=acc_gyro.get_x_axes((int32_t *)&Acc);
- if (err != IMU_6AXES_OK) {
- printf ("= * ERROR %d GetAccelerometer\n\r", err);
+ int32_t err=0;
+ err = acc_gyro.get_x_axes((int32_t*)&Acc);
+ if (err != 0) {
+ printf ("= * ERROR %d GetAccelerometer\n\r", (int)err);
}
-
- err=acc_gyro.get_g_axes((int32_t *)&Gyro);
- if (err != IMU_6AXES_OK) {
- printf ("= * ERROR %d GetGyroscope\n\r", err);
- }
- err=mag.get_m_axes((int32_t *)&Magn);
- if (err != MAGNETO_OK) {
- printf ("= * ERROR %d Get_M_Axes\n\r", err);
+
+ err = acc_gyro.get_g_axes((int32_t *)&Gyro);
+ if (err != 0) {
+ printf ("= * ERROR %d GetGyroscope\n\r", (int)err);
+ }
+
+ err = magnetometer.get_m_axes((int32_t *)&Magn);
+ if (err != 0) {
+ printf ("= * ERROR %d Get_M_Axes\n\r", (int)err);
}
-
- if(CounterAGM >= ACC_GYRO_MAG_UPDATE_MUL_10MS) {
- CounterAGM=0;
+ if(CounterAGM >= ACC_GYRO_MAG_UPDATE_MUL_10MS) {
+ CounterAGM=0;
p_customsensorservice->updateEnvAccelerometer (&Acc, TimeStamp);
p_customsensorservice->updateEnvGyroscope(&Gyro, TimeStamp);
- p_customsensorservice->updateEnvMagnetometer(&Magn, TimeStamp, magOffset);
- p_customsensorservice->updateEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, magOffset);
- }
-#endif
-
+ p_customsensorservice->updateEnvMagnetometer(&Magn, TimeStamp, &magOffset);
+ p_customsensorservice->updateEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, &magOffset);
+ } else {
+ CounterAGM++;
+ }
+#endif
+
#ifdef USE_SENSOR_FUSION_LIB
if (SensorFusionOK) {
- MotionFX_manager_run((AxesRaw_t*)&Acc, (AxesRaw_t*)&Gyro, (AxesRaw_t*)&Magn, magOffset);
- p_MotionFX_Engine_Out = MotionFX_manager_getDataOUT();
- }
-#endif
-#ifdef CUST_SW_SERV
+ MotionFX_Engine_In.gyro[0] = Gyro.AXIS_X * FROM_MDPS_TO_DPS;
+ MotionFX_Engine_In.gyro[1] = Gyro.AXIS_Y * FROM_MDPS_TO_DPS;
+ MotionFX_Engine_In.gyro[2] = Gyro.AXIS_Z * FROM_MDPS_TO_DPS;
+
+ MotionFX_Engine_In.acc[0] = Acc.AXIS_X * FROM_MG_TO_G;
+ MotionFX_Engine_In.acc[1] = Acc.AXIS_Y * FROM_MG_TO_G;
+ MotionFX_Engine_In.acc[2] = Acc.AXIS_Z * FROM_MG_TO_G;
+
+ MotionFX_Engine_In.mag[0] = Magn.AXIS_X * FROM_MGAUSS_TO_UT50;
+ MotionFX_Engine_In.mag[1] = Magn.AXIS_Y * FROM_MGAUSS_TO_UT50;
+ MotionFX_Engine_In.mag[2] = Magn.AXIS_Z * FROM_MGAUSS_TO_UT50;
+
+ MotionFX_manager_run(p_MotionFX_Engine_In, p_MotionFX_Engine_Out, MOTIONFX_ENGINE_DELTATIME);
+ }
+#endif
+#ifdef CUST_SW_SERV
if (SensorFusionOK) {
- /* Update quaternions */
+ /* Update quaternions (compact)*/
if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) {
- quat_axes[QuaternionNumber].AXIS_X = (int)(p_MotionFX_Engine_Out->quaternion_9X[0] * (-10000));
- quat_axes[QuaternionNumber].AXIS_Y = (int)(p_MotionFX_Engine_Out->quaternion_9X[1] * (-10000));
- quat_axes[QuaternionNumber].AXIS_Z = (int)(p_MotionFX_Engine_Out->quaternion_9X[2] * (-10000));
+ quat_axes[QuaternionNumber].AXIS_X = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[0] * (-10000));
+ quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[1] * (-10000));
+ quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[2] * (-10000));
} else {
- quat_axes[QuaternionNumber].AXIS_X = (int)(p_MotionFX_Engine_Out->quaternion_9X[0] * 10000);
- quat_axes[QuaternionNumber].AXIS_Y = (int)(p_MotionFX_Engine_Out->quaternion_9X[1] * 10000);
- quat_axes[QuaternionNumber].AXIS_Z = (int)(p_MotionFX_Engine_Out->quaternion_9X[2] * 10000);
+ quat_axes[QuaternionNumber].AXIS_X = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[0] * 10000);
+ quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[1] * 10000);
+ quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[2] * 10000);
}
- if (QuaternionNumber == SEND_N_QUATERNIONS-1) {
+ if (QuaternionNumber >= SEND_N_QUATERNIONS-1) {
p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp);
QuaternionNumber =0;
} else {
@@ -922,7 +732,7 @@
/* Update Float Quaternions */
/* Every QUAT_FLOAT_UPDATE_MUL_10MS*10 mSeconds Send Float Quaternion informations via bluetooth */
- if(CounterFloat==QUAT_FLOAT_UPDATE_MUL_10MS) {
+ if(CounterFloat>=QUAT_FLOAT_UPDATE_MUL_10MS) {
if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) {
QuatFloat[0] = - p_MotionFX_Engine_Out->quaternion_9X[0];
QuatFloat[1] = - p_MotionFX_Engine_Out->quaternion_9X[1];
@@ -938,110 +748,134 @@
CounterFloat++;
}
}
-#endif
- }
-
- /* Check if is calibrated @ 25Hz */
- if(isCal!=0x01 && timeoutCalib && BLEConnectionStatus && SensorFusionOK ) {
- timeoutCalib =0;
+#endif
+ }
+#ifdef CUST_CONFIG_SERV
+ MFX_MagCal_input_t mag_data_in;
+ MFX_MagCal_output_t cal_data_out;
+ // Check if is calibrated @ 25Hz
+ calibTime = calibTime + MEMS_TIMER;
+ if(isCal!=0x01 && calibTime>= CALIB_TIMER && BLEConnectionStatus && SensorFusionOK ) {
+ calibTime = 0;
/* Run Compass Calibration @ 25Hz */
-#ifdef USE_SENSOR_FUSION_LIB
- osx_MotionFX_compass_saveAcc(Acc.AXIS_X, Acc.AXIS_Y, Acc.AXIS_Z); /* Accelerometer data ENU systems coordinate */
- osx_MotionFX_compass_saveMag(Magn.AXIS_X, Magn.AXIS_Y, Magn.AXIS_Z); /* Magnetometer data ENU systems coordinate */
- osx_MotionFX_compass_run();
-
+ mag_data_in.mag[0]= MotionFX_Engine_In.mag[0];
+ mag_data_in.mag[1]= MotionFX_Engine_In.mag[1];
+ mag_data_in.mag[2]= MotionFX_Engine_In.mag[2];
+ mag_data_in.time_stamp = TimeStamp;
+ MotionFX_manager_MagCal_run(&mag_data_in, &cal_data_out);
+
/* Control the calibration status */
- isCal = osx_MotionFX_compass_isCalibrated();
-#endif
- if(isCal == 0x01) {
-
- printf("Compass Calibrated\n\r");
- /* Get new magnetometer offset */
-#ifdef USE_SENSOR_FUSION_LIB
- osx_MotionFX_getCalibrationData(&magOffset);
-#endif
- /* Save the calibration in Memory */
- SaveCalibrationToMemory();
-
- /* Switch on the Led */
-// greenled = 1;
-
+ if( (cal_data_out.cal_quality == MFX_MAGCALOK) || (cal_data_out.cal_quality == MFX_MAGCALGOOD) )
+ {
+ magOffset.magOffX = (int32_t)(cal_data_out.hi_bias[0] * FROM_UT50_TO_MGAUSS);
+ magOffset.magOffY = (int32_t)(cal_data_out.hi_bias[1] * FROM_UT50_TO_MGAUSS);
+ magOffset.magOffZ = (int32_t)(cal_data_out.hi_bias[2] * FROM_UT50_TO_MGAUSS);
+
+ /* Disable magnetometer calibration */
+ MotionFX_manager_MagCal_stop(SAMPLE_PERIOD);
+
+ isCal= 1;
+// greenled = 1;
+ printf("Compass Calibrated\n\r");
+
+ SaveCalibrationToMemory();
+
/* Notifications of Compass Calibration */
-#ifdef CUST_CONFIG_SERV
p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
- p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
-#endif
+ p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
}
}
-
- // Update environmental sensors @ 2Hz
- if (timeoutEnv && BLEConnectionStatus) {
- timeoutEnv =0;
+#endif
+ // Update environmental sensors @ 2Hz
+ if (BLEConnectionStatus) {
+ envTime = envTime + MEMS_TIMER;
+ if ( envTime >= ENV_TIMER) {
+ envTime =0;
int32_t err;
#ifdef CUST_SENS_SERV
- if (p_customsensorservice->isTempNotificationEn()) {
- err=press_temp.get_temperature(&temp);
- if ( err != HUM_TEMP_OK) {
- printf ("= * ERROR %d GetTemperature\n\r", err);
- } else {
- floatToInt(temp, &intPart, &decPart, 1);
- TempToSend = intPart*10+decPart;
- p_customsensorservice->updateEnvTemperature(TempToSend, TimeStamp);
- }
- }
-#if 0 // Hum sensor not present on SensorTile (hum is HTS221 on mini cradle)
- if (p_customsensorservice->isHumNotificationEn()) {
-// err = p_mems_expansion_board->ht_sensor->GetHumidity(&hum); /** FIXME no hum sens on Tile */
- if ( err != HUM_TEMP_OK) {
- printf ("= * ERROR %d GetHumidity\n\r", err);
- } else {
- floatToInt(hum, &intPart, &decPart, 1);
- HumToSend = intPart*10+decPart;
- p_customsensorservice->updateEnvHumidity(HumToSend, TimeStamp);
- }
- }
+#ifdef DISAGGREGATED_ENV_CHAR
+ if (p_customsensorservice->isTempNotificationEn()) {
+ err = pt_sensor.get_temperature(&temp);
+ if ( err != 0) {
+ printf ("= * ERROR %d GetTemperature\n\r", (int)err);
+ } else {
+ floatToInt(temp, &intPart, &decPart, 1);
+ TempToSend = intPart*10+decPart;
+ p_customsensorservice->updateEnvTemperature(TempToSend, TimeStamp);
+ }
+ }
+
+ if (p_customsensorservice->isHumNotificationEn()) {
+// err = ht_sensor.get_humidity(&hum);
+ if ( err != 0) {
+ printf ("= * ERROR %d GetHumidity\n\r", (int)err);
+ } else {
+ floatToInt(hum, &intPart, &decPart, 1);
+ HumToSend = intPart*10+decPart;
+ p_customsensorservice->updateEnvHumidity(HumToSend, TimeStamp);
+ }
+ }
+
+ if (p_customsensorservice->isPresNotificationEn()) {
+ err = pt_sensor.get_pressure(&pres);
+ if ( err != 0) {
+ printf ("= * ERROR GetPressure\n\r");
+ } else {
+ floatToInt(pres, &intPart, &decPart, 1);
+ PresToSend = intPart*100+decPart;
+ p_customsensorservice->updateEnvPressure(PresToSend, TimeStamp);
+ }
+ }
+#endif
+ if (p_customsensorservice->isPresHumTempNotificationEn()) {
+ err = pt_sensor.get_pressure(&pres);
+ if ( err != 0) {
+ printf ("= * ERROR GetPressure\n\r");
+ } else {
+ floatToInt(pres, &intPart, &decPart, 1);
+ PresToSend = intPart*100+decPart;
+ }
+// err = ht_sensor.get_humidity(&hum);
+ if ( err != 0) {
+ printf ("= * ERROR %d GetHumidity\n\r", (int)err);
+ } else {
+ floatToInt(hum, &intPart, &decPart, 1);
+ HumToSend = intPart*10+decPart;
+ }
+ err = pt_sensor.get_temperature(&temp);
+ if ( err != 0) {
+ printf ("= * ERROR %d GetTemperature\n\r", (int)err);
+ } else {
+ floatToInt(temp, &intPart, &decPart, 1);
+ TempToSend = intPart*10+decPart;
+ }
+ p_customsensorservice->updateEnvPresHumTemp(PresToSend, HumToSend, TempToSend, TimeStamp);
+ }
#endif
- if (p_customsensorservice->isPresNotificationEn()) {
- err=press_temp.get_pressure(&pres);
- if ( err != PRESSURE_OK) {
- printf ("= * ERROR GetPressure\n\r");
- } else {
- floatToInt(pres, &intPart, &decPart, 1);
- PresToSend = intPart*100+decPart;
- p_customsensorservice->updateEnvPressure(PresToSend, TimeStamp);
- }
- }
-#endif
- }
- }
-}
+ }
+ }
+}
-
+void periodicCallback(void)
+{
+ eventQueue.call(updateSensorValue);
+}
+
+
int main()
{
- Ticker EnvTimer;
- Ticker CalibTimer;
- Ticker MemsTimer;
bool isgyro_lsm6ds0Present=false;
bool isgyro_lsm6ds3Present=false;
-#ifdef CUST_CONS_SERV
-// static uint8_t BufferToWrite[256];
-// static uint8_t BytesToWrite;
-#endif
-
-// DevI2C *i2c = new DevI2C(I2C_SDA, I2C_SCL);
-// i2c->frequency(NUCLEO_I2C_SHIELDS_SPEED);
+ TimeStamp =0;
#ifdef CUST_SENS_SERV
-// p_mems_expansion_board = X_NUCLEO_IKS01A1::Instance(i2c);
// if (p_mems_expansion_board->gyro_lsm6ds0) isgyro_lsm6ds0Present=true;
// if (p_mems_expansion_board->gyro_lsm6ds3) isgyro_lsm6ds3Present=true;
#endif
-//#ifdef CUST_CONFIG_SERV
+// greenled = 0;
// InterruptIn BlueButton(USER_BUTTON);
// BlueButton.fall(&BlueButtonPressed);
-//#endif
-
+
printf("\r\nSTMicroelectronics BlueMicrosystem1 W2ST:\r\n"
#ifdef CUST_SENS_SERV
"\tGyro lsmds0 present: %d, lsmds3 present: %d\n\r"
@@ -1055,25 +889,39 @@
#ifdef CUST_SENS_SERV
,isgyro_lsm6ds0Present,
isgyro_lsm6ds3Present,
- ENV_TIMER/1000,
- MEMS_TIMER/1000*ACC_GYRO_MAG_UPDATE_MUL_10MS
+ ENV_TIMER,
+ MEMS_TIMER*ACC_GYRO_MAG_UPDATE_MUL_10MS
#endif
#ifdef CUST_SW_SERV
- ,MEMS_TIMER/1000*SEND_N_QUATERNIONS,SEND_N_QUATERNIONS,
- MEMS_TIMER/1000*QUAT_FLOAT_UPDATE_MUL_10MS
+ ,MEMS_TIMER*SEND_N_QUATERNIONS,SEND_N_QUATERNIONS,
+ MEMS_TIMER*QUAT_FLOAT_UPDATE_MUL_10MS
#endif
);
-// eventQueue.call_every(500, periodicCallback);
+
+
+ /* Initialize MotionFX library */
+#ifdef USE_SENSOR_FUSION_LIB
+// bool DS3_OnBoard = DS3_ONBOARD;
+ MotionFX_manager_init ((unsigned char)LSM6DSL_G_0);
+ MotionFX_manager_start_9X();
+ SensorFusionOK = true;
+ printf("SW Service sensor fusion library added successfully\r\n");
+#else
+ printf("sensor fusion lib disabled \n\r");
+ SensorFusionOK = false;
+#endif
+
+ if (NUCLEO_I2C_SHIELDS_SPEED < MAX_I2C_SPEED)
+ printf ("Warning I2C speed set to: %d (max: %d)\n\r", NUCLEO_I2C_SHIELDS_SPEED, MAX_I2C_SPEED);
+
+ ble_started = 0;
BLE &ble = BLE::Instance();
- p_BLEdev = &ble;
+ p_BLEdev = &ble;
p_BLEdev->onEventsToProcess(scheduleBleEventsProcessing);
p_BLEdev->init(bleInitComplete);
- ble_started = 0;
osEvent RxEvt = QueueMainActivity.get(); // waiting for bleInitComplete cb to be called
- if (RxEvt.value.v == BLE_STARTED) ble_started = 1;
-
-// greenled = 0;//Switch OFF LED1
+ if (RxEvt.value.v == BLE_STARTED) ble_started = 1;
/* set the BLE CB functions */
p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
@@ -1085,23 +933,7 @@
p_BLEdev->gap().onConnection(onConnectionCallback);
p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
-/* p_BLEdev->gap().onTimeout(onTimeoutCallback);
-*/
- /* Initialize MotionFX library */
-#ifdef USE_SENSOR_FUSION_LIB
- bool DS3_OnBoard = DS3_ONBOARD;
- if (MotionFX_manager_init(DS3_OnBoard, magOffset)) {
- MotionFX_manager_start_9X();
- SensorFusionOK = true;
- printf("SW Service sensor fusion library added successfully\r\n");
- } else { /* sensor fusion lib init failed: likely because of wrong license */
- printf("sensor fusion lib init failed: likely because of wrong license \n\r");
- SensorFusionOK = false;
- }
-#else
- printf("sensor fusion lib disabled \n\r");
- SensorFusionOK = false;
-#endif
+ p_BLEdev->gap().onTimeout(onTimeoutCallback);
#ifdef CUST_CONFIG_SERV
p_customconfigservice = new CustomConfigService(*p_BLEdev);
@@ -1112,28 +944,40 @@
printf("SW Service W2ST calibration added successfully\r\n");
#endif
#ifdef CUST_SENS_SERV
- press_temp.enable();
- acc_gyro.enable_x();
- acc_gyro.enable_g();
- mag.enable();
- acc.enable();
-
- uint8_t id22=0, idLSM6=0, id303mag=0, id303acc=0;
- press_temp.read_id(&id22);
+ magnetometer.enable();
+ accelerometer.enable();
+ acc_gyro.enable_g();
+ acc_gyro.enable_x();
+// ht_sensor.enable();
+ pt_sensor.enable();
+
+ uint8_t id22=0, idLSM6=0, id303mag=0, id303acc=0, id221;
+ pt_sensor.read_id(&id22);
acc_gyro.read_id(&idLSM6);
- mag.read_id(&id303mag);
- acc.read_id(&id303acc);
+ magnetometer.read_id(&id303mag);
+ accelerometer.read_id(&id303acc);
+// ht_sensor.read_id(&id221);
- printf("LS303acc ID %x LS303mag ID %x LSM6DSL ID %x LPS22HB ID %x \r\n", id303acc, id303mag, idLSM6, id22);
- printf("\r\n");
-
+ printf("LS303acc ID %x LS303mag ID %x LSM6DSL ID %x LPS22HB ID %x \r\n", id303acc, id303mag, idLSM6, id22/*, id221*/);
+ printf("\r\n");
+
p_customsensorservice = new CustomSensorService(*p_BLEdev);
if (!p_customsensorservice) {
printf("\n\rHW Service W2ST sensors add FAILED!\n\r");
return 0;
}
printf("\rHW Service W2ST sensors added successfully\r\n");
-#endif
+#endif
+#ifdef CUST_SW_SERV
+ if (SensorFusionOK) {
+ p_customsoftwareservice = new CustomSoftwareService(*p_BLEdev);
+ if (!p_customsoftwareservice) {
+ printf("SW Service W2ST quaternions add FAILED!\n\r");
+ return 0;
+ }
+ printf("SW Service W2ST quaternions added successfully\r\n");
+ }
+#endif
#ifdef CUST_CONS_SERV
p_customconsoleservice = new CustomConsoleService(*p_BLEdev);
if (!p_customconsoleservice) {
@@ -1142,18 +986,6 @@
}
printf("\rHW Service W2ST console added successfully\r\n");
#endif
-#ifdef CUST_SW_SERV
- AxesRaw_TypeDef quat_axes[SEND_N_QUATERNIONS];
- float QuatFloat[SEND_N_QUATERNIONS];
- if (SensorFusionOK) {
- p_customsoftwareservice = new CustomSoftwareService(*p_BLEdev);
- if (!p_customsoftwareservice) {
- printf("SW Service W2ST quaternions add FAILED!\n\r");
- return 0;
- }
- printf("SW Service W2ST quaternions added successfully\r\n");
- }
-#endif
const static char DEVICE_NAME[] = BLE_DEV_NAME;
/* Setup advertising. */
@@ -1162,48 +994,34 @@
#ifdef USE_SENSOR_FUSION_LIB
uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x01,0x80};
#else
- uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x00,0x00};
+// uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x00,0x00};
+ uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x00,0x00};
#endif
p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);
p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
-
- // p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, SENS_SERVICE_UUID_128, sizeof(SENS_SERVICE_UUID_128));
-
p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
-
-
+
printf("SERVER: BLE Stack Initialized\r\n");
printf("Starting Advertising...\n\r");
p_BLEdev->gap().startAdvertising();
-
- TimeStamp =0;
- EnvTimer.attach_us(&Ticker_Env, ENV_TIMER);
-// CalibTimer.attach_us(&Ticker_Calib, CALIB_TIMER );
- MemsTimer.attach_us(&Ticker_Mems, MEMS_TIMER);
-
+
/* Control if the calibration is already available in memory */
+#ifdef CUST_CONFIG_SERV
if (SensorFusionOK) {
- ReCallCalibrationFromMemory();
+ ReCallCalibrationFromMemory();
- /* Switch on/off the LED according to calibration */
- if(isCal) {
-// greenled =1;
- } else {
- // greenled =0;
- }
- }
-#ifdef CUST_SW_SERV
- uint32_t QuaternionNumber =0;
- uint32_t CounterFloat =0;
-#endif
-
- Thread ThObjMainActivity;
- ThObjMainActivity.start(Callback<void()>(ThreadMainActivity));
-
+ /* Switch on/off the LED according to calibration */
+ if(isCal) {
+// greenled =1;
+ } else {
+// greenled =0;
+ }
+ }
+#endif
+ eventQueue.call_every(MEMS_TIMER, periodicCallback); // Start the activity
eventQueue.dispatch_forever();
}
-