diff --git a/.github/workflows/CodeSpell.yml b/.github/workflows/CodeSpell.yml new file mode 100644 index 0000000..edf8090 --- /dev/null +++ b/.github/workflows/CodeSpell.yml @@ -0,0 +1,27 @@ +name: Check misspelled words with codespell + +on: + push: + branches: + - main + pull_request: + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: +jobs: + codespell: + name: Check for spelling errors + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@main + + # See: https://github.com/codespell-project/actions-codespell/blob/master/README.md + - name: Spell check + uses: codespell-project/actions-codespell@master + with: + check_filenames: true + check_hidden: true + # In the event of a false positive, add the word in all lower case to this file: + ignore_words_file: ./extras/.codespellignore + skip: ./.git,./extras/.codespellignore diff --git a/README.md b/README.md index 5e2e474..616532f 100644 --- a/README.md +++ b/README.md @@ -23,12 +23,12 @@ motion sensors (humidity, temperature, pressure, accelerometer, gyroscope). The to SensiBLE and see sensors data. The data are also printed to the serial port. * Flight1: This application provides an example of usage of a NUCLEO board (it was tested with NUCLEO-F401RE, NUCLEO-L476RG, NUCLEO-L152RE) with X-NUCLEO-IKS01A2 or X-NUCLEO-IKS01A3, X-NUCLEO-IDB05A1 and X-NUCLEO-53L1A1 Expansion Boards. The application gather data from the sensor mounted and then communicates -them via bluetooth to a suitable Android app (such as ST BLE Sensor). The data gathered include enviromental data (temperature, pression, humidity), +them via bluetooth to a suitable Android app (such as ST BLE Sensor). The data gathered include environmental data (temperature, pression, humidity), distance, gesture recognition (tap and directional swipe), acceleration (with accelerometer hardware events) and a gyroscope. -* Flight1v2: This application provides an example of usage of a NUCLEO board (it was tested with NUCLEO-F401RE) with -X-NUCLEO-IKS01A3, X-NUCLEO-IDB05A2 and X-NUCLEO-53L1A1 Expansion Boards. The application gathers data from the sensors mounted and then communicates -them via bluetooth to a suitable Android or iOS app (such as ST BLE Sensor). The data gathered include enviromental data (temperature, pression, humidity), -distance, gesture recognition (tap and directional swipe), acceleration (with accelerometer hardware events), a gyroscope and a magnetometer. Note that +* Flight1v2: This application provides an example of usage of a NUCLEO board (it was tested with a NUCLEO-F401RE and a NUCLEO-L476RG) with +X-NUCLEO-IKS01A3, X-NUCLEO-IDB05A2, X-NUCLEO-BNRG2A1 and X-NUCLEO-53L1A1 Expansion Boards. The application gathers data from the sensors mounted and then communicates +them via bluetooth to a suitable Android or iOS app (such as ST BLE Sensor). The data gathered include environmental data (temperature, pression, humidity), +distance, gesture recognition (tap and directional swipe), acceleration (with accelerometer hardware events), a gyroscope and a magnetometer. In addition, 9x sensor fusion functionality for accurate orientation detection is also available. Note that unlike the Flight1 application, this one makes use of the STM32duinoBLE API instead of SPBTLE-RF, and does not support X-NUCLEO-IKS01A2 boards. In addition, an up-to-date version of the STM32duinoBLE library is required, as this application uses some of the newest features. * IKS01A3_S2LP_P2P_Demo: This application provides a simple example of usage of two NUCLEO boards (it was tested with NUCLEO-F401RE and NUCLEO-L053R8) with a X-NUCLEO-IKS01A3 and one among X-NUCLEO-S2868A1, X-NUCLEO-S2868A2 and X-NUCLEO-S2915A1. It shows how to send and receive all the sensor data @@ -66,6 +66,7 @@ The FP_Examples library requires the following STM32duino libraries (link to the * STM32duino M95640-R: https://github.com/stm32duino/M95640-R * STM32duino X-NUCLEO-IKS02A1 Audio: https://github.com/stm32duino/X-NUCLEO-IKS02A1-Audio * STM32duinoBLE: https://github.com/stm32duino/STM32duinoBLE +* STM32duino MotionFX: https://github.com/stm32duino/MotionFX ## Documentation diff --git a/examples/BleSensors_SensiBLE/BleSensors_SensiBLE.ino b/examples/BleSensors_SensiBLE/BleSensors_SensiBLE.ino index aeeeb48..2979052 100644 --- a/examples/BleSensors_SensiBLE/BleSensors_SensiBLE.ino +++ b/examples/BleSensors_SensiBLE/BleSensors_SensiBLE.ino @@ -209,7 +209,7 @@ void Print_Sensors() SerialPort.print("Temp[C] (HTS221): "); SerialPort.println(temperature, 2); // Print LPS25 data - SerialPort.print("Pres[hPa]: "); + SerialPort.print("Press[hPa]: "); SerialPort.println(pressure, 2); SerialPort.print("TempP[C] (LPS25): "); SerialPort.println(temperatureP, 2); diff --git a/examples/Flight1/Flight1.ino b/examples/Flight1/Flight1.ino index fb0ccb5..fad2720 100644 --- a/examples/Flight1/Flight1.ino +++ b/examples/Flight1/Flight1.ino @@ -95,7 +95,7 @@ volatile int mems_event = 0; volatile uint8_t AccGyroMag_Enable = 0; -volatile uint8_t Enviroment_Enable = 0; +volatile uint8_t Environment_Enable = 0; volatile uint8_t Distance_Enable = 0; volatile uint8_t Gestures_Enable = 0; @@ -360,7 +360,7 @@ fail: return 0; } - /*Update enviromental data (press*/ + /*Update environmental data (press*/ tBleStatus Environmental_Update(int32_t Press,uint16_t Hum,int16_t Temp) { tBleStatus ret; @@ -656,12 +656,12 @@ fail: uint8_t EnvironmentalCharSize=2; /* Size for Environmental BLE characteristic */ uint8_t manuf_data[30] = { - 2 /* lenght*/,0x0A,0x00 /* 0 dBm */, // Trasmission Power - 8 /* lenght*/,0x09,NAME_FLIGHT1, // Complete Name - 13 /* lenght*/ ,0xFF,0x01,/*SKD version */ + 2 /* length*/,0x0A,0x00 /* 0 dBm */, // Transmission Power + 8 /* length*/,0x09,NAME_FLIGHT1, // Complete Name + 13 /* length*/ ,0xFF,0x01,/*SKD version */ 0x80, /* NUCLEO-Board */ 0x02, /* Prox */ - 0xE0, /* ACC+Gyro+Mag+Pres+Hum+Temp */ + 0xE0, /* ACC+Gyro+Mag+Press+Hum+Temp */ 0x00, /* SensorFusionShort */ 0x00, /* SensorFusionFloat */ 0x00, /* BLE MAC start */ @@ -921,13 +921,13 @@ void Attribute_Modified_CB(uint16_t attr_handle, uint8_t * att_data, uint8_t dat { if (att_data[0] == 01) { - Enviroment_Enable = 1; - FLIGHT1_PRINTF("Enviroment enabled\n"); + Environment_Enable = 1; + FLIGHT1_PRINTF("Environment enabled\n"); } else if (att_data[0] == 0) { - Enviroment_Enable = 0; - FLIGHT1_PRINTF("Enviroment disabled\n"); + Environment_Enable = 0; + FLIGHT1_PRINTF("Environment disabled\n"); } } else if(attr_handle == GestureDetCharHandle + 2) @@ -1169,9 +1169,9 @@ void loop() BTLE.update(); if(Flight1.connected) { - //Get enviroment data + //Get environment data float humidity, temperature, pressure; - if (Enviroment_Enable) + if (Environment_Enable) { HumTemp.GetHumidity(&humidity); #ifdef USE_IKS01A3 @@ -1452,7 +1452,7 @@ void loop() } //Send all mems sensors data - if (Enviroment_Enable) + if (Environment_Enable) { Flight1.Environmental_Update(PressToSend, HumToSend, TempToSend); delay(30); diff --git a/examples/Flight1v2/Flight1v2.ino b/examples/Flight1v2/Flight1v2.ino index 9e59b26..e3f765d 100644 --- a/examples/Flight1v2/Flight1v2.ino +++ b/examples/Flight1v2/Flight1v2.ino @@ -2,10 +2,10 @@ ****************************************************************************** @file Flight1v2.ino @author STMicroelectronics - @version V1.0.0 - @date 15 March 2023 + @version V1.2.0 + @date 30 March 2023 @brief Arduino demo application for the STMicrolectronics - X-NUCLEO-IKS01A3, X-NUCLEO-53L1A1 + X-NUCLEO-IKS01A3, X-NUCLEO-53L1A1, X-NUCLEO-BNRG2A1 and X-NUCLEO-IDB05A1 ****************************************************************************** @attention @@ -57,24 +57,45 @@ #include #include +#if (__CORTEX_M == 0U) +#include "motion_fx_cm0p.h" +#else +#include "motion_fx.h" +#endif +#include "LSM6DSOSensor.h" +#include "LIS2MDLSensor.h" + #include #define DEV_I2C Wire #define SerialPort Serial -//#define DEBUG_MODE - #define INT_1 4 #define INT_2 5 +//#define DEBUG_MODE + +//#define USE_BNRG2A1 // By default, the IDB05A2 is used. Uncomment this to use the BNRG2A1 instead. + // BLE boards /* Shield IDB05A2 with SPI clock on D3 */ +#ifndef USE_BNRG2A1 SPIClass SpiHCI(D11, D12, D3); HCISpiTransportClass HCISpiTransport(SpiHCI, BLUENRG_M0, A1, A0, D7, 8000000, SPI_MODE0); #if !defined(FAKE_BLELOCALDEVICE) BLELocalDevice BLEObj(&HCISpiTransport); BLELocalDevice &BLE = BLEObj; #endif +#endif + +#ifdef USE_BNRG2A1 +SPIClass SpiHCI(D11, D12, D3); +HCISpiTransportClass HCISpiTransport(SpiHCI, BLUENRG_M2SP, A1, A0, D7, 1000000, SPI_MODE1); +#if !defined(FAKE_BLELOCALDEVICE) +BLELocalDevice BLEObj(&HCISpiTransport); +BLELocalDevice &BLE = BLEObj; +#endif +#endif // Interrupts volatile int mems_event = 0; @@ -125,6 +146,46 @@ volatile int mems_event = 0; #define FEATURE_MASK_ACC_EVENTS 0x00000400u +// Sensor Fusion +#define ALGO_FREQ 100U /* Algorithm frequency 100Hz */ +#define ALGO_PERIOD (1000U / ALGO_FREQ) /* Algorithm period [ms] */ +#define MOTION_FX_ENGINE_DELTATIME 0.01f // Originally 0.01f +#define FROM_MG_TO_G 0.001f +#define FROM_G_TO_MG 1000.0f +#define FROM_MDPS_TO_DPS 0.001f +#define FROM_DPS_TO_MDPS 1000.0f +#define FROM_MGAUSS_TO_UT50 (0.1f/50.0f) +#define FROM_UT50_TO_MGAUSS 500.0f + +#define STATE_SIZE (size_t)(2432) + +#define SAMPLETODISCARD 15 + +#define GBIAS_ACC_TH_SC (2.0f*0.000765f) +#define GBIAS_GYRO_TH_SC (2.0f*0.002f) +#define GBIAS_MAG_TH_SC (2.0f*0.001500f) + +#define DECIMATION 1U +#define FUSION_FRAME 30 // Approximately 30 fps/ups + +/* Private variables ---------------------------------------------------------*/ +#if !(__CORTEX_M == 0U) +static MFX_knobs_t iKnobs; +static MFX_knobs_t *ipKnobs = &iKnobs; +static uint8_t mfxstate[STATE_SIZE]; +#endif + +static volatile int sampleToDiscard = SAMPLETODISCARD; +static int discardedCount = 0; + +static volatile uint32_t TimeStamp = 0; + +volatile uint8_t fusion_flag; + +bool mag_calibrated = false; + +HardwareTimer *MyTim; + // Distance components STMPE1600DigiOut xshutdown_top(&DEV_I2C, GPIO_15, (0x42 * 2)); STMPE1600DigiOut xshutdown_left(&DEV_I2C, GPIO_14, (0x43 * 2)); @@ -147,6 +208,12 @@ LPS22HHSensor PressTemp(&DEV_I2C); HTS221Sensor HumTemp(&DEV_I2C); STTS751Sensor Temp(&DEV_I2C); +// MEMS variables +int32_t accelerometer[3]; +int32_t gyroscope[3]; +int32_t magnetometer[3]; +int32_t MagOffset[3]; + // STRING UUIDs const char *uuidSensorService = "00000000-0001-11e1-9ab4-0002a5d5c51b"; const char *uuidConfigService = "00000000-000f-11e1-9ab4-0002a5d5c51b"; @@ -160,6 +227,7 @@ const char *uuidProxChar = "02000000-0001-11e1-ac36-0002a5d5c51b"; const char *uuidGestureChar = "00000004-0001-11e1-ac36-0002a5d5c51b"; const char *uuidAccEventChar = "00000400-0001-11e1-ac36-0002a5d5c51b"; const char *uuidConfigChar = "00000002-000f-11e1-ac36-0002a5d5c51b"; +const char *uuidFusionChar = "00000100-0001-11e1-ac36-0002a5d5c51b"; #define LEN_DISTANCE 4 #define LEN_GESTURE 3 @@ -170,6 +238,7 @@ const char *uuidConfigChar = "00000002-000f-11e1-ac36-0002a5d5c51b"; #define LEN_PRESS 6 #define LEN_HUM 4 #define LEN_TEMP 4 +#define LEN_FUSION 20 #define LEN_CFG 20 // ABLE services and characteristics @@ -188,6 +257,8 @@ BLECharacteristic accEventC(uuidAccEventChar, BLERead | BLENotify, LEN_ACCEVENT) BLECharacteristic gyroC(uuidGyroChar, BLERead | BLENotify, LEN_GYRO); BLECharacteristic magC(uuidMagChar, BLERead | BLENotify, LEN_MAG); +BLECharacteristic fusionC(uuidFusionChar, BLERead | BLENotify, LEN_FUSION); + BLECharacteristic configC(uuidConfigChar, BLENotify | BLEWrite, LEN_CFG); // Class for bluetooth communication and services @@ -214,7 +285,7 @@ class Flight1Service { uint8_t addr[6] = {0xff}; BLE.getRandomAddress(addr); -#define FEATURE_MASK 0x02,0xfc,0x04,0x15 +#define FEATURE_MASK 0x02,0xfc,0x05,0x15 uint8_t data [14] = {0x0d, 0xff, 0x01, 0x80, FEATURE_MASK}; @@ -236,7 +307,7 @@ class Flight1Service { return ret; } - // Update enviromental data + // Update environmental data int Environmental_Update(int32_t Press, uint16_t Hum, int16_t Temp) { uint8_t pBuff[LEN_PRESS]; @@ -330,7 +401,7 @@ class Flight1Service { return ret; } - int AccEvent_Notify(uint16_t steps, uint8_t event) + int AccEvent_Notify(uint8_t event) { uint8_t buff_2[2 + 1]; // Only event uint8_t buff_4[5]; // Event and pedometer @@ -338,18 +409,33 @@ class Flight1Service { int ret = 0; if (shortMode) { - STORE_LE_16(buff_4, millis()); + STORE_LE_16(buff_2, millis()); buff_2[2] = event; ret += accEventC.writeValue(buff_2, 2 + 1); } else { STORE_LE_16(buff_4, millis()); buff_4[2] = event; - STORE_LE_16(buff_4 + 3, steps); + STORE_LE_16(buff_4 + 3, globalSteps); ret += accEventC.writeValue(buff_4, LEN_ACCEVENT); } return ret; } + int Fusion_Update(int16_t qi, int16_t qj, int16_t qk) + { + uint8_t buf[LEN_FUSION] = {0x00}; + STORE_LE_16(buf, millis()); + STORE_LE_16(buf + 2, qi); + STORE_LE_16(buf + 4, qj); + STORE_LE_16(buf + 6, qk); + + + + int ret = 0; + ret += fusionC.writeValue(buf, 8); + return ret; + } + int Config_Notify(uint8_t Feature [4], uint8_t Command, uint8_t data) { uint8_t buff[2 + 4 + 1 + 1]; @@ -382,6 +468,7 @@ class Flight1Service { sensorService.addCharacteristic(accEventC); sensorService.addCharacteristic(gyroC); sensorService.addCharacteristic(magC); + sensorService.addCharacteristic(fusionC); configService.addCharacteristic(configC); @@ -448,13 +535,14 @@ void INT2Event_cb() mems_event = 1; } +void fusion_update(void) +{ + fusion_flag = 1; +} - -void configCB(BLEDevice unused1, BLECharacteristic unused2) +void configCB(BLEDevice, BLECharacteristic) { uint8_t buf[LEN_CFG]; - (void)unused1; - (void)unused2; configC.readValue(buf, LEN_CFG); char command = buf[4]; @@ -487,7 +575,7 @@ void configCB(BLEDevice unused1, BLECharacteristic unused2) break; case 'o': data ? Flight1.shortMode = true : Flight1.shortMode = false; - data ? AccGyr.Enable_6D_Orientation(LSM6DSO_INT1_PIN) : AccGyr.Disable_6D_Orientation();; + data ? AccGyr.Enable_6D_Orientation(LSM6DSO_INT1_PIN) : AccGyr.Disable_6D_Orientation(); break; } } @@ -502,17 +590,29 @@ int gestureGuard = 0; bool envEnable = false; bool proxEnable = false; -bool eventEnable = false; bool accEnable = false; bool gyroEnable = false; bool magEnable = false; +bool fusionEnable = false; + +long fusionTime = 0; +long currTime = 0; +long lastTime = 0; + +long upsTiming = 0; void setup() { + // Initialize serial port SerialPort.begin(115200); + while (!SerialPort); + + // Initialize I2C bus DEV_I2C.begin(); + DEV_I2C.setClock(400000); - pinMode(LED_BUILTIN, OUTPUT); //D13 LED + //D13 LED + pinMode(LED_BUILTIN, OUTPUT); //Interrupts. attachInterrupt(INT_1, INT1Event_cb, RISING); @@ -552,7 +652,6 @@ void setup() SetupSingleShot(&sensor_vl53l1x_left); SetupSingleShot(&sensor_vl53l1x_right); - //Top sensor should be in long distance mode sensor_vl53l1x_top.VL53L1X_SetDistanceMode(2); @@ -575,12 +674,68 @@ void setup() PressTemp.begin(); PressTemp.Enable(); AccGyr.begin(); + AccGyr.Set_X_ODR((float)ALGO_FREQ); + AccGyr.Set_X_FS(4); AccGyr.Enable_X(); - AccGyr.Set_X_ODR(4.0f); + AccGyr.Set_G_ODR((float)ALGO_FREQ); + AccGyr.Set_G_FS(2000); AccGyr.Enable_G(); Mag.begin(); + Mag.SetOutputDataRate((float)ALGO_FREQ); Mag.Enable(); + delay(10); + + // Initialize sensor fusion +#if (__CORTEX_M == 0U) + MotionFX_CM0P_initialize(MFX_CM0P_MCU_STM32); + MotionFX_CM0P_setOrientation("seu", "seu", "neu"); + MotionFX_CM0P_enable_gbias(MFX_CM0P_ENGINE_ENABLE); + MotionFX_CM0P_enable_euler(MFX_CM0P_ENGINE_ENABLE); + MotionFX_CM0P_enable_6X(MFX_CM0P_ENGINE_DISABLE); + MotionFX_CM0P_enable_9X(MFX_CM0P_ENGINE_ENABLE); + + /* Enable magnetometer calibration */ + MotionFX_CM0P_MagCal_init(ALGO_PERIOD, 1); +#else + MotionFX_initialize((MFXState_t *)mfxstate); + + MotionFX_getKnobs(mfxstate, ipKnobs); + + ipKnobs->acc_orientation[0] = 's'; + ipKnobs->acc_orientation[1] = 'e'; + ipKnobs->acc_orientation[2] = 'u'; + ipKnobs->gyro_orientation[0] = 's'; + ipKnobs->gyro_orientation[1] = 'e'; + ipKnobs->gyro_orientation[2] = 'u'; + ipKnobs->mag_orientation[0] = 'n'; + ipKnobs->mag_orientation[1] = 'e'; + ipKnobs->mag_orientation[2] = 'u'; + + ipKnobs->gbias_acc_th_sc = GBIAS_ACC_TH_SC; + ipKnobs->gbias_gyro_th_sc = GBIAS_GYRO_TH_SC; + ipKnobs->gbias_mag_th_sc = GBIAS_MAG_TH_SC; + + ipKnobs->output_type = MFX_ENGINE_OUTPUT_ENU; + ipKnobs->LMode = 1; + ipKnobs->modx = DECIMATION; + + MotionFX_setKnobs(mfxstate, ipKnobs); + MotionFX_enable_6X(mfxstate, MFX_ENGINE_DISABLE); + MotionFX_enable_9X(mfxstate, MFX_ENGINE_ENABLE); + + /* Enable magnetometer calibration */ + MotionFX_MagCal_init(ALGO_PERIOD, 1); +#endif + MyTim = new HardwareTimer(TIM3); + MyTim->setOverflow(ALGO_FREQ, HERTZ_FORMAT); + MyTim->attachInterrupt(fusion_update); + MyTim->resume(); + + currTime = millis(); + lastTime = millis(); + + // Prepare functionality callback disableAllFunc(); configC.setEventHandler(BLEWritten, configCB); } @@ -603,15 +758,165 @@ void loop() BLE.poll(); - envEnable = pressC.subscribed() || tempC.subscribed() || humC.subscribed(); - accEnable = accC.subscribed(); - gyroEnable = gyroC.subscribed(); - magEnable = magC.subscribed(); - eventEnable = accEventC.subscribed(); - proxEnable = distanceC.subscribed() || gestureC.subscribed(); + // If using the slower bluetooth chip BNRG2A1, we slow down the update rate to 30 ups +#ifdef USE_BNRG2A1 + // Calculate how much time passed since the last update + bool newUpdate = (millis() - upsTiming) > 33 ; // If it is over 33 milliseconds, activate the update flag + if (millis() - upsTiming > 33) { + upsTiming = millis(); // Also update the time of last update to current time + } + // Otherwise, we place no limit on update rate +#else + bool newUpdate = true; +#endif + + envEnable = (pressC.subscribed() || tempC.subscribed() || humC.subscribed()) && newUpdate; + accEnable = accC.subscribed() && newUpdate; + gyroEnable = gyroC.subscribed() && newUpdate; + magEnable = magC.subscribed() && newUpdate; + proxEnable = (distanceC.subscribed() || gestureC.subscribed()) && newUpdate; + fusionEnable = fusionC.subscribed() && newUpdate; + + if (fusionEnable) { + if (!mag_calibrated) { + if (fusion_flag) { + float ans_float; +#if (__CORTEX_M == 0U) + MFX_CM0P_MagCal_input_t mag_data_in; + MFX_CM0P_MagCal_output_t mag_data_out; +#else + MFX_MagCal_input_t mag_data_in; + MFX_MagCal_output_t mag_data_out; +#endif + fusion_flag = 0; + Mag.GetAxes(magnetometer); +#if (__CORTEX_M == 0U) + mag_data_in.Mag[0] = (float)magnetometer[0] * FROM_MGAUSS_TO_UT50; + mag_data_in.Mag[1] = (float)magnetometer[1] * FROM_MGAUSS_TO_UT50; + mag_data_in.Mag[2] = (float)magnetometer[2] * FROM_MGAUSS_TO_UT50; + MotionFX_CM0P_MagCal_run(&mag_data_in); + MotionFX_CM0P_MagCal_getParams(&mag_data_out); + + if (mag_data_out.CalQuality == MFX_CM0P_CALQSTATUSBEST) { + mag_calibrated = true; + ans_float = (mag_data_out.HI_Bias[0] * FROM_UT50_TO_MGAUSS); + MagOffset[0] = (int32_t)ans_float; + ans_float = (mag_data_out.HI_Bias[1] * FROM_UT50_TO_MGAUSS); + MagOffset[1] = (int32_t)ans_float; + ans_float = (mag_data_out.HI_Bias[2] * FROM_UT50_TO_MGAUSS); + MagOffset[2] = (int32_t)ans_float; + /* Disable magnetometer calibration */ + MotionFX_CM0P_MagCal_init(ALGO_PERIOD, 0); + digitalWrite(LED_BUILTIN, HIGH); + SerialPort.println("Magnetomer calibration done!"); + } +#else + mag_data_in.mag[0] = (float)magnetometer[0] * FROM_MGAUSS_TO_UT50; + mag_data_in.mag[1] = (float)magnetometer[1] * FROM_MGAUSS_TO_UT50; + mag_data_in.mag[2] = (float)magnetometer[2] * FROM_MGAUSS_TO_UT50; + mag_data_in.time_stamp = (int)TimeStamp; + + TimeStamp += (uint32_t)ALGO_PERIOD; + + MotionFX_MagCal_run(&mag_data_in); + MotionFX_MagCal_getParams(&mag_data_out); + + if (mag_data_out.cal_quality == MFX_MAGCALGOOD) { + mag_calibrated = true; + ans_float = (mag_data_out.hi_bias[0] * FROM_UT50_TO_MGAUSS); + MagOffset[0] = (int32_t)ans_float; + ans_float = (mag_data_out.hi_bias[1] * FROM_UT50_TO_MGAUSS); + MagOffset[1] = (int32_t)ans_float; + ans_float = (mag_data_out.hi_bias[2] * FROM_UT50_TO_MGAUSS); + MagOffset[2] = (int32_t)ans_float; + /* Disable magnetometer calibration */ + MotionFX_MagCal_init(ALGO_PERIOD, 0); + digitalWrite(LED_BUILTIN, HIGH); + SerialPort.println("Magnetomer calibration done!"); + } +#endif + } + } else { + if (fusion_flag) { +#if (__CORTEX_M == 0U) + MFX_CM0P_input_t data_in; + MFX_CM0P_output_t data_out; +#else + MFX_input_t data_in; + MFX_output_t data_out; +#endif + currTime = millis(); + float delta_time = ((float)(currTime - lastTime)) / 1000.0f; // dT is in fractions of a second, so difference/1000 milliseconds + lastTime = currTime; + fusion_flag = 0; + AccGyr.Get_X_Axes(accelerometer); + AccGyr.Get_G_Axes(gyroscope); + Mag.GetAxes(magnetometer); + + /* Convert angular velocity from [mdps] to [dps] */ + data_in.gyro[0] = (float)gyroscope[0] * FROM_MDPS_TO_DPS; + data_in.gyro[1] = (float)gyroscope[1] * FROM_MDPS_TO_DPS; + data_in.gyro[2] = (float)gyroscope[2] * FROM_MDPS_TO_DPS; + + /* Convert acceleration from [mg] to [g] */ + data_in.acc[0] = (float)accelerometer[0] * FROM_MG_TO_G; + data_in.acc[1] = (float)accelerometer[1] * FROM_MG_TO_G; + data_in.acc[2] = (float)accelerometer[2] * FROM_MG_TO_G; + + /* Convert magnetic field intensity from [mGauss] to [uT / 50] */ + data_in.mag[0] = (float)(magnetometer[0] - MagOffset[0]) * FROM_MGAUSS_TO_UT50; + data_in.mag[1] = (float)(magnetometer[1] - MagOffset[1]) * FROM_MGAUSS_TO_UT50; + data_in.mag[2] = (float)(magnetometer[2] - MagOffset[2]) * FROM_MGAUSS_TO_UT50; + + if (discardedCount == sampleToDiscard) { +#if (__CORTEX_M == 0U) + MotionFX_CM0P_update(&data_out, &data_in, delta_time); +#else + MotionFX_propagate(mfxstate, &data_out, &data_in, &delta_time); + MotionFX_update(mfxstate, &data_out, &data_in, &delta_time, NULL); +#endif + + // Get data +#if (__CORTEX_M == 0U) + double qi = data_out.quaternion_6X[0]; + double qj = data_out.quaternion_6X[1]; + double qk = data_out.quaternion_6X[2]; + double qs = data_out.quaternion_6X[3]; +#else + double qi = data_out.quaternion[0]; + double qj = data_out.quaternion[1]; + double qk = data_out.quaternion[2]; + double qs = data_out.quaternion[3]; + +#endif + + // Do math on data + int32_t iqi, iqj, iqk; + + if (qs < 0) { + iqi = (int32_t)(qi * (-10000)); + iqj = (int32_t)(qj * (-10000)); + iqk = (int32_t)(qk * (-10000)); + } else { + iqi = (int32_t)(qi * (10000)); + iqj = (int32_t)(qj * (10000)); + iqk = (int32_t)(qk * (10000)); + } + + // Transmit data + if ((millis() - fusionTime > FUSION_FRAME)) { + Flight1.Fusion_Update(iqi, iqj, iqk); + fusionTime = millis(); + } + } else { + discardedCount++; + } + } + } + } if (envEnable) { - //Get enviroment data + //Get environment data float humidity, temperature, pressure; HumTemp.GetHumidity(&humidity); Temp.GetTemperature(&temperature); @@ -624,10 +929,6 @@ void loop() TempToSend = intPart * 10 + decPart; } - int32_t accelerometer[3]; - int32_t gyroscope[3]; - int32_t magnetometer[3]; - if (accEnable) { // Read accelerometer AccGyr.Get_X_Axes(accelerometer); @@ -641,6 +942,7 @@ void loop() Mag.GetAxes(magnetometer); } + // Do note that this is interrupt-based: it does NOT follow the timing rules of all other processes if (mems_event) { mems_event = 0; LSM6DSO_Event_Status_t Astatus; @@ -709,7 +1011,7 @@ void loop() FLIGHT1_PRINTF("Wake Up\n"); stat = stat | 0x80u; } - Flight1.AccEvent_Notify(Flight1.globalSteps, stat); + Flight1.AccEvent_Notify(stat); } if (proxEnable) { diff --git a/examples/IKS01A3_S2LP_P2P_Demo/IKS01A3_S2LP_P2P_Demo.ino b/examples/IKS01A3_S2LP_P2P_Demo/IKS01A3_S2LP_P2P_Demo.ino index 119648b..0d35728 100644 --- a/examples/IKS01A3_S2LP_P2P_Demo/IKS01A3_S2LP_P2P_Demo.ino +++ b/examples/IKS01A3_S2LP_P2P_Demo/IKS01A3_S2LP_P2P_Demo.ino @@ -241,7 +241,7 @@ void recv_data(void) SerialPort.print(*((float *)(&read_buf[0])), 2); SerialPort.print(" | Temp[C]: "); SerialPort.print(*((float *)(&read_buf[sizeof(float)])), 2); - SerialPort.print(" | Pres[hPa]: "); + SerialPort.print(" | Press[hPa]: "); SerialPort.print(*((float *)(&read_buf[2*sizeof(float)])), 2); SerialPort.print(" | Temp2[C]: "); SerialPort.print(*((float *)(&read_buf[3*sizeof(float)])), 2); diff --git a/examples/X_NUCLEO_IKS02A1_Audio_Button/WaveEncoder.cpp b/examples/X_NUCLEO_IKS02A1_Audio_Button/WaveEncoder.cpp index e1adc0c..88ac015 100644 --- a/examples/X_NUCLEO_IKS02A1_Audio_Button/WaveEncoder.cpp +++ b/examples/X_NUCLEO_IKS02A1_Audio_Button/WaveEncoder.cpp @@ -26,7 +26,7 @@ void WaveClass::header_init(uint8_t *pHeader, uint32_t SampleRate, uint32_t Bits /* Write the file length --------------------------------------------------- /* The sampling time: this value will be be written back at the end of the - * recording opearation. + * recording operation. * Example: 44 Bytes = 0x000A17FC, byte[7]=0x00, byte[4]=0xFC */ pHeader[4] = 0x2C; @@ -108,7 +108,7 @@ void WaveClass::header_update(uint8_t *pHeader, uint32_t *byteswritten) { /* Write the file length ----------------------------------------------------*/ /* The sampling time: this value will be be written back at the end of the - recording opearation. Example: 661500 Btyes = 0x000A17FC, byte[7]=0x00, byte[4]=0xFC */ + recording operation. Example: 661500 Bytes = 0x000A17FC, byte[7]=0x00, byte[4]=0xFC */ pHeader[4] = (uint8_t) * byteswritten; //<-- man mano che scrivo salva qua dentro i byte scritti pHeader[5] = (uint8_t)(*byteswritten >> 8); pHeader[6] = (uint8_t)(*byteswritten >> 16); diff --git a/examples/X_NUCLEO_IKS02A1_Audio_Button/X_NUCLEO_IKS02A1_Audio_Button.ino b/examples/X_NUCLEO_IKS02A1_Audio_Button/X_NUCLEO_IKS02A1_Audio_Button.ino index eb16efb..c2f7f05 100644 --- a/examples/X_NUCLEO_IKS02A1_Audio_Button/X_NUCLEO_IKS02A1_Audio_Button.ino +++ b/examples/X_NUCLEO_IKS02A1_Audio_Button/X_NUCLEO_IKS02A1_Audio_Button.ino @@ -51,7 +51,7 @@ void setup() { /* SD Card Initialization */ if (!SD.begin(12000000, 10)) { - Serial.println("Failed to inizialize SD!\n"); + Serial.println("Failed to initialize SD!\n"); while(1); } @@ -100,7 +100,7 @@ void loop() { } if (!(myFile = SD.open(file_name, FILE_WRITE))) { - Serial.println("Failed to Inizialize File\n"); + Serial.println("Failed to initialize File\n"); while(1); } diff --git a/extras/.codespellignore b/extras/.codespellignore new file mode 100644 index 0000000..dc092c8 --- /dev/null +++ b/extras/.codespellignore @@ -0,0 +1 @@ +aci diff --git a/library.properties b/library.properties index bf77ec7..23c4a07 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=STM32duino FP_Examples -version=1.6.3 +version=1.8.0 author=STMicroelectronics maintainer=stm32duino sentence=Provides several Function Packs that combine the usage of several X-NUCLEO boards @@ -7,4 +7,4 @@ paragraph=This library provides several Function Packs that combine the usage of category=Other url=https://github.com/stm32duino/FP_Examples architectures=stm32 -depends=STM32duino LSM6DSL, STM32duino LSM303AGR, STM32duino LPS22HB, STM32duino VL6180X, STM32duino VL53L0X, STM32duino VL53L1X, STM32duino Proximity Gesture, STM32duino X-NUCLEO-6180XA1, STM32duino X-NUCLEO-53L0A1, STM32duino X-NUCLEO-53L1A1, STM32duino SPBTLE-RF, STM32duino X-NUCLEO-IHM02A1, STM32duino X-NUCLEO-LED61A1, STM32duino LPS25HB, STM32duino LSM6DS3, STM32duino LSM6DSO, STM32duino LIS2DW12, STM32duino LIS2MDL, STM32duino HTS221, STM32duino LPS22HH, STM32duino STTS751, STM32duino S2-LP, STM32duino M95640-R, STM32duino X-NUCLEO-IKS02A1 Audio, STM32duinoBLE +depends=STM32duino LSM6DSL, STM32duino LSM303AGR, STM32duino LPS22HB, STM32duino VL6180X, STM32duino VL53L0X, STM32duino VL53L1X, STM32duino Proximity Gesture, STM32duino X-NUCLEO-6180XA1, STM32duino X-NUCLEO-53L0A1, STM32duino X-NUCLEO-53L1A1, STM32duino SPBTLE-RF, STM32duino X-NUCLEO-IHM02A1, STM32duino X-NUCLEO-LED61A1, STM32duino LPS25HB, STM32duino LSM6DS3, STM32duino LSM6DSO, STM32duino LIS2DW12, STM32duino LIS2MDL, STM32duino HTS221, STM32duino LPS22HH, STM32duino STTS751, STM32duino S2-LP, STM32duino M95640-R, STM32duino X-NUCLEO-IKS02A1 Audio, STM32duinoBLE, STM32duino MotionFX