From 90c0beac08e6af1f197088c1d7d71d54705b60ef Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sun, 5 Jan 2025 02:26:31 +0100 Subject: [PATCH 01/21] Move temperature reading into the FIFO whenever possible (no love for MPU) --- platformio.ini | 42 ++-- src/configuration/SensorConfig.h | 3 + src/defines.h | 209 ++---------------- src/sensors/softfusion/drivers/bmi270.h | 8 +- src/sensors/softfusion/drivers/icm42688.h | 60 +++-- src/sensors/softfusion/drivers/icm45605.h | 2 - src/sensors/softfusion/drivers/icm45686.h | 2 - src/sensors/softfusion/drivers/icm45base.h | 18 +- .../softfusion/drivers/lsm6ds-common.h | 19 +- src/sensors/softfusion/drivers/lsm6ds3trc.h | 28 ++- src/sensors/softfusion/drivers/lsm6dso.h | 38 ++-- src/sensors/softfusion/drivers/lsm6dsr.h | 38 ++-- src/sensors/softfusion/drivers/lsm6dsv.h | 38 ++-- src/sensors/softfusion/softfusionsensor.h | 122 +++++++--- 14 files changed, 279 insertions(+), 348 deletions(-) diff --git a/platformio.ini b/platformio.ini index fb654067..f6c504f9 100644 --- a/platformio.ini +++ b/platformio.ini @@ -11,9 +11,6 @@ ; https://docs.slimevr.dev/firmware/configuring-project.html#1-configuring-platformioini ; ================================================ -[platformio] -build_cache_dir = cache - [env] lib_deps= https://github.com/SlimeVR/CmdParser.git @@ -60,12 +57,13 @@ build_unflags = -Os -std=gnu++11 -std=gnu++17 ; Settings for different boards -[env:esp12e] -platform = espressif8266 @ 4.2.1 -board = esp12e +;[env:esp12e] +;platform = espressif8266 @ 4.2.1 +;board = esp12e ; Comment out this line below if you have any trouble uploading the firmware ; and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed -upload_speed = 921600 +;upload_speed = 921600 +;board_build.f_cpu = 160000000L ; Uncomment below if you want to build for ESP-01 ;[env:esp01_1m] @@ -100,22 +98,14 @@ upload_speed = 921600 ; -DARDUINO_USB_MODE=1 ; -DARDUINO_USB_CDC_ON_BOOT=1 -;[env:esp32c3] -;platform = espressif32 @ 6.7.0 -;platform_packages = -; framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 -; framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip -;build_flags = -; ${env.build_flags} -; -DESP32C3 -;board = lolin_c3_mini - -; If you want to use a ESP32C6, you can use this (experimental) -;[env:esp32c6] -;platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip -;board = esp32-c6-devkitc-1 -;build_flags = -; ${env.build_flags} -; -DESP32C6 -; -DARDUINO_USB_MODE=1 -; -DARDUINO_USB_CDC_ON_BOOT=1 \ No newline at end of file +[env:esp32c3] +platform = espressif32 @ 6.7.0 +platform_packages = + framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 + framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip +build_flags = + ${env.build_flags} + -DESP32C3 + -DARDUINO_USB_MODE=1 + -DARDUINO_USB_CDC_ON_BOOT=1 +board = lolin_c3_mini diff --git a/src/configuration/SensorConfig.h b/src/configuration/SensorConfig.h index 24b9477c..0e5977be 100644 --- a/src/configuration/SensorConfig.h +++ b/src/configuration/SensorConfig.h @@ -73,6 +73,9 @@ struct SoftFusionSensorConfig { float G_Sens[3]; uint8_t MotionlessData[60]; + + // temperature sampling rate (placed at the end to not break existing configs) + float T_Ts; }; struct MPU6050SensorConfig { diff --git a/src/defines.h b/src/defines.h index 992c2539..b73b16d1 100644 --- a/src/defines.h +++ b/src/defines.h @@ -20,216 +20,45 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -// ================================================ -// See docs for configuration options and examples: -// https://docs.slimevr.dev/firmware/configuring-project.html#2-configuring-definesh -// ================================================ -// Set parameters of IMU and board used -#define IMU IMU_BNO085 -#define SECOND_IMU IMU -#define BOARD BOARD_SLIMEVR -#define IMU_ROTATION DEG_270 -#define SECOND_IMU_ROTATION DEG_270 +#define IMU IMU_LSM6DSV +#define BOARD BOARD_CUSTOM +#define IMU_ROTATION DEG_90 #define PRIMARY_IMU_OPTIONAL false -#define SECONDARY_IMU_OPTIONAL true -// Set I2C address here or directly in IMU_DESC_ENTRY for each IMU used -// If not set, default address is used based on the IMU and Sensor ID -// #define PRIMARY_IMU_ADDRESS_ONE 0x4a -// #define SECONDARY_IMU_ADDRESS_TWO 0x4b +#define MAX_IMU_COUNT 1 -#define MAX_IMU_COUNT 2 - -// Axis mapping example -/* -#include "sensors/axisremap.h" -#define BMI160_QMC_REMAP AXIS_REMAP_BUILD(AXIS_REMAP_USE_Y, AXIS_REMAP_USE_XN, -AXIS_REMAP_USE_Z, \ AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z) - -IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, -PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ -*/ +#define ON_OFF_BUTTON 1 #ifndef IMU_DESC_LIST -#define IMU_DESC_LIST \ - IMU_DESC_ENTRY( \ - IMU, \ - PRIMARY_IMU_ADDRESS_ONE, \ - IMU_ROTATION, \ - PIN_IMU_SCL, \ - PIN_IMU_SDA, \ - PRIMARY_IMU_OPTIONAL, \ - PIN_IMU_INT \ - ) \ - IMU_DESC_ENTRY( \ - SECOND_IMU, \ - SECONDARY_IMU_ADDRESS_TWO, \ - SECOND_IMU_ROTATION, \ - PIN_IMU_SCL, \ - PIN_IMU_SDA, \ - SECONDARY_IMU_OPTIONAL, \ - PIN_IMU_INT_2 \ +#define IMU_DESC_LIST \ + IMU_DESC_ENTRY( \ + IMU, \ + PRIMARY_IMU_ADDRESS_ONE, \ + IMU_ROTATION, \ + PIN_IMU_SCL, \ + PIN_IMU_SDA, \ + PRIMARY_IMU_OPTIONAL, \ + PIN_IMU_INT \ ) #endif -// Battery monitoring options (comment to disable): -// BAT_EXTERNAL for ADC pin, -// BAT_INTERNAL for internal - can detect only low battery, -// BAT_MCP3021 for external ADC connected over I2C #define BATTERY_MONITOR BAT_EXTERNAL -// BAT_EXTERNAL definition override -// D1 Mini boards with ESP8266 have internal resistors. For these boards you only have -// to adjust BATTERY_SHIELD_RESISTANCE. For other boards you can now adjust the other -// resistor values. The diagram looks like this: -// (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)--- [BATTERY_SHIELD_R2] -// ---(ESP32_INPUT)--- [BATTERY_SHIELD_R1] --- (GND) -// #define BATTERY_SHIELD_RESISTANCE 180 //130k BatteryShield, 180k SlimeVR or fill in -// external resistor value in kOhm #define BATTERY_SHIELD_R1 100 // Board voltage -// divider resistor Ain to GND in kOhm #define BATTERY_SHIELD_R2 220 // Board voltage -// divider resistor Ain to INPUT_BOARD in kOhm - -// LED configuration: -// Configuration Priority 1 = Highest: -// 1. LED_PIN -// 2. LED_BUILTIN -// -// LED_PIN -// - Number or Symbol (D1,..) of the Output -// - To turn off the LED, set LED_PIN to LED_OFF -// LED_INVERTED -// - false for output 3.3V on high -// - true for pull down to GND on high - -// Board-specific configurations -#if BOARD == BOARD_SLIMEVR -#define PIN_IMU_SDA 14 -#define PIN_IMU_SCL 12 -#define PIN_IMU_INT 16 -#define PIN_IMU_INT_2 13 -#define PIN_BATTERY_LEVEL 17 -#define LED_PIN 2 -#define LED_INVERTED true -#ifndef BATTERY_SHIELD_RESISTANCE -#define BATTERY_SHIELD_RESISTANCE 0 -#endif -#ifndef BATTERY_SHIELD_R1 -#define BATTERY_SHIELD_R1 10 -#endif -#ifndef BATTERY_SHIELD_R2 -#define BATTERY_SHIELD_R2 40.2 -#endif -#elif BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV -#define PIN_IMU_SDA 4 -#define PIN_IMU_SCL 5 -#define PIN_IMU_INT 10 -#define PIN_IMU_INT_2 13 -#define PIN_BATTERY_LEVEL 17 -#define LED_PIN 2 -#define LED_INVERTED true -#ifndef BATTERY_SHIELD_RESISTANCE -#define BATTERY_SHIELD_RESISTANCE 0 -#endif -#ifndef BATTERY_SHIELD_R1 -#define BATTERY_SHIELD_R1 10 -#endif -#ifndef BATTERY_SHIELD_R2 -#define BATTERY_SHIELD_R2 40.2 -#endif -#elif BOARD == BOARD_NODEMCU || BOARD == BOARD_WEMOSD1MINI -#define PIN_IMU_SDA D2 -#define PIN_IMU_SCL D1 -#define PIN_IMU_INT D5 -#define PIN_IMU_INT_2 D6 -#define PIN_BATTERY_LEVEL A0 -// #define LED_PIN 2 -// #define LED_INVERTED true -#ifndef BATTERY_SHIELD_RESISTANCE -#define BATTERY_SHIELD_RESISTANCE 180 -#endif -#ifndef BATTERY_SHIELD_R1 -#define BATTERY_SHIELD_R1 100 -#endif -#ifndef BATTERY_SHIELD_R2 -#define BATTERY_SHIELD_R2 220 -#endif -#elif BOARD == BOARD_ESP01 -#define PIN_IMU_SDA 2 -#define PIN_IMU_SCL 0 +#define PIN_IMU_SDA 5 +#define PIN_IMU_SCL 6 #define PIN_IMU_INT 255 #define PIN_IMU_INT_2 255 -#define PIN_BATTERY_LEVEL 255 -#define LED_PIN LED_OFF -#define LED_INVERTED false -#elif BOARD == BOARD_TTGO_TBASE -#define PIN_IMU_SDA 5 -#define PIN_IMU_SCL 4 -#define PIN_IMU_INT 14 -#define PIN_IMU_INT_2 13 -#define PIN_BATTERY_LEVEL A0 -// #define LED_PIN 2 -// #define LED_INVERTED false -#elif BOARD == BOARD_CUSTOM -// Define pins by the examples above -#elif BOARD == BOARD_WROOM32 -#define PIN_IMU_SDA 21 -#define PIN_IMU_SCL 22 -#define PIN_IMU_INT 23 -#define PIN_IMU_INT_2 25 -#define PIN_BATTERY_LEVEL 36 -// #define LED_PIN 2 -// #define LED_INVERTED false -#elif BOARD == BOARD_LOLIN_C3_MINI -#define PIN_IMU_SDA 5 -#define PIN_IMU_SCL 4 -#define PIN_IMU_INT 6 -#define PIN_IMU_INT_2 8 #define PIN_BATTERY_LEVEL 3 -#define LED_PIN 7 -// #define LED_INVERTED false -#elif BOARD == BOARD_BEETLE32C3 -#define PIN_IMU_SDA 8 -#define PIN_IMU_SCL 9 -#define PIN_IMU_INT 6 -#define PIN_IMU_INT_2 7 -#define PIN_BATTERY_LEVEL 3 -#define LED_PIN 10 -#define LED_INVERTED false -#elif BOARD == BOARD_ES32C3DEVKITM1 || BOARD == BOARD_ES32C6DEVKITC1 -#define PIN_IMU_SDA 5 -#define PIN_IMU_SCL 4 -#define PIN_IMU_INT 6 -#define PIN_IMU_INT_2 7 -#define PIN_BATTERY_LEVEL 3 -#define LED_PIN \ - LED_OFF // RGB LED Protocol would need to be implementetet did not brother for the - // test, because the board ideal for tracker ifself -// #define LED_INVERTED false -#elif BOARD == BOARD_WEMOSWROOM02 -#define PIN_IMU_SDA 2 -#define PIN_IMU_SCL 14 -#define PIN_IMU_INT 0 -#define PIN_IMU_INT_2 4 -#define PIN_BATTERY_LEVEL A0 -#define LED_PIN 16 +#define LED_PIN 0 #define LED_INVERTED true -#elif BOARD == BOARD_XIAO_ESP32C3 -#define PIN_IMU_SDA 6 // D4 -#define PIN_IMU_SCL 7 // D5 -#define PIN_IMU_INT 5 // D3 -#define PIN_IMU_INT_2 10 // D10 -#define LED_PIN 4 // D2 -#define LED_INVERTED false -#define PIN_BATTERY_LEVEL 2 // D0 / A0 #ifndef BATTERY_SHIELD_RESISTANCE #define BATTERY_SHIELD_RESISTANCE 0 #endif #ifndef BATTERY_SHIELD_R1 -#define BATTERY_SHIELD_R1 100 +#define BATTERY_SHIELD_R1 150 #endif #ifndef BATTERY_SHIELD_R2 -#define BATTERY_SHIELD_R2 100 -#endif +#define BATTERY_SHIELD_R2 150 #endif diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index d122ce97..a19ef715 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -391,8 +391,12 @@ struct BMI270 { return to_ret; } - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTempSample + ) { const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); const auto bytes_to_read = std::min( diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index 16088994..48c4a404 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -42,12 +42,18 @@ struct ICM42688 { static constexpr float GyrTs = 1.0 / 500.0; static constexpr float AccTs = 1.0 / 100.0; + static constexpr float TempTs = 1.0 / 500.0; static constexpr float MagTs = 1.0 / 100; static constexpr float GyroSensitivity = 32.8f; static constexpr float AccelSensitivity = 4096.0f; + static constexpr bool Uses32BitSensorData = true; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 2.07f; + I2CImpl i2c; SlimeVR::Logging::Logger& logger; ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger) @@ -59,7 +65,6 @@ struct ICM42688 { static constexpr uint8_t reg = 0x75; static constexpr uint8_t value = 0x47; }; - static constexpr uint8_t TempData = 0x1d; struct DeviceConfig { static constexpr uint8_t reg = 0x11; @@ -78,8 +83,8 @@ struct ICM42688 { struct FifoConfig1 { static constexpr uint8_t reg = 0x5f; static constexpr uint8_t value - = 0b1 | (0b1 << 1) - | (0b0 << 2); // fifo accel en=1, gyro=1, temp=0 todo: fsync, hires + = 0b1 | (0b1 << 1) | (0b1 << 2) + | (0b0 << 4); // fifo accel en=1, gyro=1, temp=1, hires=1 }; struct GyroConfig { static constexpr uint8_t reg = 0x4f; @@ -111,15 +116,18 @@ struct ICM42688 { struct { int16_t accel[3]; int16_t gyro[3]; - uint8_t temp; - uint8_t timestamp[2]; // cannot do uint16_t because it's unaligned + uint16_t temp; + uint16_t timestamp; + uint8_t xlsb; + uint8_t ylsb; + uint8_t zlsb; } part; - uint8_t raw[15]; + uint8_t raw[19]; }; }; #pragma pack(pop) - static constexpr size_t FullFifoEntrySize = 16; + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; bool initialize() { // perform initialization step @@ -137,14 +145,12 @@ struct ICM42688 { return true; } - float getDirectTemp() const { - const auto value = static_cast(i2c.readReg16(Regs::TempData)); - float result = ((float)value / 132.48f) + 25.0f; - return result; - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTemperatureSample + ) { const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); std::array read_buffer; // max 8 readings @@ -161,13 +167,31 @@ struct ICM42688 { &read_buffer[i + 0x1], sizeof(FifoEntryAligned) ); // skip fifo header - processGyroSample(entry.part.gyro, GyrTs); + + const int32_t gyroData[3]{ + static_cast(entry.part.gyro[0]) << 4 | (entry.part.xlsb & 0xf), + static_cast(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf), + static_cast(entry.part.gyro[2]) << 4 | (entry.part.zlsb & 0xf), + }; + processGyroSample(gyroData, GyrTs); if (entry.part.accel[0] != -32768) { - processAccelSample(entry.part.accel, AccTs); + const int32_t accelData[3]{ + static_cast(entry.part.accel[0]) << 4 + | (static_cast(entry.part.xlsb) & 0xf0 >> 4), + static_cast(entry.part.accel[1]) << 4 + | (static_cast(entry.part.ylsb) & 0xf0 >> 4), + static_cast(entry.part.accel[2]) << 4 + | (static_cast(entry.part.zlsb) & 0xf0 >> 4), + }; + processAccelSample(accelData, AccTs); + } + + if (entry.part.temp != 0x8000) { + processTemperatureSample(static_cast(entry.part.temp), TempTs); } } } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/icm45605.h b/src/sensors/softfusion/drivers/icm45605.h index de045ed7..0c01d1d6 100644 --- a/src/sensors/softfusion/drivers/icm45605.h +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -49,8 +49,6 @@ struct ICM45605 : public ICM45Base { }; }; - float getDirectTemp() const { return ICM45Base::getDirectTemp(); } - bool initialize() { ICM45Base::softResetIMU(); return ICM45Base::initializeBase(); diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h index 838ac462..adeed49a 100644 --- a/src/sensors/softfusion/drivers/icm45686.h +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -59,8 +59,6 @@ struct ICM45686 : public ICM45Base { }; }; - float getDirectTemp() const { return ICM45Base::getDirectTemp(); } - using ICM45Base::i2c; bool initialize() { diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 47dc5b1c..b980fbdb 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -138,14 +138,12 @@ struct ICM45Base { return true; } - float getDirectTemp() const { - const auto value = static_cast(i2c.readReg16(BaseRegs::TempData)); - float result = ((float)value / 132.48f) + 25.0f; - return result; - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTemperatureSample + ) { const auto fifo_packets = i2c.readReg16(BaseRegs::FifoCount); const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); @@ -184,6 +182,10 @@ struct ICM45Base { }; processAccelSample(accelData, AccTs); } + + if (entry.part.temp != 0x8000) { + processTemperatureSample(static_cast(entry.part.temp), TempTs); + } } } }; diff --git a/src/sensors/softfusion/drivers/lsm6ds-common.h b/src/sensors/softfusion/drivers/lsm6ds-common.h index 382103b9..a8d0c4ee 100644 --- a/src/sensors/softfusion/drivers/lsm6ds-common.h +++ b/src/sensors/softfusion/drivers/lsm6ds-common.h @@ -38,14 +38,6 @@ struct LSM6DSOutputHandler { I2CImpl i2c; SlimeVR::Logging::Logger& logger; - template - float getDirectTemp() const { - const auto value = static_cast(i2c.readReg16(Regs::OutTemp)); - float result = ((float)value / 256.0f) + 25.0f; - - return result; - } - #pragma pack(push, 1) struct FifoEntryAligned { union { @@ -57,12 +49,14 @@ struct LSM6DSOutputHandler { static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; - template + template void bulkRead( AccelCall& processAccelSample, GyroCall& processGyroSample, + TempCall& processTempSample, float GyrTs, - float AccTs + float AccTs, + float TempTs ) { constexpr auto FIFO_SAMPLES_MASK = 0x3ff; constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800; @@ -99,9 +93,12 @@ struct LSM6DSOutputHandler { case 0x02: // Accel NC processAccelSample(entry.xyz, AccTs); break; + case 0x03: // Temperature + processTempSample(entry.x, TempTs); + break; } } } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index 75f036e4..4654fa5c 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -44,10 +44,14 @@ struct LSM6DS3TRC { static constexpr float GyrTs = 1.0 / Freq; static constexpr float AccTs = 1.0 / Freq; static constexpr float MagTs = 1.0 / Freq; + static constexpr float TempTs = 1.0 / Freq; static constexpr float GyroSensitivity = 28.571428571f; static constexpr float AccelSensitivity = 4098.360655738f; + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + I2CImpl i2c; SlimeVR::Logging::Logger logger; LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger& logger) @@ -59,7 +63,6 @@ struct LSM6DS3TRC { static constexpr uint8_t reg = 0x0f; static constexpr uint8_t value = 0x6a; }; - static constexpr uint8_t OutTemp = 0x20; struct Ctrl1XL { static constexpr uint8_t reg = 0x10; static constexpr uint8_t value = (0b11 << 2) | (0b0110 << 4); // 8g, 416Hz @@ -75,6 +78,10 @@ struct LSM6DS3TRC { static constexpr uint8_t value = (1 << 6) | (1 << 2); // BDU = 1, IF_INC = // 1 }; + struct FifoCtrl2 { + static constexpr uint8_t reg = 0x07; + static constexpr uint8_t value = 0b1000; // temperature in fifo + }; struct FifoCtrl3 { static constexpr uint8_t reg = 0x08; static constexpr uint8_t value @@ -97,20 +104,18 @@ struct LSM6DS3TRC { i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); i2c.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value); i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::FifoCtrl2::reg, Regs::FifoCtrl2::value); i2c.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value); i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); return true; } - float getDirectTemp() const { - const auto value = static_cast(i2c.readReg16(Regs::OutTemp)); - float result = ((float)value / 256.0f) + 25.0f; - - return result; - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTemperatureSample + ) { const auto read_result = i2c.readReg16(Regs::FifoStatus); if (read_result & 0x4000) { // overrun! // disable and re-enable fifo to clear it @@ -145,8 +150,9 @@ struct LSM6DS3TRC { reinterpret_cast(&read_buffer[i + 3]), AccTs ); + processTemperatureSample(read_buffer[i + 9], TempTs); } } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index 7c26c85b..db5cce71 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -44,14 +44,19 @@ struct LSM6DSO : LSM6DSOutputHandler { static constexpr float GyrFreq = 416; static constexpr float AccFreq = 104; static constexpr float MagFreq = 120; + static constexpr float TempFreq = 52; static constexpr float GyrTs = 1.0 / GyrFreq; static constexpr float AccTs = 1.0 / AccFreq; static constexpr float MagTs = 1.0 / MagFreq; + static constexpr float TempTs = 1.0 / TempFreq; static constexpr float GyroSensitivity = 1000 / 35.0f; static constexpr float AccelSensitivity = 1000 / 0.244f; + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + using LSM6DSOutputHandler::i2c; struct Regs { @@ -59,7 +64,6 @@ struct LSM6DSO : LSM6DSOutputHandler { static constexpr uint8_t reg = 0x0f; static constexpr uint8_t value = 0x6c; }; - static constexpr uint8_t OutTemp = 0x20; struct Ctrl1XL { static constexpr uint8_t reg = 0x10; static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS @@ -81,7 +85,8 @@ struct LSM6DSO : LSM6DSOutputHandler { }; struct FifoCtrl4Mode { static constexpr uint8_t reg = 0x0a; - static constexpr uint8_t value = (0b110); // continuous mode + static constexpr uint8_t value = (0b110110); // continuous mode, + // temperature at 52Hz }; static constexpr uint8_t FifoStatus = 0x3a; @@ -103,19 +108,22 @@ struct LSM6DSO : LSM6DSOutputHandler { return true; } - float getDirectTemp() const { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - LSM6DSOutputHandler::template bulkRead( - processAccelSample, - processGyroSample, - GyrTs, - AccTs - ); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTempSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTempSample, + GyrTs, + AccTs, + TempTs + ); } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h index 24867980..cd796c59 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -44,14 +44,19 @@ struct LSM6DSR : LSM6DSOutputHandler { static constexpr float GyrFreq = 416; static constexpr float AccFreq = 104; static constexpr float MagFreq = 120; + static constexpr float TempFreq = 52; static constexpr float GyrTs = 1.0 / GyrFreq; static constexpr float AccTs = 1.0 / AccFreq; static constexpr float MagTs = 1.0 / MagFreq; + static constexpr float TempTs = 1.0 / TempFreq; static constexpr float GyroSensitivity = 1000 / 35.0f; static constexpr float AccelSensitivity = 1000 / 0.244f; + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + using LSM6DSOutputHandler::i2c; struct Regs { @@ -59,7 +64,6 @@ struct LSM6DSR : LSM6DSOutputHandler { static constexpr uint8_t reg = 0x0f; static constexpr uint8_t value = 0x6b; }; - static constexpr uint8_t OutTemp = 0x20; struct Ctrl1XL { static constexpr uint8_t reg = 0x10; static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS @@ -81,7 +85,8 @@ struct LSM6DSR : LSM6DSOutputHandler { }; struct FifoCtrl4Mode { static constexpr uint8_t reg = 0x0a; - static constexpr uint8_t value = (0b110); // continuous mode + static constexpr uint8_t value = (0b110110); // continuous mode, + // temperature at 52Hz }; static constexpr uint8_t FifoStatus = 0x3a; @@ -103,19 +108,22 @@ struct LSM6DSR : LSM6DSOutputHandler { return true; } - float getDirectTemp() const { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - LSM6DSOutputHandler::template bulkRead( - processAccelSample, - processGyroSample, - GyrTs, - AccTs - ); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTempSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTempSample, + GyrTs, + AccTs, + TempTs + ); } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index f0aa11ee..6f1a16c3 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -44,14 +44,19 @@ struct LSM6DSV : LSM6DSOutputHandler { static constexpr float GyrFreq = 480; static constexpr float AccFreq = 120; static constexpr float MagFreq = 120; + static constexpr float TempFreq = 60; static constexpr float GyrTs = 1.0 / GyrFreq; static constexpr float AccTs = 1.0 / AccFreq; static constexpr float MagTs = 1.0 / MagFreq; + static constexpr float TempTs = 1.0 / TempFreq; static constexpr float GyroSensitivity = 1000 / 35.0f; static constexpr float AccelSensitivity = 1000 / 0.244f; + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + using LSM6DSOutputHandler::i2c; struct Regs { @@ -59,7 +64,6 @@ struct LSM6DSV : LSM6DSOutputHandler { static constexpr uint8_t reg = 0x0f; static constexpr uint8_t value = 0x70; }; - static constexpr uint8_t OutTemp = 0x20; struct HAODRCFG { static constexpr uint8_t reg = 0x62; static constexpr uint8_t value = (0b00); // 1st ODR table @@ -93,7 +97,8 @@ struct LSM6DSV : LSM6DSOutputHandler { }; struct FifoCtrl4Mode { static constexpr uint8_t reg = 0x0a; - static constexpr uint8_t value = (0b110); // continuous mode + static constexpr uint8_t value = (0b110110); // continuous mode, + // temperature at 60Hz }; static constexpr uint8_t FifoStatus = 0x1b; @@ -118,19 +123,22 @@ struct LSM6DSV : LSM6DSOutputHandler { return true; } - float getDirectTemp() const { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - LSM6DSOutputHandler::template bulkRead( - processAccelSample, - processGyroSample, - GyrTs, - AccTs - ); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTempSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTempSample, + GyrTs, + AccTs, + TempTs + ); } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 857b24fe..ae4974bd 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -23,9 +23,13 @@ #pragma once +#include +#include + #include "../SensorFusionRestDetect.h" #include "../sensor.h" #include "GlobalVars.h" +#include "motionprocessing/types.h" namespace SlimeVR::Sensors { @@ -36,6 +40,8 @@ class SoftFusionSensor : public Sensor { static constexpr bool Uses32BitSensorData = requires(imu& i) { i.Uses32BitSensorData; }; + static constexpr bool DirectTempReadOnly = requires(imu& i) { i.getDirectTemp(); }; + using RawSensorT = typename std::conditional::type; using RawVectorT = std::array; @@ -63,6 +69,20 @@ class SoftFusionSensor : public Sensor { } } + static constexpr float DirectTempReadFreq = 15; + static constexpr uint32_t DirectTempReadMicros + = static_cast(1e6 / DirectTempReadFreq); + float lastReadTemperature = 0; + uint32_t lastTempPollTime = micros(); + + constexpr sensor_real_t getImuTempTs() const { + if constexpr (DirectTempReadOnly) { + return static_cast(1.0) / DirectTempReadFreq; + } else { + return imu::TempTs; + } + } + bool detected() const { const auto value = m_sensor.i2c.readReg(imu::Regs::WhoAmI::reg); if (imu::Regs::WhoAmI::value != value) { @@ -84,9 +104,8 @@ class SoftFusionSensor : public Sensor { constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6; uint32_t elapsed = now - m_lastTemperaturePacketSent; if (elapsed >= sendInterval) { - const float temperature = m_sensor.getDirectTemp(); m_lastTemperaturePacketSent = now - (elapsed - sendInterval); - networkConnection.sendTemperature(sensorId, temperature); + networkConnection.sendTemperature(sensorId, lastReadTemperature); } } @@ -140,6 +159,17 @@ class SoftFusionSensor : public Sensor { m_fusion.updateGyro(scaledData, m_calibration.G_Ts); } + void + processTempSample(const int16_t rawTemperature, const sensor_real_t timeDelta) { + if constexpr (!DirectTempReadOnly) { + const float scaledTemperature = imu::TemperatureBias + + static_cast(rawTemperature) + * (1.0 / imu::TemperatureSensitivity); + + lastReadTemperature = scaledTemperature; + } + } + void eatSamplesForSeconds(const uint32_t seconds) { const auto targetDelay = millis() + 1000 * seconds; auto lastSecondsRemaining = seconds; @@ -154,15 +184,18 @@ class SoftFusionSensor : public Sensor { } m_sensor.bulkRead( [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, - [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {} + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [](const int16_t xyz, const sensor_real_t timeDelta) {} ); } } - std::pair eatSamplesReturnLast(const uint32_t milliseconds + std::tuple eatSamplesReturnLast( + const uint32_t milliseconds ) { RawVectorT accel = {0}; RawVectorT gyro = {0}; + int16_t temp = 0; const auto targetDelay = millis() + milliseconds; while (millis() < targetDelay) { m_sensor.bulkRead( @@ -175,11 +208,14 @@ class SoftFusionSensor : public Sensor { gyro[0] = xyz[0]; gyro[1] = xyz[1]; gyro[2] = xyz[2]; + }, + [&](const int16_t rawTemp, const sensor_real_t timeDelta) { + temp = rawTemp; } ); yield(); } - return std::make_pair(accel, gyro); + return std::make_tuple(accel, gyro, temp); } public: @@ -204,6 +240,15 @@ class SoftFusionSensor : public Sensor { // read fifo updating fusion uint32_t now = micros(); + + if constexpr (DirectTempReadOnly) { + uint32_t tempElapsed = now - lastTempPollTime; + if (tempElapsed >= DirectTempReadMicros) { + lastTempPollTime = now - (tempElapsed - DirectTempReadMicros); + lastReadTemperature = m_sensor.getDirectTemp(); + } + } + constexpr uint32_t targetPollIntervalMicros = 6000; uint32_t elapsed = now - m_lastPollTime; if (elapsed >= targetPollIntervalMicros) { @@ -214,6 +259,9 @@ class SoftFusionSensor : public Sensor { }, [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { processGyroSample(xyz, timeDelta); + }, + [&](const int16_t rawTemp, const sensor_real_t timeDelta) { + processTempSample(rawTemp, timeDelta); } ); optimistic_yield(100); @@ -290,7 +338,7 @@ class SoftFusionSensor : public Sensor { [[maybe_unused]] auto lastRawSample = eatSamplesReturnLast(1000); if constexpr (UpsideDownCalibrationInit) { auto gravity = static_cast( - AScale * static_cast(lastRawSample.first[2]) + AScale * static_cast(std::get<0>(lastRawSample)[2]) ); m_Logger.info( "Gravity read: %.1f (need < -7.5 to start calibration)", @@ -301,7 +349,7 @@ class SoftFusionSensor : public Sensor { m_Logger.info("Flip front in 5 seconds to start calibration"); lastRawSample = eatSamplesReturnLast(5000); gravity = static_cast( - AScale * static_cast(lastRawSample.first[2]) + AScale * static_cast(std::get<0>(lastRawSample)[2]) ); if (gravity > 7.5f) { m_Logger.debug("Starting calibration..."); @@ -377,7 +425,7 @@ class SoftFusionSensor : public Sensor { eatSamplesForSeconds(GyroCalibDelaySeconds); ledManager.off(); - m_calibration.temperature = m_sensor.getDirectTemp(); + m_calibration.temperature = lastReadTemperature; m_Logger.trace("Calibration temperature: %f", m_calibration.temperature); ledManager.pattern(100, 100, 3); @@ -400,7 +448,8 @@ class SoftFusionSensor : public Sensor { sumXYZ[1] += xyz[1]; sumXYZ[2] += xyz[2]; ++sampleCount; - } + }, + [](const int16_t rawTemp, const sensor_real_t timeDelta) {} ); } @@ -514,7 +563,8 @@ class SoftFusionSensor : public Sensor { samplesGathered = true; } }, - [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {} + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [](const int16_t rawTemp, const sensor_real_t timeDelta) {} ); } ledManager.off(); @@ -553,18 +603,21 @@ class SoftFusionSensor : public Sensor { uint32_t accelSamples = 0; uint32_t gyroSamples = 0; + uint32_t tempSamples = 0; const auto calibTarget = millis() + 1000 * SampleRateCalibSeconds; m_Logger.debug("Counting samples now..."); uint32_t currentTime; while ((currentTime = millis()) < calibTarget) { m_sensor.bulkRead( - [&accelSamples]( - const RawSensorT xyz[3], - const sensor_real_t timeDelta - ) { accelSamples++; }, - [&gyroSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + accelSamples++; + }, + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { gyroSamples++; + }, + [&](const int16_t rawTemp, const sensor_real_t timeDelta) { + tempSamples++; } ); yield(); @@ -580,11 +633,13 @@ class SoftFusionSensor : public Sensor { ); m_calibration.A_Ts = millisFromStart / (accelSamples * 1000.0); m_calibration.G_Ts = millisFromStart / (gyroSamples * 1000.0); + m_calibration.T_Ts = millisFromStart / (tempSamples * 1000.0); m_Logger.debug( - "Gyro frequency %fHz, accel frequency: %fHz", + "Gyro frequency %fHz, accel frequency: %fHz, temperature frequency: %fHz", 1.0 / m_calibration.G_Ts, - 1.0 / m_calibration.A_Ts + 1.0 / m_calibration.A_Ts, + 1.0 / m_calibration.T_Ts ); ledManager.off(); @@ -596,22 +651,23 @@ class SoftFusionSensor : public Sensor { SensorFusionRestDetect m_fusion; T m_sensor; - SlimeVR::Configuration::SoftFusionSensorConfig m_calibration - = {// let's create here transparent calibration that doesn't affect input data - .ImuType = {imu::Type}, - .MotionlessDataLen = {MotionlessCalibDataSize()}, - .A_B = {0.0, 0.0, 0.0}, - .A_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, - .M_B = {0.0, 0.0, 0.0}, - .M_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, - .G_off = {0.0, 0.0, 0.0}, - .temperature = 0.0, - .A_Ts = imu::AccTs, - .G_Ts = imu::GyrTs, - .M_Ts = imu::MagTs, - .G_Sens = {1.0, 1.0, 1.0}, - .MotionlessData = {} - }; + SlimeVR::Configuration::SoftFusionSensorConfig m_calibration = { + // let's create here transparent calibration that doesn't affect input data + .ImuType = {imu::Type}, + .MotionlessDataLen = {MotionlessCalibDataSize()}, + .A_B = {0.0, 0.0, 0.0}, + .A_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + .M_B = {0.0, 0.0, 0.0}, + .M_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + .G_off = {0.0, 0.0, 0.0}, + .temperature = 0.0, + .A_Ts = imu::AccTs, + .G_Ts = imu::GyrTs, + .M_Ts = imu::MagTs, + .G_Sens = {1.0, 1.0, 1.0}, + .MotionlessData = {}, + .T_Ts = getImuTempTs(), + }; SensorStatus m_status = SensorStatus::SENSOR_OFFLINE; uint32_t m_lastPollTime = micros(); From 5edfd285d08ec971f35cb6577e5248897b755853 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sun, 5 Jan 2025 03:19:12 +0100 Subject: [PATCH 02/21] Calculate gradient and feed into VQF --- lib/vqf/vqf.cpp | 24 +- lib/vqf/vqf.h | 1987 +++++++++-------- src/sensors/SensorFusion.cpp | 7 + src/sensors/SensorFusion.h | 4 + .../softfusion/TempGradientCalculator.cpp | 27 + .../softfusion/TempGradientCalculator.h | 46 + src/sensors/softfusion/drivers/bmi270.h | 2 + src/sensors/softfusion/drivers/icm42688.h | 2 + src/sensors/softfusion/drivers/icm45base.h | 2 + .../softfusion/drivers/lsm6ds-common.h | 2 +- src/sensors/softfusion/drivers/lsm6ds3trc.h | 2 + src/sensors/softfusion/drivers/lsm6dso.h | 2 + src/sensors/softfusion/drivers/lsm6dsr.h | 2 + src/sensors/softfusion/drivers/lsm6dsv.h | 2 + src/sensors/softfusion/drivers/mpu6050.h | 2 + src/sensors/softfusion/softfusionsensor.h | 21 +- 16 files changed, 1193 insertions(+), 941 deletions(-) create mode 100644 src/sensors/softfusion/TempGradientCalculator.cpp create mode 100644 src/sensors/softfusion/TempGradientCalculator.h diff --git a/lib/vqf/vqf.cpp b/lib/vqf/vqf.cpp index 3ecd9bde..fea0e7c1 100644 --- a/lib/vqf/vqf.cpp +++ b/lib/vqf/vqf.cpp @@ -917,16 +917,7 @@ void VQF::setup() coeffs.biasP0 = square(params.biasSigmaInit*100.0); // the system noise increases the variance from 0 to (0.1 °/s)^2 in biasForgettingTime seconds - coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime; - -#ifndef VQF_NO_MOTION_BIAS_ESTIMATION - vqf_real_t pMotion = square(params.biasSigmaMotion*100.0); - coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion; - coeffs.biasVerticalW = coeffs.biasMotionW / std::max(params.biasVerticalForgettingFactor, vqf_real_t(1e-10)); -#endif - - vqf_real_t pRest = square(params.biasSigmaRest*100.0); - coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest; + updateBiasForgettingTime(params.biasForgettingTime); filterCoeffs(params.restFilterTau, coeffs.gyrTs, coeffs.restGyrLpB, coeffs.restGyrLpA); filterCoeffs(params.restFilterTau, coeffs.accTs, coeffs.restAccLpB, coeffs.restAccLpA); @@ -941,3 +932,16 @@ void VQF::setup() resetState(); } + +void VQF::updateBiasForgettingTime(float biasForgettingTime) { + coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime; + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + vqf_real_t pMotion = square(params.biasSigmaMotion*100.0); + coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion; + coeffs.biasVerticalW = coeffs.biasMotionW / std::max(params.biasVerticalForgettingFactor, vqf_real_t(1e-10)); +#endif + + vqf_real_t pRest = square(params.biasSigmaRest*100.0); + coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest; +} \ No newline at end of file diff --git a/lib/vqf/vqf.h b/lib/vqf/vqf.h index cda5cc10..4f7f55f1 100644 --- a/lib/vqf/vqf.h +++ b/lib/vqf/vqf.h @@ -18,9 +18,10 @@ /** * @brief Typedef for the floating-point data type used for most operations. * - * By default, all floating-point calculations are performed using `double`. Set the `VQF_SINGLE_PRECISION` define to - * change this type to `float`. Note that the Butterworth filter implementation will always use double precision as - * using floats can cause numeric issues. + * By default, all floating-point calculations are performed using `double`. Set the + * `VQF_SINGLE_PRECISION` define to change this type to `float`. Note that the + * Butterworth filter implementation will always use double precision as using floats + * can cause numeric issues. */ #ifndef VQF_SINGLE_PRECISION typedef double vqf_real_t; @@ -31,988 +32,1122 @@ typedef float vqf_real_t; /** * @brief Struct containing all tuning parameters used by the VQF class. * - * The parameters influence the behavior of the algorithm and are independent of the sampling rate of the IMU data. The - * constructor sets all parameters to the default values. + * The parameters influence the behavior of the algorithm and are independent of the + * sampling rate of the IMU data. The constructor sets all parameters to the default + * values. * - * The parameters #motionBiasEstEnabled, #restBiasEstEnabled, and #magDistRejectionEnabled can be used to enable/disable - * the main features of the VQF algorithm. The time constants #tauAcc and #tauMag can be tuned to change the trust on - * the accelerometer and magnetometer measurements, respectively. The remaining parameters influence bias estimation - * and magnetometer rejection. + * The parameters #motionBiasEstEnabled, #restBiasEstEnabled, and + * #magDistRejectionEnabled can be used to enable/disable the main features of the VQF + * algorithm. The time constants #tauAcc and #tauMag can be tuned to change the trust on + * the accelerometer and magnetometer measurements, respectively. The remaining + * parameters influence bias estimation and magnetometer rejection. */ -struct VQFParams -{ - /** - * @brief Constructor that initializes the struct with the default parameters. - */ - VQFParams(); - - /** - * @brief Time constant \f$\tau_\mathrm{acc}\f$ for accelerometer low-pass filtering in seconds. - * - * Small values for \f$\tau_\mathrm{acc}\f$ imply trust on the accelerometer measurements and while large values of - * \f$\tau_\mathrm{acc}\f$ imply trust on the gyroscope measurements. - * - * The time constant \f$\tau_\mathrm{acc}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the - * second-order Butterworth low-pass filter as follows: \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau_\mathrm{acc}}\f$. - * - * Default value: 3.0 s - */ - vqf_real_t tauAcc; - /** - * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds. - * - * Small values for \f$\tau_\mathrm{mag}\f$ imply trust on the magnetometer measurements and while large values of - * \f$\tau_\mathrm{mag}\f$ imply trust on the gyroscope measurements. - * - * The time constant \f$\tau_\mathrm{mag}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the - * first-order low-pass filter for the heading correction as follows: - * \f$f_\mathrm{c} = \frac{1}{2\pi\tau_\mathrm{mag}}\f$. - * - * Default value: 9.0 s - */ - vqf_real_t tauMag; +struct VQFParams { + /** + * @brief Constructor that initializes the struct with the default parameters. + */ + VQFParams(); + + /** + * @brief Time constant \f$\tau_\mathrm{acc}\f$ for accelerometer low-pass filtering + * in seconds. + * + * Small values for \f$\tau_\mathrm{acc}\f$ imply trust on the accelerometer + * measurements and while large values of \f$\tau_\mathrm{acc}\f$ imply trust on the + * gyroscope measurements. + * + * The time constant \f$\tau_\mathrm{acc}\f$ corresponds to the cutoff frequency + * \f$f_\mathrm{c}\f$ of the second-order Butterworth low-pass filter as follows: + * \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau_\mathrm{acc}}\f$. + * + * Default value: 3.0 s + */ + vqf_real_t tauAcc; + /** + * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds. + * + * Small values for \f$\tau_\mathrm{mag}\f$ imply trust on the magnetometer + * measurements and while large values of \f$\tau_\mathrm{mag}\f$ imply trust on the + * gyroscope measurements. + * + * The time constant \f$\tau_\mathrm{mag}\f$ corresponds to the cutoff frequency + * \f$f_\mathrm{c}\f$ of the first-order low-pass filter for the heading correction + * as follows: \f$f_\mathrm{c} = \frac{1}{2\pi\tau_\mathrm{mag}}\f$. + * + * Default value: 9.0 s + */ + vqf_real_t tauMag; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Enables gyroscope bias estimation during motion phases. - * - * If set to true (default), gyroscope bias is estimated based on the inclination correction only, i.e. without - * using magnetometer measurements. - */ - bool motionBiasEstEnabled; + /** + * @brief Enables gyroscope bias estimation during motion phases. + * + * If set to true (default), gyroscope bias is estimated based on the inclination + * correction only, i.e. without using magnetometer measurements. + */ + bool motionBiasEstEnabled; #endif - /** - * @brief Enables rest detection and gyroscope bias estimation during rest phases. - * - * If set to true (default), phases in which the IMU is at rest are detected. During rest, the gyroscope bias - * is estimated from the low-pass filtered gyroscope readings. - */ - bool restBiasEstEnabled; - /** - * @brief Enables magnetic disturbance detection and magnetic disturbance rejection. - * - * If set to true (default), the magnetic field is analyzed. For short disturbed phases, the magnetometer-based - * correction is disabled totally. If the magnetic field is always regarded as disturbed or if the duration of - * the disturbances exceeds #magMaxRejectionTime, magnetometer-based updates are performed, but with an increased - * time constant. - */ - bool magDistRejectionEnabled; - - /** - * @brief Standard deviation of the initial bias estimation uncertainty (in degrees per second). - * - * Default value: 0.5 °/s - */ - vqf_real_t biasSigmaInit; - /** - * @brief Time in which the bias estimation uncertainty increases from 0 °/s to 0.1 °/s (in seconds). - * - * This value determines the system noise assumed by the Kalman filter. - * - * Default value: 100.0 s - */ - vqf_real_t biasForgettingTime; - /** - * @brief Maximum expected gyroscope bias (in degrees per second). - * - * This value is used to clip the bias estimate and the measurement error in the bias estimation update step. It is - * further used by the rest detection algorithm in order to not regard measurements with a large but constant - * angular rate as rest. - * - * Default value: 2.0 °/s - */ - vqf_real_t biasClip; + /** + * @brief Enables rest detection and gyroscope bias estimation during rest phases. + * + * If set to true (default), phases in which the IMU is at rest are detected. During + * rest, the gyroscope bias is estimated from the low-pass filtered gyroscope + * readings. + */ + bool restBiasEstEnabled; + /** + * @brief Enables magnetic disturbance detection and magnetic disturbance rejection. + * + * If set to true (default), the magnetic field is analyzed. For short disturbed + * phases, the magnetometer-based correction is disabled totally. If the magnetic + * field is always regarded as disturbed or if the duration of the disturbances + * exceeds #magMaxRejectionTime, magnetometer-based updates are performed, but with + * an increased time constant. + */ + bool magDistRejectionEnabled; + + /** + * @brief Standard deviation of the initial bias estimation uncertainty (in degrees + * per second). + * + * Default value: 0.5 °/s + */ + vqf_real_t biasSigmaInit; + /** + * @brief Time in which the bias estimation uncertainty increases from 0 °/s to 0.1 + * °/s (in seconds). + * + * This value determines the system noise assumed by the Kalman filter. + * + * Default value: 100.0 s + */ + vqf_real_t biasForgettingTime; + /** + * @brief Maximum expected gyroscope bias (in degrees per second). + * + * This value is used to clip the bias estimate and the measurement error in the + * bias estimation update step. It is further used by the rest detection algorithm + * in order to not regard measurements with a large but constant angular rate as + * rest. + * + * Default value: 2.0 °/s + */ + vqf_real_t biasClip; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Standard deviation of the converged bias estimation uncertainty during motion (in degrees per second). - * - * This value determines the trust on motion bias estimation updates. A small value leads to fast convergence. - * - * Default value: 0.1 °/s - */ - vqf_real_t biasSigmaMotion; - /** - * @brief Forgetting factor for unobservable bias in vertical direction during motion. - * - * As magnetometer measurements are deliberately not used during motion bias estimation, gyroscope bias is not - * observable in vertical direction. This value is the relative weight of an artificial zero measurement that - * ensures that the bias estimate in the unobservable direction will eventually decay to zero. - * - * Default value: 0.0001 - */ - vqf_real_t biasVerticalForgettingFactor; + /** + * @brief Standard deviation of the converged bias estimation uncertainty during + * motion (in degrees per second). + * + * This value determines the trust on motion bias estimation updates. A small value + * leads to fast convergence. + * + * Default value: 0.1 °/s + */ + vqf_real_t biasSigmaMotion; + /** + * @brief Forgetting factor for unobservable bias in vertical direction during + * motion. + * + * As magnetometer measurements are deliberately not used during motion bias + * estimation, gyroscope bias is not observable in vertical direction. This value is + * the relative weight of an artificial zero measurement that ensures that the bias + * estimate in the unobservable direction will eventually decay to zero. + * + * Default value: 0.0001 + */ + vqf_real_t biasVerticalForgettingFactor; #endif - /** - * @brief Standard deviation of the converged bias estimation uncertainty during rest (in degrees per second). - * - * This value determines the trust on rest bias estimation updates. A small value leads to fast convergence. - * - * Default value: 0.03 ° - */ - vqf_real_t biasSigmaRest; - - /** - * @brief Time threshold for rest detection (in seconds). - * - * Rest is detected when the measurements have been close to the low-pass filtered reference for the given time. - * - * Default value: 1.5 s - */ - vqf_real_t restMinT; - /** - * @brief Time constant for the low-pass filter used in rest detection (in seconds). - * - * This time constant characterizes a second-order Butterworth low-pass filter used to obtain the reference for - * rest detection. - * - * Default value: 0.5 s - */ - vqf_real_t restFilterTau; - /** - * @brief Angular velocity threshold for rest detection (in °/s). - * - * For rest to be detected, the norm of the deviation between measurement and reference must be below the given - * threshold. (Furthermore, the absolute value of each component must be below #biasClip). - * - * Default value: 2.0 °/s - */ - vqf_real_t restThGyr; - /** - * @brief Acceleration threshold for rest detection (in m/s²). - * - * For rest to be detected, the norm of the deviation between measurement and reference must be below the given - * threshold. - * - * Default value: 0.5 m/s² - */ - vqf_real_t restThAcc; - - /** - * @brief Time constant for current norm/dip value in magnetic disturbance detection (in seconds). - * - * This (very fast) low-pass filter is intended to provide additional robustness when the magnetometer measurements - * are noisy or not sampled perfectly in sync with the gyroscope measurements. Set to -1 to disable the low-pass - * filter and directly use the magnetometer measurements. - * - * Default value: 0.05 s - */ - vqf_real_t magCurrentTau; - /** - * @brief Time constant for the adjustment of the magnetic field reference (in seconds). - * - * This adjustment allows the reference estimate to converge to the observed undisturbed field. - * - * Default value: 20.0 s - */ - vqf_real_t magRefTau; - /** - * @brief Relative threshold for the magnetic field strength for magnetic disturbance detection. - * - * This value is relative to the reference norm. - * - * Default value: 0.1 (10%) - */ - vqf_real_t magNormTh; - /** - * @brief Threshold for the magnetic field dip angle for magnetic disturbance detection (in degrees). - * - * Default vaule: 10 ° - */ - vqf_real_t magDipTh; - /** - * @brief Duration after which to accept a different homogeneous magnetic field (in seconds). - * - * A different magnetic field reference is accepted as the new field when the measurements are within the thresholds - * #magNormTh and #magDipTh for the given time. Additionally, only phases with sufficient movement, specified by - * #magNewMinGyr, count. - * - * Default value: 20.0 - */ - vqf_real_t magNewTime; - /** - * @brief Duration after which to accept a homogeneous magnetic field for the first time (in seconds). - * - * This value is used instead of #magNewTime when there is no current estimate in order to allow for the initial - * magnetic field reference to be obtained faster. - * - * Default value: 5.0 - */ - vqf_real_t magNewFirstTime; - /** - * @brief Minimum angular velocity needed in order to count time for new magnetic field acceptance (in °/s). - * - * Durations for which the angular velocity norm is below this threshold do not count towards reaching #magNewTime. - * - * Default value: 20.0 °/s - */ - vqf_real_t magNewMinGyr; - /** - * @brief Minimum duration within thresholds after which to regard the field as undisturbed again (in seconds). - * - * Default value: 0.5 s - */ - vqf_real_t magMinUndisturbedTime; - /** - * @brief Maximum duration of full magnetic disturbance rejection (in seconds). - * - * For magnetic disturbances up to this duration, heading correction is fully disabled and heading changes are - * tracked by gyroscope only. After this duration (or for many small disturbed phases without sufficient time in the - * undisturbed field in between), the heading correction is performed with an increased time constant (see - * #magRejectionFactor). - * - * Default value: 60.0 s - */ - vqf_real_t magMaxRejectionTime; - /** - * @brief Factor by which to slow the heading correction during long disturbed phases. - * - * After #magMaxRejectionTime of full magnetic disturbance rejection, heading correction is performed with an - * increased time constant. This parameter (approximately) specifies the factor of the increase. - * - * Furthermore, after spending #magMaxRejectionTime/#magRejectionFactor seconds in an undisturbed magnetic field, - * the time is reset and full magnetic disturbance rejection will be performed for up to #magMaxRejectionTime again. - * - * Default value: 2.0 - */ - vqf_real_t magRejectionFactor; + /** + * @brief Standard deviation of the converged bias estimation uncertainty during + * rest (in degrees per second). + * + * This value determines the trust on rest bias estimation updates. A small value + * leads to fast convergence. + * + * Default value: 0.03 ° + */ + vqf_real_t biasSigmaRest; + + /** + * @brief Time threshold for rest detection (in seconds). + * + * Rest is detected when the measurements have been close to the low-pass filtered + * reference for the given time. + * + * Default value: 1.5 s + */ + vqf_real_t restMinT; + /** + * @brief Time constant for the low-pass filter used in rest detection (in seconds). + * + * This time constant characterizes a second-order Butterworth low-pass filter used + * to obtain the reference for rest detection. + * + * Default value: 0.5 s + */ + vqf_real_t restFilterTau; + /** + * @brief Angular velocity threshold for rest detection (in °/s). + * + * For rest to be detected, the norm of the deviation between measurement and + * reference must be below the given threshold. (Furthermore, the absolute value of + * each component must be below #biasClip). + * + * Default value: 2.0 °/s + */ + vqf_real_t restThGyr; + /** + * @brief Acceleration threshold for rest detection (in m/s²). + * + * For rest to be detected, the norm of the deviation between measurement and + * reference must be below the given threshold. + * + * Default value: 0.5 m/s² + */ + vqf_real_t restThAcc; + + /** + * @brief Time constant for current norm/dip value in magnetic disturbance detection + * (in seconds). + * + * This (very fast) low-pass filter is intended to provide additional robustness + * when the magnetometer measurements are noisy or not sampled perfectly in sync + * with the gyroscope measurements. Set to -1 to disable the low-pass filter and + * directly use the magnetometer measurements. + * + * Default value: 0.05 s + */ + vqf_real_t magCurrentTau; + /** + * @brief Time constant for the adjustment of the magnetic field reference (in + * seconds). + * + * This adjustment allows the reference estimate to converge to the observed + * undisturbed field. + * + * Default value: 20.0 s + */ + vqf_real_t magRefTau; + /** + * @brief Relative threshold for the magnetic field strength for magnetic + * disturbance detection. + * + * This value is relative to the reference norm. + * + * Default value: 0.1 (10%) + */ + vqf_real_t magNormTh; + /** + * @brief Threshold for the magnetic field dip angle for magnetic disturbance + * detection (in degrees). + * + * Default vaule: 10 ° + */ + vqf_real_t magDipTh; + /** + * @brief Duration after which to accept a different homogeneous magnetic field (in + * seconds). + * + * A different magnetic field reference is accepted as the new field when the + * measurements are within the thresholds #magNormTh and #magDipTh for the given + * time. Additionally, only phases with sufficient movement, specified by + * #magNewMinGyr, count. + * + * Default value: 20.0 + */ + vqf_real_t magNewTime; + /** + * @brief Duration after which to accept a homogeneous magnetic field for the first + * time (in seconds). + * + * This value is used instead of #magNewTime when there is no current estimate in + * order to allow for the initial magnetic field reference to be obtained faster. + * + * Default value: 5.0 + */ + vqf_real_t magNewFirstTime; + /** + * @brief Minimum angular velocity needed in order to count time for new magnetic + * field acceptance (in °/s). + * + * Durations for which the angular velocity norm is below this threshold do not + * count towards reaching #magNewTime. + * + * Default value: 20.0 °/s + */ + vqf_real_t magNewMinGyr; + /** + * @brief Minimum duration within thresholds after which to regard the field as + * undisturbed again (in seconds). + * + * Default value: 0.5 s + */ + vqf_real_t magMinUndisturbedTime; + /** + * @brief Maximum duration of full magnetic disturbance rejection (in seconds). + * + * For magnetic disturbances up to this duration, heading correction is fully + * disabled and heading changes are tracked by gyroscope only. After this duration + * (or for many small disturbed phases without sufficient time in the undisturbed + * field in between), the heading correction is performed with an increased time + * constant (see #magRejectionFactor). + * + * Default value: 60.0 s + */ + vqf_real_t magMaxRejectionTime; + /** + * @brief Factor by which to slow the heading correction during long disturbed + * phases. + * + * After #magMaxRejectionTime of full magnetic disturbance rejection, heading + * correction is performed with an increased time constant. This parameter + * (approximately) specifies the factor of the increase. + * + * Furthermore, after spending #magMaxRejectionTime/#magRejectionFactor seconds in + * an undisturbed magnetic field, the time is reset and full magnetic disturbance + * rejection will be performed for up to #magMaxRejectionTime again. + * + * Default value: 2.0 + */ + vqf_real_t magRejectionFactor; }; /** * @brief Struct containing the filter state of the VQF class. * - * The relevant parts of the state can be accessed via functions of the VQF class, e.g. VQF::getQuat6D(), - * VQF::getQuat9D(), VQF::getGyrBiasEstimate(), VQF::setGyrBiasEstimate(), VQF::getRestDetected() and - * VQF::getMagDistDetected(). To reset the state to the initial values, use VQF::resetState(). + * The relevant parts of the state can be accessed via functions of the VQF class, e.g. + * VQF::getQuat6D(), VQF::getQuat9D(), VQF::getGyrBiasEstimate(), + * VQF::setGyrBiasEstimate(), VQF::getRestDetected() and VQF::getMagDistDetected(). To + * reset the state to the initial values, use VQF::resetState(). * - * Direct access to the full state is typically not needed but can be useful in some cases, e.g. for debugging. For this - * purpose, the state can be accessed by VQF::getState() and set by VQF::setState(). + * Direct access to the full state is typically not needed but can be useful in some + * cases, e.g. for debugging. For this purpose, the state can be accessed by + * VQF::getState() and set by VQF::setState(). */ struct VQFState { - /** - * @brief Angular velocity strapdown integration quaternion \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. - */ - vqf_real_t gyrQuat[4]; - /** - * @brief Inclination correction quaternion \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$. - */ - vqf_real_t accQuat[4]; - /** - * @brief Heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$. - * - * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & - * \sin\frac{\delta}{2}\end{bmatrix}^T\f$. - */ - vqf_real_t delta; - /** - * @brief True if it has been detected that the IMU is currently at rest. - * - * Used to switch between rest and motion gyroscope bias estimation. - */ - bool restDetected; - /** - * @brief True if magnetic disturbances have been detected. - */ - bool magDistDetected; - - /** - * @brief Last low-pass filtered acceleration in the \f$\mathcal{I}_i\f$ frame. - */ - vqf_real_t lastAccLp[3]; - /** - * @brief Internal low-pass filter state for #lastAccLp. - */ - double accLpState[3*2]; - /** - * @brief Last inclination correction angular rate. - * - * Change to inclination correction quaternion \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$ performed in the - * last accelerometer update, expressed as an angular rate (in rad/s). - */ - vqf_real_t lastAccCorrAngularRate; - - /** - * @brief Gain used for heading correction to ensure fast initial convergence. - * - * This value is used as the gain for heading correction in the beginning if it is larger than the normal filter - * gain. It is initialized to 1 and then updated to 0.5, 0.33, 0.25, ... After VQFParams::tauMag seconds, it is - * set to zero. - */ - vqf_real_t kMagInit; - /** - * @brief Last heading disagreement angle. - * - * Disagreement between the heading \f$\hat\delta\f$ estimated from the last magnetometer sample and the state - * \f$\delta\f$ (in rad). - */ - vqf_real_t lastMagDisAngle; - /** - * @brief Last heading correction angular rate. - * - * Change to heading \f$\delta\f$ performed in the last magnetometer update, - * expressed as an angular rate (in rad/s). - */ - vqf_real_t lastMagCorrAngularRate; - - /** - * @brief Current gyroscope bias estimate (in rad/s). - */ - vqf_real_t bias[3]; + /** + * @brief Angular velocity strapdown integration quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. + */ + vqf_real_t gyrQuat[4]; + /** + * @brief Inclination correction quaternion + * \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$. + */ + vqf_real_t accQuat[4]; + /** + * @brief Heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and + * \f$\mathcal{E}\f$. + * + * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} + * & 0 & 0 & \sin\frac{\delta}{2}\end{bmatrix}^T\f$. + */ + vqf_real_t delta; + /** + * @brief True if it has been detected that the IMU is currently at rest. + * + * Used to switch between rest and motion gyroscope bias estimation. + */ + bool restDetected; + /** + * @brief True if magnetic disturbances have been detected. + */ + bool magDistDetected; + + /** + * @brief Last low-pass filtered acceleration in the \f$\mathcal{I}_i\f$ frame. + */ + vqf_real_t lastAccLp[3]; + /** + * @brief Internal low-pass filter state for #lastAccLp. + */ + double accLpState[3 * 2]; + /** + * @brief Last inclination correction angular rate. + * + * Change to inclination correction quaternion + * \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$ performed in the last + * accelerometer update, expressed as an angular rate (in rad/s). + */ + vqf_real_t lastAccCorrAngularRate; + + /** + * @brief Gain used for heading correction to ensure fast initial convergence. + * + * This value is used as the gain for heading correction in the beginning if it is + * larger than the normal filter gain. It is initialized to 1 and then updated to + * 0.5, 0.33, 0.25, ... After VQFParams::tauMag seconds, it is set to zero. + */ + vqf_real_t kMagInit; + /** + * @brief Last heading disagreement angle. + * + * Disagreement between the heading \f$\hat\delta\f$ estimated from the last + * magnetometer sample and the state \f$\delta\f$ (in rad). + */ + vqf_real_t lastMagDisAngle; + /** + * @brief Last heading correction angular rate. + * + * Change to heading \f$\delta\f$ performed in the last magnetometer update, + * expressed as an angular rate (in rad/s). + */ + vqf_real_t lastMagCorrAngularRate; + + /** + * @brief Current gyroscope bias estimate (in rad/s). + */ + vqf_real_t bias[3]; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Covariance matrix of the gyroscope bias estimate. - * - * The 3x3 matrix is stored in row-major order. Note that for numeric reasons the internal unit used is 0.01 °/s, - * i.e. to get the standard deviation in degrees per second use \f$\sigma = \frac{\sqrt{p_{ii}}}{100}\f$. - */ - vqf_real_t biasP[9]; + /** + * @brief Covariance matrix of the gyroscope bias estimate. + * + * The 3x3 matrix is stored in row-major order. Note that for numeric reasons the + * internal unit used is 0.01 °/s, i.e. to get the standard deviation in degrees per + * second use \f$\sigma = \frac{\sqrt{p_{ii}}}{100}\f$. + */ + vqf_real_t biasP[9]; #else - // If only rest gyr bias estimation is enabled, P and K of the KF are always diagonal - // and matrix inversion is not needed. If motion bias estimation is disabled at compile - // time, storing the full P matrix is not necessary. - vqf_real_t biasP; + // If only rest gyr bias estimation is enabled, P and K of the KF are always + // diagonal and matrix inversion is not needed. If motion bias estimation is + // disabled at compile time, storing the full P matrix is not necessary. + vqf_real_t biasP; #endif #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Internal state of the Butterworth low-pass filter for the rotation matrix coefficients used in motion - * bias estimation. - */ - double motionBiasEstRLpState[9*2]; - /** - * @brief Internal low-pass filter state for the rotated bias estimate used in motion bias estimation. - */ - double motionBiasEstBiasLpState[2*2]; + /** + * @brief Internal state of the Butterworth low-pass filter for the rotation matrix + * coefficients used in motion bias estimation. + */ + double motionBiasEstRLpState[9 * 2]; + /** + * @brief Internal low-pass filter state for the rotated bias estimate used in + * motion bias estimation. + */ + double motionBiasEstBiasLpState[2 * 2]; #endif - /** - * @brief Last (squared) deviations from the reference of the last sample used in rest detection. - * - * Looking at those values can be useful to understand how rest detection is working and which thresholds are - * suitable. The array contains the last values for gyroscope and accelerometer in the respective - * units. Note that the values are squared. - * - * The method VQF::getRelativeRestDeviations() provides an easier way to obtain and interpret those values. - */ - vqf_real_t restLastSquaredDeviations[2]; - /** - * @brief The current duration for which all sensor readings are within the rest detection thresholds. - * - * Rest is detected if this value is larger or equal to VQFParams::restMinT. - */ - vqf_real_t restT; - /** - * @brief Last low-pass filtered gyroscope measurement used as the reference for rest detection. - * - * Note that this value is also used for gyroscope bias estimation when rest is detected. - */ - vqf_real_t restLastGyrLp[3]; - /** - * @brief Internal low-pass filter state for #restLastGyrLp. - */ - double restGyrLpState[3*2]; - /** - * @brief Last low-pass filtered accelerometer measurement used as the reference for rest detection. - */ - vqf_real_t restLastAccLp[3]; - /** - * @brief Internal low-pass filter state for #restLastAccLp. - */ - double restAccLpState[3*2]; - - /** - * @brief Norm of the currently accepted magnetic field reference. - * - * A value of -1 indicates that no homogeneous field is found yet. - */ - vqf_real_t magRefNorm; - /** - * @brief Dip angle of the currently accepted magnetic field reference. - */ - vqf_real_t magRefDip; - /** - * @brief The current duration for which the current norm and dip are close to the reference. - * - * The magnetic field is regarded as undisturbed when this value reaches VQFParams::magMinUndisturbedTime. - */ - vqf_real_t magUndisturbedT; - /** - * @brief The current duration for which the magnetic field was rejected. - * - * If the magnetic field is disturbed and this value is smaller than VQFParams::magMaxRejectionTime, heading - * correction updates are fully disabled. - */ - vqf_real_t magRejectT; - /** - * @brief Norm of the alternative magnetic field reference currently being evaluated. - */ - vqf_real_t magCandidateNorm; - /** - * @brief Dip angle of the alternative magnetic field reference currently being evaluated. - */ - vqf_real_t magCandidateDip; - /** - * @brief The current duration for which the norm and dip are close to the candidate. - * - * If this value exceeds VQFParams::magNewTime (or VQFParams::magNewFirstTime if #magRefNorm < 0), the current - * candidate is accepted as the new reference. - */ - vqf_real_t magCandidateT; - /** - * @brief Norm and dip angle of the current magnetometer measurements. - * - * Slightly low-pass filtered, see VQFParams::magCurrentTau. - */ - vqf_real_t magNormDip[2]; - /** - * @brief Internal low-pass filter state for the current norm and dip angle. - */ - double magNormDipLpState[2*2]; + /** + * @brief Last (squared) deviations from the reference of the last sample used in + * rest detection. + * + * Looking at those values can be useful to understand how rest detection is working + * and which thresholds are suitable. The array contains the last values for + * gyroscope and accelerometer in the respective units. Note that the values are + * squared. + * + * The method VQF::getRelativeRestDeviations() provides an easier way to obtain and + * interpret those values. + */ + vqf_real_t restLastSquaredDeviations[2]; + /** + * @brief The current duration for which all sensor readings are within the rest + * detection thresholds. + * + * Rest is detected if this value is larger or equal to VQFParams::restMinT. + */ + vqf_real_t restT; + /** + * @brief Last low-pass filtered gyroscope measurement used as the reference for + * rest detection. + * + * Note that this value is also used for gyroscope bias estimation when rest is + * detected. + */ + vqf_real_t restLastGyrLp[3]; + /** + * @brief Internal low-pass filter state for #restLastGyrLp. + */ + double restGyrLpState[3 * 2]; + /** + * @brief Last low-pass filtered accelerometer measurement used as the reference for + * rest detection. + */ + vqf_real_t restLastAccLp[3]; + /** + * @brief Internal low-pass filter state for #restLastAccLp. + */ + double restAccLpState[3 * 2]; + + /** + * @brief Norm of the currently accepted magnetic field reference. + * + * A value of -1 indicates that no homogeneous field is found yet. + */ + vqf_real_t magRefNorm; + /** + * @brief Dip angle of the currently accepted magnetic field reference. + */ + vqf_real_t magRefDip; + /** + * @brief The current duration for which the current norm and dip are close to the + * reference. + * + * The magnetic field is regarded as undisturbed when this value reaches + * VQFParams::magMinUndisturbedTime. + */ + vqf_real_t magUndisturbedT; + /** + * @brief The current duration for which the magnetic field was rejected. + * + * If the magnetic field is disturbed and this value is smaller than + * VQFParams::magMaxRejectionTime, heading correction updates are fully disabled. + */ + vqf_real_t magRejectT; + /** + * @brief Norm of the alternative magnetic field reference currently being + * evaluated. + */ + vqf_real_t magCandidateNorm; + /** + * @brief Dip angle of the alternative magnetic field reference currently being + * evaluated. + */ + vqf_real_t magCandidateDip; + /** + * @brief The current duration for which the norm and dip are close to the + * candidate. + * + * If this value exceeds VQFParams::magNewTime (or VQFParams::magNewFirstTime if + * #magRefNorm < 0), the current candidate is accepted as the new reference. + */ + vqf_real_t magCandidateT; + /** + * @brief Norm and dip angle of the current magnetometer measurements. + * + * Slightly low-pass filtered, see VQFParams::magCurrentTau. + */ + vqf_real_t magNormDip[2]; + /** + * @brief Internal low-pass filter state for the current norm and dip angle. + */ + double magNormDipLpState[2 * 2]; }; /** * @brief Struct containing coefficients used by the VQF class. * - * Coefficients are values that depend on the parameters and the sampling times, but do not change during update steps. - * They are calculated in VQF::setup(). + * Coefficients are values that depend on the parameters and the sampling times, but do + * not change during update steps. They are calculated in VQF::setup(). */ -struct VQFCoefficients -{ - /** - * @brief Sampling time of the gyroscope measurements (in seconds). - */ - vqf_real_t gyrTs; - /** - * @brief Sampling time of the accelerometer measurements (in seconds). - */ - vqf_real_t accTs; - /** - * @brief Sampling time of the magnetometer measurements (in seconds). - */ - vqf_real_t magTs; - - /** - * @brief Numerator coefficients of the acceleration low-pass filter. - * - * The array contains \f$\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\f$. - */ - double accLpB[3]; - /** - * @brief Denominator coefficients of the acceleration low-pass filter. - * - * The array contains \f$\begin{bmatrix}a_1 & a_2\end{bmatrix}\f$ and \f$a_0=1\f$. - */ - double accLpA[2]; - - /** - * @brief Gain of the first-order filter used for heading correction. - */ - vqf_real_t kMag; - - /** - * @brief Variance of the initial gyroscope bias estimate. - */ - vqf_real_t biasP0; - /** - * @brief System noise variance used in gyroscope bias estimation. - */ - vqf_real_t biasV; +struct VQFCoefficients { + /** + * @brief Sampling time of the gyroscope measurements (in seconds). + */ + vqf_real_t gyrTs; + /** + * @brief Sampling time of the accelerometer measurements (in seconds). + */ + vqf_real_t accTs; + /** + * @brief Sampling time of the magnetometer measurements (in seconds). + */ + vqf_real_t magTs; + + /** + * @brief Numerator coefficients of the acceleration low-pass filter. + * + * The array contains \f$\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\f$. + */ + double accLpB[3]; + /** + * @brief Denominator coefficients of the acceleration low-pass filter. + * + * The array contains \f$\begin{bmatrix}a_1 & a_2\end{bmatrix}\f$ and \f$a_0=1\f$. + */ + double accLpA[2]; + + /** + * @brief Gain of the first-order filter used for heading correction. + */ + vqf_real_t kMag; + + /** + * @brief Variance of the initial gyroscope bias estimate. + */ + vqf_real_t biasP0; + /** + * @brief System noise variance used in gyroscope bias estimation. + */ + vqf_real_t biasV; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Measurement noise variance for the motion gyroscope bias estimation update. - */ - vqf_real_t biasMotionW; - /** - * @brief Measurement noise variance for the motion gyroscope bias estimation update in vertical direction. - */ - vqf_real_t biasVerticalW; + /** + * @brief Measurement noise variance for the motion gyroscope bias estimation + * update. + */ + vqf_real_t biasMotionW; + /** + * @brief Measurement noise variance for the motion gyroscope bias estimation update + * in vertical direction. + */ + vqf_real_t biasVerticalW; #endif - /** - * @brief Measurement noise variance for the rest gyroscope bias estimation update. - */ - vqf_real_t biasRestW; - - /** - * @brief Numerator coefficients of the gyroscope measurement low-pass filter for rest detection. - */ - double restGyrLpB[3]; - /** - * @brief Denominator coefficients of the gyroscope measurement low-pass filter for rest detection. - */ - double restGyrLpA[2]; - /** - * @brief Numerator coefficients of the accelerometer measurement low-pass filter for rest detection. - */ - double restAccLpB[3]; - /** - * @brief Denominator coefficients of the accelerometer measurement low-pass filter for rest detection. - */ - double restAccLpA[2]; - - /** - * @brief Gain of the first-order filter used for to update the magnetic field reference and candidate. - */ - vqf_real_t kMagRef; - /** - * @brief Numerator coefficients of the low-pass filter for the current magnetic norm and dip. - */ - double magNormDipLpB[3]; - /** - * @brief Denominator coefficients of the low-pass filter for the current magnetic norm and dip. - */ - double magNormDipLpA[2]; + /** + * @brief Measurement noise variance for the rest gyroscope bias estimation update. + */ + vqf_real_t biasRestW; + + /** + * @brief Numerator coefficients of the gyroscope measurement low-pass filter for + * rest detection. + */ + double restGyrLpB[3]; + /** + * @brief Denominator coefficients of the gyroscope measurement low-pass filter for + * rest detection. + */ + double restGyrLpA[2]; + /** + * @brief Numerator coefficients of the accelerometer measurement low-pass filter + * for rest detection. + */ + double restAccLpB[3]; + /** + * @brief Denominator coefficients of the accelerometer measurement low-pass filter + * for rest detection. + */ + double restAccLpA[2]; + + /** + * @brief Gain of the first-order filter used for to update the magnetic field + * reference and candidate. + */ + vqf_real_t kMagRef; + /** + * @brief Numerator coefficients of the low-pass filter for the current magnetic + * norm and dip. + */ + double magNormDipLpB[3]; + /** + * @brief Denominator coefficients of the low-pass filter for the current magnetic + * norm and dip. + */ + double magNormDipLpA[2]; }; /** * @brief A Versatile Quaternion-based Filter for IMU Orientation Estimation. * * \rst - * This class implements the orientation estimation filter described in the following publication: + * This class implements the orientation estimation filter described in the following + * publication: * * - * D. Laidig and T. Seel. "VQF: Highly Accurate IMU Orientation Estimation with Bias Estimation and Magnetic - * Disturbance Rejection." Information Fusion 2023, 91, 187--204. - * `doi:10.1016/j.inffus.2022.10.014 `_. - * [Accepted manuscript available at `arXiv:2203.17024 `_.] + * D. Laidig and T. Seel. "VQF: Highly Accurate IMU Orientation Estimation with Bias + * Estimation and Magnetic Disturbance Rejection." Information Fusion 2023, 91, + * 187--204. `doi:10.1016/j.inffus.2022.10.014 + * `_. [Accepted manuscript available at + * `arXiv:2203.17024 `_.] * - * The filter can perform simultaneous 6D (magnetometer-free) and 9D (gyr+acc+mag) sensor fusion and can also be used - * without magnetometer data. It performs rest detection, gyroscope bias estimation during rest and motion, and magnetic - * disturbance detection and rejection. Different sampling rates for gyroscopes, accelerometers, and magnetometers are - * supported as well. While in most cases, the defaults will be reasonable, the algorithm can be influenced via a - * number of tuning parameters. + * The filter can perform simultaneous 6D (magnetometer-free) and 9D (gyr+acc+mag) + * sensor fusion and can also be used without magnetometer data. It performs rest + * detection, gyroscope bias estimation during rest and motion, and magnetic disturbance + * detection and rejection. Different sampling rates for gyroscopes, accelerometers, and + * magnetometers are supported as well. While in most cases, the defaults will be + * reasonable, the algorithm can be influenced via a number of tuning parameters. * * To use this C++ implementation, * - * 1. create a instance of the class and provide the sampling time and, optionally, parameters - * 2. for every sample, call one of the update functions to feed the algorithm with IMU data - * 3. access the estimation results with :meth:`getQuat6D() `, :meth:`getQuat9D() ` and - * the other getter methods. + * 1. create a instance of the class and provide the sampling time and, optionally, + * parameters + * 2. for every sample, call one of the update functions to feed the algorithm with IMU + * data + * 3. access the estimation results with :meth:`getQuat6D() `, + * :meth:`getQuat9D() ` and the other getter methods. * - * If the full data is available in (row-major) data buffers, you can use :meth:`updateBatch() `. + * If the full data is available in (row-major) data buffers, you can use + * :meth:`updateBatch() `. * - * This class is the main C++ implementation of the algorithm. Depending on use case and programming language of choice, - * the following alternatives might be useful: + * This class is the main C++ implementation of the algorithm. Depending on use case and + * programming language of choice, the following alternatives might be useful: * * +------------------------+--------------------------+--------------------------+---------------------------+ - * | | Full Version | Basic Version | Offline Version | - * | | | | | + * | | Full Version | Basic Version | + * Offline Version | | | | | | * +========================+==========================+==========================+===========================+ - * | **C++** | **VQF (this class)** | :cpp:class:`BasicVQF` | :cpp:func:`offlineVQF` | + * | **C++** | **VQF (this class)** | :cpp:class:`BasicVQF` | + * :cpp:func:`offlineVQF` | * +------------------------+--------------------------+--------------------------+---------------------------+ - * | **Python/C++ (fast)** | :py:class:`vqf.VQF` | :py:class:`vqf.BasicVQF` | :py:meth:`vqf.offlineVQF` | + * | **Python/C++ (fast)** | :py:class:`vqf.VQF` | :py:class:`vqf.BasicVQF` | + * :py:meth:`vqf.offlineVQF` | * +------------------------+--------------------------+--------------------------+---------------------------+ - * | **Pure Python (slow)** | :py:class:`vqf.PyVQF` | -- | -- | + * | **Pure Python (slow)** | :py:class:`vqf.PyVQF` | -- | -- | * +------------------------+--------------------------+--------------------------+---------------------------+ - * | **Pure Matlab (slow)** | :mat:class:`VQF.m ` | -- | -- | + * | **Pure Matlab (slow)** | :mat:class:`VQF.m ` | -- | -- | * +------------------------+--------------------------+--------------------------+---------------------------+ * \endrst */ -class VQF -{ +class VQF { public: - /** - * Initializes the object with default parameters. - * - * In the most common case (using the default parameters and all data being sampled with the same frequency, - * create the class like this: - * \rst - * .. code-block:: c++ - * - * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz - * \endrst - * - * @param gyrTs sampling time of the gyroscope measurements in seconds - * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1) - * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1) - * - */ - VQF(vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0); - /** - * @brief Initializes the object with custom parameters. - * - * Example code to create an object with magnetic disturbance rejection disabled: - * \rst - * .. code-block:: c++ - * - * VQFParams params; - * params.magDistRejectionEnabled = false; - * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz - * \endrst - * - * @param params VQFParams struct containing the desired parameters - * @param gyrTs sampling time of the gyroscope measurements in seconds - * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1) - * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1) - */ - VQF(const VQFParams& params, vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0); - - /** - * @brief Performs gyroscope update step. - * - * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have - * different sampling rates. Otherwise, simply use #update(). - * - * @param gyr gyroscope measurement in rad/s - */ - void updateGyr(const vqf_real_t gyr[3], double gyrTs); - /** - * @brief Performs accelerometer update step. - * - * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have - * different sampling rates. Otherwise, simply use #update(). - * - * Should be called after #updateGyr and before #updateMag. - * - * @param acc accelerometer measurement in m/s² - */ - void updateAcc(const vqf_real_t acc[3]); - /** - * @brief Performs magnetometer update step. - * - * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have - * different sampling rates. Otherwise, simply use #update(). - * - * Should be called after #updateAcc. - * - * @param mag magnetometer measurement in arbitrary units - */ - void updateMag(const vqf_real_t mag[3]); - - /** - * @brief Returns the angular velocity strapdown integration quaternion - * \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. - * @param out output array for the quaternion - */ - void getQuat3D(vqf_real_t out[4]) const; - /** - * @brief Returns the 6D (magnetometer-free) orientation quaternion - * \f$^{\mathcal{S}_i}_{\mathcal{E}_i}\mathbf{q}\f$. - * @param out output array for the quaternion - */ - void getQuat6D(vqf_real_t out[4]) const; - /** - * @brief Returns the 9D (with magnetometers) orientation quaternion - * \f$^{\mathcal{S}_i}_{\mathcal{E}}\mathbf{q}\f$. - * @param out output array for the quaternion - */ - void getQuat9D(vqf_real_t out[4]) const; - /** - * @brief Returns the heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$. - * - * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & - * \sin\frac{\delta}{2}\end{bmatrix}^T\f$. - * - * @return delta angle in rad (VQFState::delta) - */ - vqf_real_t getDelta() const; - - /** - * @brief Returns the current gyroscope bias estimate and the uncertainty. - * - * The returned standard deviation sigma represents the estimation uncertainty in the worst direction and is based - * on an upper bound of the largest eigenvalue of the covariance matrix. - * - * @param out output array for the gyroscope bias estimate (rad/s) - * @return standard deviation sigma of the estimation uncertainty (rad/s) - */ - vqf_real_t getBiasEstimate(vqf_real_t out[3]) const; - /** - * @brief Sets the current gyroscope bias estimate and the uncertainty. - * - * If a value for the uncertainty sigma is given, the covariance matrix is set to a corresponding scaled identity - * matrix. - * - * @param bias gyroscope bias estimate (rad/s) - * @param sigma standard deviation of the estimation uncertainty (rad/s) - set to -1 (default) in order to not - * change the estimation covariance matrix - */ - void setBiasEstimate(vqf_real_t bias[3], vqf_real_t sigma=-1.0); - /** - * @brief Returns true if rest was detected. - */ - bool getRestDetected() const; - /** - * @brief Returns true if a disturbed magnetic field was detected. - */ - bool getMagDistDetected() const; - /** - * @brief Returns the relative deviations used in rest detection. - * - * Looking at those values can be useful to understand how rest detection is working and which thresholds are - * suitable. The output array is filled with the last values for gyroscope and accelerometer, - * relative to the threshold. In order for rest to be detected, both values must stay below 1. - * - * @param out output array of size 2 for the relative rest deviations - */ - void getRelativeRestDeviations(vqf_real_t out[2]) const; - /** - * @brief Returns the norm of the currently accepted magnetic field reference. - */ - vqf_real_t getMagRefNorm() const; - /** - * @brief Returns the dip angle of the currently accepted magnetic field reference. - */ - vqf_real_t getMagRefDip() const; - /** - * @brief Overwrites the current magnetic field reference. - * @param norm norm of the magnetic field reference - * @param dip dip angle of the magnetic field reference - */ - void setMagRef(vqf_real_t norm, vqf_real_t dip); - - /** - * @brief Sets the time constant for accelerometer low-pass filtering. - * - * For more details, see VQFParams.tauAcc. - * - * @param tauAcc time constant \f$\tau_\mathrm{acc}\f$ in seconds - */ - void setTauAcc(vqf_real_t tauAcc); - /** - * @brief Sets the time constant for the magnetometer update. - * - * For more details, see VQFParams.tauMag. - * - * @param tauMag time constant \f$\tau_\mathrm{mag}\f$ in seconds - */ - void setTauMag(vqf_real_t tauMag); + /** + * Initializes the object with default parameters. + * + * In the most common case (using the default parameters and all data being sampled + * with the same frequency, create the class like this: \rst + * .. code-block:: c++ + * + * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * \endrst + * + * @param gyrTs sampling time of the gyroscope measurements in seconds + * @param accTs sampling time of the accelerometer measurements in seconds (the + * value of `gyrTs` is used if set to -1) + * @param magTs sampling time of the magnetometer measurements in seconds (the value + * of `gyrTs` is used if set to -1) + * + */ + VQF(vqf_real_t gyrTs, vqf_real_t accTs = -1.0, vqf_real_t magTs = -1.0); + /** + * @brief Initializes the object with custom parameters. + * + * Example code to create an object with magnetic disturbance rejection disabled: + * \rst + * .. code-block:: c++ + * + * VQFParams params; + * params.magDistRejectionEnabled = false; + * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * \endrst + * + * @param params VQFParams struct containing the desired parameters + * @param gyrTs sampling time of the gyroscope measurements in seconds + * @param accTs sampling time of the accelerometer measurements in seconds (the + * value of `gyrTs` is used if set to -1) + * @param magTs sampling time of the magnetometer measurements in seconds (the value + * of `gyrTs` is used if set to -1) + */ + VQF(const VQFParams& params, + vqf_real_t gyrTs, + vqf_real_t accTs = -1.0, + vqf_real_t magTs = -1.0); + + /** + * @brief Performs gyroscope update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers + * and magnetometers have different sampling rates. Otherwise, simply use #update(). + * + * @param gyr gyroscope measurement in rad/s + */ + void updateGyr(const vqf_real_t gyr[3], double gyrTs); + /** + * @brief Performs accelerometer update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers + * and magnetometers have different sampling rates. Otherwise, simply use #update(). + * + * Should be called after #updateGyr and before #updateMag. + * + * @param acc accelerometer measurement in m/s² + */ + void updateAcc(const vqf_real_t acc[3]); + /** + * @brief Performs magnetometer update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers + * and magnetometers have different sampling rates. Otherwise, simply use #update(). + * + * Should be called after #updateAcc. + * + * @param mag magnetometer measurement in arbitrary units + */ + void updateMag(const vqf_real_t mag[3]); + + /** + * @brief Returns the angular velocity strapdown integration quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat3D(vqf_real_t out[4]) const; + /** + * @brief Returns the 6D (magnetometer-free) orientation quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{E}_i}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat6D(vqf_real_t out[4]) const; + /** + * @brief Returns the 9D (with magnetometers) orientation quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{E}}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat9D(vqf_real_t out[4]) const; + /** + * @brief Returns the heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ + * and \f$\mathcal{E}\f$. + * + * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} + * & 0 & 0 & \sin\frac{\delta}{2}\end{bmatrix}^T\f$. + * + * @return delta angle in rad (VQFState::delta) + */ + vqf_real_t getDelta() const; + + /** + * @brief Returns the current gyroscope bias estimate and the uncertainty. + * + * The returned standard deviation sigma represents the estimation uncertainty in + * the worst direction and is based on an upper bound of the largest eigenvalue of + * the covariance matrix. + * + * @param out output array for the gyroscope bias estimate (rad/s) + * @return standard deviation sigma of the estimation uncertainty (rad/s) + */ + vqf_real_t getBiasEstimate(vqf_real_t out[3]) const; + /** + * @brief Sets the current gyroscope bias estimate and the uncertainty. + * + * If a value for the uncertainty sigma is given, the covariance matrix is set to a + * corresponding scaled identity matrix. + * + * @param bias gyroscope bias estimate (rad/s) + * @param sigma standard deviation of the estimation uncertainty (rad/s) - set to -1 + * (default) in order to not change the estimation covariance matrix + */ + void setBiasEstimate(vqf_real_t bias[3], vqf_real_t sigma = -1.0); + /** + * @brief Returns true if rest was detected. + */ + bool getRestDetected() const; + /** + * @brief Returns true if a disturbed magnetic field was detected. + */ + bool getMagDistDetected() const; + /** + * @brief Returns the relative deviations used in rest detection. + * + * Looking at those values can be useful to understand how rest detection is working + * and which thresholds are suitable. The output array is filled with the last + * values for gyroscope and accelerometer, relative to the threshold. In order for + * rest to be detected, both values must stay below 1. + * + * @param out output array of size 2 for the relative rest deviations + */ + void getRelativeRestDeviations(vqf_real_t out[2]) const; + /** + * @brief Returns the norm of the currently accepted magnetic field reference. + */ + vqf_real_t getMagRefNorm() const; + /** + * @brief Returns the dip angle of the currently accepted magnetic field reference. + */ + vqf_real_t getMagRefDip() const; + /** + * @brief Overwrites the current magnetic field reference. + * @param norm norm of the magnetic field reference + * @param dip dip angle of the magnetic field reference + */ + void setMagRef(vqf_real_t norm, vqf_real_t dip); + + /** + * @brief Sets the time constant for accelerometer low-pass filtering. + * + * For more details, see VQFParams.tauAcc. + * + * @param tauAcc time constant \f$\tau_\mathrm{acc}\f$ in seconds + */ + void setTauAcc(vqf_real_t tauAcc); + /** + * @brief Sets the time constant for the magnetometer update. + * + * For more details, see VQFParams.tauMag. + * + * @param tauMag time constant \f$\tau_\mathrm{mag}\f$ in seconds + */ + void setTauMag(vqf_real_t tauMag); #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Enables/disabled gyroscope bias estimation during motion. - */ - void setMotionBiasEstEnabled(bool enabled); + /** + * @brief Enables/disabled gyroscope bias estimation during motion. + */ + void setMotionBiasEstEnabled(bool enabled); #endif - /** - * @brief Enables/disables rest detection and bias estimation during rest. - */ - void setRestBiasEstEnabled(bool enabled); - /** - * @brief Enables/disables magnetic disturbance detection and rejection. - */ - void setMagDistRejectionEnabled(bool enabled); - /** - * @brief Sets the current thresholds for rest detection. - * - * For details about the parameters, see VQFParams.restThGyr and VQFParams.restThAcc. - */ - void setRestDetectionThresholds(vqf_real_t thGyr, vqf_real_t thAcc); - - /** - * @brief Returns the current parameters. - */ - const VQFParams& getParams() const; - /** - * @brief Returns the coefficients used by the algorithm. - */ - const VQFCoefficients& getCoeffs() const; - /** - * @brief Returns the current state. - */ - const VQFState& getState() const; - /** - * @brief Overwrites the current state. - * - * This method allows to set a completely arbitrary filter state and is intended for debugging purposes. In - * combination with #getState, individual elements of the state can be modified. - * - * @param state A VQFState struct containing the new state - */ - void setState(const VQFState& state); - /** - * @brief Resets the state to the default values at initialization. - * - * Resetting the state is equivalent to creating a new instance of this class. - */ - void resetState(); - - /** - * @brief Performs quaternion multiplication (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}_1 \otimes \mathbf{q}_2\f$). - */ - static void quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]); - /** - * @brief Calculates the quaternion conjugate (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}^*\f$). - */ - static void quatConj(const vqf_real_t q[4], vqf_real_t out[4]); - /** - * @brief Sets the output quaternion to the identity quaternion (\f$\mathbf{q}_\mathrm{out} = - * \begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\f$). - */ - static void quatSetToIdentity(vqf_real_t out[4]); - /** - * @brief Applies a heading rotation by the angle delta (in rad) to a quaternion. - * - * \f$\mathbf{q}_\mathrm{out} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & - * \sin\frac{\delta}{2}\end{bmatrix} \otimes \mathbf{q}\f$ - */ - static void quatApplyDelta(vqf_real_t q[4], vqf_real_t delta, vqf_real_t out[4]); - /** - * @brief Rotates a vector with a given quaternion. - * - * \f$\begin{bmatrix}0 & \mathbf{v}_\mathrm{out}\end{bmatrix} = - * \mathbf{q} \otimes \begin{bmatrix}0 & \mathbf{v}\end{bmatrix} \otimes \mathbf{q}^*\f$ - */ - static void quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]); - /** - * @brief Calculates the Euclidean norm of a vector. - * @param vec pointer to an array of N elements - * @param N number of elements - */ - static vqf_real_t norm(const vqf_real_t vec[], size_t N); - /** - * @brief Normalizes a vector in-place. - * @param vec pointer to an array of N elements that will be normalized - * @param N number of elements - */ - static void normalize(vqf_real_t vec[], size_t N); - /** - * @brief Clips a vector in-place. - * @param vec pointer to an array of N elements that will be clipped - * @param N number of elements - * @param min smallest allowed value - * @param max largest allowed value - */ - static void clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max); - /** - * @brief Calculates the gain for a first-order low-pass filter from the 1/e time constant. - * - * \f$k = 1 - \exp\left(-\frac{T_\mathrm{s}}{\tau}\right)\f$ - * - * The cutoff frequency of the resulting filter is \f$f_\mathrm{c} = \frac{1}{2\pi\tau}\f$. - * - * @param tau time constant \f$\tau\f$ in seconds - use -1 to disable update (\f$k=0\f$) or 0 to obtain - * unfiltered values (\f$k=1\f$) - * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds - * @return filter gain *k* - */ - static vqf_real_t gainFromTau(vqf_real_t tau, vqf_real_t Ts); - /** - * @brief Calculates coefficients for a second-order Butterworth low-pass filter. - * - * The filter is parametrized via the time constant of the dampened, non-oscillating part of step response and the - * resulting cutoff frequency is \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau}\f$. - * - * @param tau time constant \f$\tau\f$ in seconds - * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds - * @param outB output array for numerator coefficients - * @param outA output array for denominator coefficients (without \f$a_0=1\f$) - */ - static void filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2]); - /** - * @brief Calculates the initial filter state for a given steady-state value. - * @param x0 steady state value - * @param b numerator coefficients - * @param a denominator coefficients (without \f$a_0=1\f$) - * @param out output array for filter state - */ - static void filterInitialState(vqf_real_t x0, const double b[], const double a[], double out[2]); - /** - * @brief Adjusts the filter state when changing coefficients. - * - * This function assumes that the filter is currently in a steady state, i.e. the last input values and the last - * output values are all equal. Based on this, the filter state is adjusted to new filter coefficients so that the - * output does not jump. - * - * @param last_y last filter output values (array of size N) - * @param N number of values in vector-valued signal - * @param b_old previous numerator coefficients - * @param a_old previous denominator coefficients (without \f$a_0=1\f$) - * @param b_new new numerator coefficients - * @param a_new new denominator coefficients (without \f$a_0=1\f$) - * @param state filter state (array of size N*2, will be modified) - */ - static void filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[3], - const double a_old[2], const double b_new[3], - const double a_new[2], double state[]); - /** - * @brief Performs a filter step for a scalar value. - * @param x input value - * @param b numerator coefficients - * @param a denominator coefficients (without \f$a_0=1\f$) - * @param state filter state array (will be modified) - * @return filtered value - */ - static vqf_real_t filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]); - /** - * @brief Performs filter step for vector-valued signal with averaging-based initialization. - * - * During the first \f$\tau\f$ seconds, the filter output is the mean of the previous samples. At \f$t=\tau\f$, the - * initial conditions for the low-pass filter are calculated based on the current mean value and from then on, - * regular filtering with the rational transfer function described by the coefficients b and a is performed. - * - * @param x input values (array of size N) - * @param N number of values in vector-valued signal - * @param tau filter time constant \f$\tau\f$ in seconds (used for initialization) - * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds (used for initialization) - * @param b numerator coefficients - * @param a denominator coefficients (without \f$a_0=1\f$) - * @param state filter state (array of size N*2, will be modified) - * @param out output array for filtered values (size N) - */ - static void filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3], - const double a[2], double state[], vqf_real_t out[]); + /** + * @brief Enables/disables rest detection and bias estimation during rest. + */ + void setRestBiasEstEnabled(bool enabled); + /** + * @brief Enables/disables magnetic disturbance detection and rejection. + */ + void setMagDistRejectionEnabled(bool enabled); + /** + * @brief Sets the current thresholds for rest detection. + * + * For details about the parameters, see VQFParams.restThGyr and + * VQFParams.restThAcc. + */ + void setRestDetectionThresholds(vqf_real_t thGyr, vqf_real_t thAcc); + + /** + * @brief Returns the current parameters. + */ + const VQFParams& getParams() const; + /** + * @brief Returns the coefficients used by the algorithm. + */ + const VQFCoefficients& getCoeffs() const; + /** + * @brief Returns the current state. + */ + const VQFState& getState() const; + /** + * @brief Overwrites the current state. + * + * This method allows to set a completely arbitrary filter state and is intended for + * debugging purposes. In combination with #getState, individual elements of the + * state can be modified. + * + * @param state A VQFState struct containing the new state + */ + void setState(const VQFState& state); + /** + * @brief Resets the state to the default values at initialization. + * + * Resetting the state is equivalent to creating a new instance of this class. + */ + void resetState(); + + /** + * @brief Performs quaternion multiplication (\f$\mathbf{q}_\mathrm{out} = + * \mathbf{q}_1 \otimes \mathbf{q}_2\f$). + */ + static void + quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]); + /** + * @brief Calculates the quaternion conjugate (\f$\mathbf{q}_\mathrm{out} = + * \mathbf{q}^*\f$). + */ + static void quatConj(const vqf_real_t q[4], vqf_real_t out[4]); + /** + * @brief Sets the output quaternion to the identity quaternion + * (\f$\mathbf{q}_\mathrm{out} = \begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\f$). + */ + static void quatSetToIdentity(vqf_real_t out[4]); + /** + * @brief Applies a heading rotation by the angle delta (in rad) to a quaternion. + * + * \f$\mathbf{q}_\mathrm{out} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix} \otimes \mathbf{q}\f$ + */ + static void quatApplyDelta(vqf_real_t q[4], vqf_real_t delta, vqf_real_t out[4]); + /** + * @brief Rotates a vector with a given quaternion. + * + * \f$\begin{bmatrix}0 & \mathbf{v}_\mathrm{out}\end{bmatrix} = + * \mathbf{q} \otimes \begin{bmatrix}0 & \mathbf{v}\end{bmatrix} \otimes + * \mathbf{q}^*\f$ + */ + static void + quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]); + /** + * @brief Calculates the Euclidean norm of a vector. + * @param vec pointer to an array of N elements + * @param N number of elements + */ + static vqf_real_t norm(const vqf_real_t vec[], size_t N); + /** + * @brief Normalizes a vector in-place. + * @param vec pointer to an array of N elements that will be normalized + * @param N number of elements + */ + static void normalize(vqf_real_t vec[], size_t N); + /** + * @brief Clips a vector in-place. + * @param vec pointer to an array of N elements that will be clipped + * @param N number of elements + * @param min smallest allowed value + * @param max largest allowed value + */ + static void clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max); + /** + * @brief Calculates the gain for a first-order low-pass filter from the 1/e time + * constant. + * + * \f$k = 1 - \exp\left(-\frac{T_\mathrm{s}}{\tau}\right)\f$ + * + * The cutoff frequency of the resulting filter is \f$f_\mathrm{c} = + * \frac{1}{2\pi\tau}\f$. + * + * @param tau time constant \f$\tau\f$ in seconds - use -1 to disable update + * (\f$k=0\f$) or 0 to obtain unfiltered values (\f$k=1\f$) + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds + * @return filter gain *k* + */ + static vqf_real_t gainFromTau(vqf_real_t tau, vqf_real_t Ts); + /** + * @brief Calculates coefficients for a second-order Butterworth low-pass filter. + * + * The filter is parametrized via the time constant of the dampened, non-oscillating + * part of step response and the resulting cutoff frequency is \f$f_\mathrm{c} = + * \frac{\sqrt{2}}{2\pi\tau}\f$. + * + * @param tau time constant \f$\tau\f$ in seconds + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds + * @param outB output array for numerator coefficients + * @param outA output array for denominator coefficients (without \f$a_0=1\f$) + */ + static void + filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2]); + /** + * @brief Calculates the initial filter state for a given steady-state value. + * @param x0 steady state value + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param out output array for filter state + */ + static void filterInitialState( + vqf_real_t x0, + const double b[], + const double a[], + double out[2] + ); + /** + * @brief Adjusts the filter state when changing coefficients. + * + * This function assumes that the filter is currently in a steady state, i.e. the + * last input values and the last output values are all equal. Based on this, the + * filter state is adjusted to new filter coefficients so that the output does not + * jump. + * + * @param last_y last filter output values (array of size N) + * @param N number of values in vector-valued signal + * @param b_old previous numerator coefficients + * @param a_old previous denominator coefficients (without \f$a_0=1\f$) + * @param b_new new numerator coefficients + * @param a_new new denominator coefficients (without \f$a_0=1\f$) + * @param state filter state (array of size N*2, will be modified) + */ + static void filterAdaptStateForCoeffChange( + vqf_real_t last_y[], + size_t N, + const double b_old[3], + const double a_old[2], + const double b_new[3], + const double a_new[2], + double state[] + ); + /** + * @brief Performs a filter step for a scalar value. + * @param x input value + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param state filter state array (will be modified) + * @return filtered value + */ + static vqf_real_t + filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]); + /** + * @brief Performs filter step for vector-valued signal with averaging-based + * initialization. + * + * During the first \f$\tau\f$ seconds, the filter output is the mean of the + * previous samples. At \f$t=\tau\f$, the initial conditions for the low-pass filter + * are calculated based on the current mean value and from then on, regular + * filtering with the rational transfer function described by the coefficients b and + * a is performed. + * + * @param x input values (array of size N) + * @param N number of values in vector-valued signal + * @param tau filter time constant \f$\tau\f$ in seconds (used for initialization) + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds (used for initialization) + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param state filter state (array of size N*2, will be modified) + * @param out output array for filtered values (size N) + */ + static void filterVec( + const vqf_real_t x[], + size_t N, + vqf_real_t tau, + vqf_real_t Ts, + const double b[3], + const double a[2], + double state[], + vqf_real_t out[] + ); #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Sets a 3x3 matrix to a scaled version of the identity matrix. - * @param scale value of diagonal elements - * @param out output array of size 9 (3x3 matrix stored in row-major order) - */ - static void matrix3SetToScaledIdentity(vqf_real_t scale, vqf_real_t out[9]); - /** - * @brief Performs 3x3 matrix multiplication (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2\f$). - * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) - * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) - * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) - */ - static void matrix3Multiply(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); - /** - * @brief Performs 3x3 matrix multiplication after transposing the first matrix - * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1^T\mathbf{M}_2\f$). - * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) - * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) - * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) - */ - static void matrix3MultiplyTpsFirst(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); - /** - * @brief Performs 3x3 matrix multiplication after transposing the second matrix - * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2^T\f$). - * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) - * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) - * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) - */ - static void matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); - /** - * @brief Calculates the inverse of a 3x3 matrix (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}^{-1}\f$). - * @param in input 3x3 matrix \f$\mathbf{M}\f$ (stored in row-major order) - * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) - */ - static bool matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9]); + /** + * @brief Sets a 3x3 matrix to a scaled version of the identity matrix. + * @param scale value of diagonal elements + * @param out output array of size 9 (3x3 matrix stored in row-major order) + */ + static void matrix3SetToScaledIdentity(vqf_real_t scale, vqf_real_t out[9]); + /** + * @brief Performs 3x3 matrix multiplication (\f$\mathbf{M}_\mathrm{out} = + * \mathbf{M}_1\mathbf{M}_2\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major + * order) + */ + static void matrix3Multiply( + const vqf_real_t in1[9], + const vqf_real_t in2[9], + vqf_real_t out[9] + ); + /** + * @brief Performs 3x3 matrix multiplication after transposing the first matrix + * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1^T\mathbf{M}_2\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major + * order) + */ + static void matrix3MultiplyTpsFirst( + const vqf_real_t in1[9], + const vqf_real_t in2[9], + vqf_real_t out[9] + ); + /** + * @brief Performs 3x3 matrix multiplication after transposing the second matrix + * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2^T\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major + * order) + */ + static void matrix3MultiplyTpsSecond( + const vqf_real_t in1[9], + const vqf_real_t in2[9], + vqf_real_t out[9] + ); + /** + * @brief Calculates the inverse of a 3x3 matrix (\f$\mathbf{M}_\mathrm{out} = + * \mathbf{M}^{-1}\f$). + * @param in input 3x3 matrix \f$\mathbf{M}\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major + * order) + */ + static bool matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9]); #endif + void updateBiasForgettingTime(float biasForgettingTime); + protected: - /** - * @brief Calculates coefficients based on parameters and sampling rates. - */ - void setup(); - - /** - * @brief Contains the current parameters. - * - * See #getParams. To set parameters, pass them to the constructor. Part of the parameters can be changed with - * #setTauAcc, #setTauMag, #setMotionBiasEstEnabled, #setRestBiasEstEnabled, #setMagDistRejectionEnabled, and - * #setRestDetectionThresholds. - */ - VQFParams params; - /** - * @brief Contains the current state. - * - * See #getState, #getState and #resetState. - */ - VQFState state; - /** - * @brief Contains the current coefficients (calculated in #setup). - * - * See #getCoeffs. - */ - VQFCoefficients coeffs; + /** + * @brief Calculates coefficients based on parameters and sampling rates. + */ + void setup(); + + /** + * @brief Contains the current parameters. + * + * See #getParams. To set parameters, pass them to the constructor. Part of the + * parameters can be changed with #setTauAcc, #setTauMag, #setMotionBiasEstEnabled, + * #setRestBiasEstEnabled, #setMagDistRejectionEnabled, and + * #setRestDetectionThresholds. + */ + VQFParams params; + /** + * @brief Contains the current state. + * + * See #getState, #getState and #resetState. + */ + VQFState state; + /** + * @brief Contains the current coefficients (calculated in #setup). + * + * See #getCoeffs. + */ + VQFCoefficients coeffs; }; -#endif // VQF_HPP +#endif // VQF_HPP diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp index c608d488..4502acf7 100644 --- a/src/sensors/SensorFusion.cpp +++ b/src/sensors/SensorFusion.cpp @@ -211,5 +211,12 @@ void SensorFusion::calcLinearAcc( accout[1] = accin[1] - gravVec[1] * CONST_EARTH_GRAVITY; accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY; } + +#if SENSOR_USE_VQF +void SensorFusion::updateBiasForgettingTime(float biasForgettingTime) { + vqf.updateBiasForgettingTime(biasForgettingTime); +} +#endif + } // namespace Sensors } // namespace SlimeVR diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index c60b02f2..52060c24 100644 --- a/src/sensors/SensorFusion.h +++ b/src/sensors/SensorFusion.h @@ -106,6 +106,10 @@ class SensorFusion { sensor_real_t accout[3] ); +#if SENSOR_USE_VQF + void updateBiasForgettingTime(float biasForgettingTime); +#endif + protected: sensor_real_t gyrTs; sensor_real_t accTs; diff --git a/src/sensors/softfusion/TempGradientCalculator.cpp b/src/sensors/softfusion/TempGradientCalculator.cpp new file mode 100644 index 00000000..ceae340f --- /dev/null +++ b/src/sensors/softfusion/TempGradientCalculator.cpp @@ -0,0 +1,27 @@ +#include + +#include "TempGradientCalculator.h" + +TemperatureGradientCalculator::TemperatureGradientCalculator( + const std::function& callback +) + : callback{callback} {} + +void TemperatureGradientCalculator::feedSample(float sample, float timeStep) { + tempSum += sample * timeStep; +} + +void TemperatureGradientCalculator::tick() { + uint64_t nextCheck + = lastAverageSentMillis + static_cast(AveragingTimeSeconds * 1e3); + + if (millis() < nextCheck) { + return; + } + + lastAverageSentMillis = nextCheck; + float average = tempSum / AveragingTimeSeconds; + callback((average - lastTempAverage) / AveragingTimeSeconds); + lastTempAverage = average; + tempSum = 0; +} diff --git a/src/sensors/softfusion/TempGradientCalculator.h b/src/sensors/softfusion/TempGradientCalculator.h new file mode 100644 index 00000000..1aad1ed0 --- /dev/null +++ b/src/sensors/softfusion/TempGradientCalculator.h @@ -0,0 +1,46 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include + +#include +#include + +class TemperatureGradientCalculator { +public: + explicit TemperatureGradientCalculator( + const std::function& callback + ); + void feedSample(float sample, float timeStep); + void tick(); + +private: + std::function callback; + + static constexpr float AveragingTimeSeconds = 5.0f; + float tempSum = 0; + uint64_t lastAverageSentMillis = millis(); + float lastTempAverage = 0; +}; diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index a19ef715..531098d4 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -52,6 +52,8 @@ struct BMI270 { static constexpr float GyroSensitivity = 32.768f; static constexpr float AccelSensitivity = 2048.0f; + static constexpr float TemperatureZROChange = 6.667f; + struct MotionlessCalibrationData { bool valid; uint8_t x, y, z; diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index 48c4a404..0ca1c725 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -54,6 +54,8 @@ struct ICM42688 { static constexpr float TemperatureBias = 25.0f; static constexpr float TemperatureSensitivity = 2.07f; + static constexpr float TemperatureZROChange = 20.0f; + I2CImpl i2c; SlimeVR::Logging::Logger& logger; ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger) diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index b980fbdb..b7103ea2 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -49,6 +49,8 @@ struct ICM45Base { static constexpr float TemperatureBias = 25.0f; static constexpr float TemperatureSensitivity = 128.0f; + static constexpr float TemperatureZROChange = 20.0f; + static constexpr bool Uses32BitSensorData = true; I2CImpl i2c; diff --git a/src/sensors/softfusion/drivers/lsm6ds-common.h b/src/sensors/softfusion/drivers/lsm6ds-common.h index a8d0c4ee..24c61773 100644 --- a/src/sensors/softfusion/drivers/lsm6ds-common.h +++ b/src/sensors/softfusion/drivers/lsm6ds-common.h @@ -94,7 +94,7 @@ struct LSM6DSOutputHandler { processAccelSample(entry.xyz, AccTs); break; case 0x03: // Temperature - processTempSample(entry.x, TempTs); + processTempSample(entry.xyz[0], TempTs); break; } } diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index 4654fa5c..8d1d5b00 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -52,6 +52,8 @@ struct LSM6DS3TRC { static constexpr float TemperatureBias = 25.0f; static constexpr float TemperatureSensitivity = 256.0f; + static constexpr float TemperatureZROChange = 2.0f; + I2CImpl i2c; SlimeVR::Logging::Logger logger; LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger& logger) diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index db5cce71..0e8c6931 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -57,6 +57,8 @@ struct LSM6DSO : LSM6DSOutputHandler { static constexpr float TemperatureBias = 25.0f; static constexpr float TemperatureSensitivity = 256.0f; + static constexpr float TemperatureZROChange = 10.0; + using LSM6DSOutputHandler::i2c; struct Regs { diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h index cd796c59..174febab 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -57,6 +57,8 @@ struct LSM6DSR : LSM6DSOutputHandler { static constexpr float TemperatureBias = 25.0f; static constexpr float TemperatureSensitivity = 256.0f; + static constexpr float TemperatureZROChange = 20.0f; + using LSM6DSOutputHandler::i2c; struct Regs { diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index 6f1a16c3..8fb112f2 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -57,6 +57,8 @@ struct LSM6DSV : LSM6DSOutputHandler { static constexpr float TemperatureBias = 25.0f; static constexpr float TemperatureSensitivity = 256.0f; + static constexpr float TemperatureZROChange = 16.667f; + using LSM6DSOutputHandler::i2c; struct Regs { diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index 910d85a9..e5d88310 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -65,6 +65,8 @@ struct MPU6050 { static constexpr float GyroSensitivity = 32.8f; static constexpr float AccelSensitivity = 4096.0f; + static constexpr float TemperatureZROChange = 1.6f; + I2CImpl i2c; SlimeVR::Logging::Logger& logger; MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger& logger) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index ae4974bd..184b7ab2 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -30,6 +30,7 @@ #include "../sensor.h" #include "GlobalVars.h" #include "motionprocessing/types.h" +#include "sensors/softfusion/TempGradientCalculator.h" namespace SlimeVR::Sensors { @@ -70,8 +71,7 @@ class SoftFusionSensor : public Sensor { } static constexpr float DirectTempReadFreq = 15; - static constexpr uint32_t DirectTempReadMicros - = static_cast(1e6 / DirectTempReadFreq); + static constexpr float DirectTempReadTs = 1.0f / DirectTempReadFreq; float lastReadTemperature = 0; uint32_t lastTempPollTime = micros(); @@ -109,6 +109,12 @@ class SoftFusionSensor : public Sensor { } } + TemperatureGradientCalculator tempGradientCalculator{[&](float gradient) { + m_fusion.updateBiasForgettingTime( + std::fabs(gradient) * imu::TemperatureZROChange + ); + }}; + void recalcFusion() { m_fusion = SensorFusionRestDetect( m_calibration.G_Ts, @@ -167,6 +173,7 @@ class SoftFusionSensor : public Sensor { * (1.0 / imu::TemperatureSensitivity); lastReadTemperature = scaledTemperature; + tempGradientCalculator.feedSample(lastReadTemperature, timeDelta); } } @@ -243,12 +250,18 @@ class SoftFusionSensor : public Sensor { if constexpr (DirectTempReadOnly) { uint32_t tempElapsed = now - lastTempPollTime; - if (tempElapsed >= DirectTempReadMicros) { - lastTempPollTime = now - (tempElapsed - DirectTempReadMicros); + if (tempElapsed >= DirectTempReadTs * 1e6) { + lastTempPollTime = now - (tempElapsed - DirectTempReadTs * 1e6); lastReadTemperature = m_sensor.getDirectTemp(); + tempGradientCalculator.feedSample( + lastReadTemperature, + DirectTempReadTs + ); } } + tempGradientCalculator.tick(); + constexpr uint32_t targetPollIntervalMicros = 6000; uint32_t elapsed = now - m_lastPollTime; if (elapsed >= targetPollIntervalMicros) { From a83bf6933db43e1263d6b13ac6b3d66b7764fb24 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sun, 5 Jan 2025 04:51:54 +0100 Subject: [PATCH 03/21] Per sensor VQF params --- lib/vqf/vqf.cpp | 35 ------------- lib/vqf/vqf.h | 56 +++++++++------------ src/sensors/SensorFusion.h | 29 ++++++----- src/sensors/SensorFusionRestDetect.h | 15 ++++++ src/sensors/softfusion/drivers/bmi270.h | 9 ++++ src/sensors/softfusion/drivers/icm42688.h | 8 +++ src/sensors/softfusion/drivers/icm45605.h | 8 +++ src/sensors/softfusion/drivers/icm45686.h | 9 ++++ src/sensors/softfusion/drivers/lsm6ds3trc.h | 10 ++++ src/sensors/softfusion/drivers/lsm6dso.h | 9 ++++ src/sensors/softfusion/drivers/lsm6dsr.h | 8 +++ src/sensors/softfusion/drivers/lsm6dsv.h | 9 ++++ src/sensors/softfusion/softfusionsensor.h | 3 +- 13 files changed, 128 insertions(+), 80 deletions(-) diff --git a/lib/vqf/vqf.cpp b/lib/vqf/vqf.cpp index fea0e7c1..c8eeadc1 100644 --- a/lib/vqf/vqf.cpp +++ b/lib/vqf/vqf.cpp @@ -18,41 +18,6 @@ inline vqf_real_t square(vqf_real_t x) { return x*x; } - -VQFParams::VQFParams() - : tauAcc(3.0) - , tauMag(9.0) -#ifndef VQF_NO_MOTION_BIAS_ESTIMATION - , motionBiasEstEnabled(true) -#endif - , restBiasEstEnabled(true) - , magDistRejectionEnabled(true) - , biasSigmaInit(0.5) - , biasForgettingTime(100.0) - , biasClip(2.0) -#ifndef VQF_NO_MOTION_BIAS_ESTIMATION - , biasSigmaMotion(0.1) - , biasVerticalForgettingFactor(0.0001) -#endif - , biasSigmaRest(0.03) - , restMinT(1.5) - , restFilterTau(0.5) - , restThGyr(2.0) - , restThAcc(0.5) - , magCurrentTau(0.05) - , magRefTau(20.0) - , magNormTh(0.1) - , magDipTh(10.0) - , magNewTime(20.0) - , magNewFirstTime(5.0) - , magNewMinGyr(20.0) - , magMinUndisturbedTime(0.5) - , magMaxRejectionTime(60.0) - , magRejectionFactor(2.0) -{ - -} - VQF::VQF(vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs) { coeffs.gyrTs = gyrTs; diff --git a/lib/vqf/vqf.h b/lib/vqf/vqf.h index 4f7f55f1..6f81b794 100644 --- a/lib/vqf/vqf.h +++ b/lib/vqf/vqf.h @@ -11,7 +11,6 @@ #include #define VQF_SINGLE_PRECISION -#define VQF_NO_MOTION_BIAS_ESTIMATION #define M_PI 3.14159265358979323846 #define M_SQRT2 1.41421356237309504880 @@ -43,11 +42,6 @@ typedef float vqf_real_t; * parameters influence bias estimation and magnetometer rejection. */ struct VQFParams { - /** - * @brief Constructor that initializes the struct with the default parameters. - */ - VQFParams(); - /** * @brief Time constant \f$\tau_\mathrm{acc}\f$ for accelerometer low-pass filtering * in seconds. @@ -62,7 +56,7 @@ struct VQFParams { * * Default value: 3.0 s */ - vqf_real_t tauAcc; + vqf_real_t tauAcc = 3.0; /** * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds. * @@ -76,7 +70,7 @@ struct VQFParams { * * Default value: 9.0 s */ - vqf_real_t tauMag; + vqf_real_t tauMag = 9.0; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION /** @@ -85,7 +79,7 @@ struct VQFParams { * If set to true (default), gyroscope bias is estimated based on the inclination * correction only, i.e. without using magnetometer measurements. */ - bool motionBiasEstEnabled; + bool motionBiasEstEnabled = true; #endif /** * @brief Enables rest detection and gyroscope bias estimation during rest phases. @@ -94,7 +88,7 @@ struct VQFParams { * rest, the gyroscope bias is estimated from the low-pass filtered gyroscope * readings. */ - bool restBiasEstEnabled; + bool restBiasEstEnabled = true; /** * @brief Enables magnetic disturbance detection and magnetic disturbance rejection. * @@ -104,7 +98,7 @@ struct VQFParams { * exceeds #magMaxRejectionTime, magnetometer-based updates are performed, but with * an increased time constant. */ - bool magDistRejectionEnabled; + bool magDistRejectionEnabled = true; /** * @brief Standard deviation of the initial bias estimation uncertainty (in degrees @@ -112,7 +106,7 @@ struct VQFParams { * * Default value: 0.5 °/s */ - vqf_real_t biasSigmaInit; + vqf_real_t biasSigmaInit = 0.5; /** * @brief Time in which the bias estimation uncertainty increases from 0 °/s to 0.1 * °/s (in seconds). @@ -121,7 +115,7 @@ struct VQFParams { * * Default value: 100.0 s */ - vqf_real_t biasForgettingTime; + vqf_real_t biasForgettingTime = 100.0; /** * @brief Maximum expected gyroscope bias (in degrees per second). * @@ -132,7 +126,7 @@ struct VQFParams { * * Default value: 2.0 °/s */ - vqf_real_t biasClip; + vqf_real_t biasClip = 2.0; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION /** * @brief Standard deviation of the converged bias estimation uncertainty during @@ -143,7 +137,7 @@ struct VQFParams { * * Default value: 0.1 °/s */ - vqf_real_t biasSigmaMotion; + vqf_real_t biasSigmaMotion = 0.1; /** * @brief Forgetting factor for unobservable bias in vertical direction during * motion. @@ -155,7 +149,7 @@ struct VQFParams { * * Default value: 0.0001 */ - vqf_real_t biasVerticalForgettingFactor; + vqf_real_t biasVerticalForgettingFactor = 0.0001; #endif /** * @brief Standard deviation of the converged bias estimation uncertainty during @@ -166,7 +160,7 @@ struct VQFParams { * * Default value: 0.03 ° */ - vqf_real_t biasSigmaRest; + vqf_real_t biasSigmaRest = 0.03; /** * @brief Time threshold for rest detection (in seconds). @@ -176,7 +170,7 @@ struct VQFParams { * * Default value: 1.5 s */ - vqf_real_t restMinT; + vqf_real_t restMinT = 1.5; /** * @brief Time constant for the low-pass filter used in rest detection (in seconds). * @@ -185,7 +179,7 @@ struct VQFParams { * * Default value: 0.5 s */ - vqf_real_t restFilterTau; + vqf_real_t restFilterTau = 0.5; /** * @brief Angular velocity threshold for rest detection (in °/s). * @@ -195,7 +189,7 @@ struct VQFParams { * * Default value: 2.0 °/s */ - vqf_real_t restThGyr; + vqf_real_t restThGyr = 2.0; /** * @brief Acceleration threshold for rest detection (in m/s²). * @@ -204,7 +198,7 @@ struct VQFParams { * * Default value: 0.5 m/s² */ - vqf_real_t restThAcc; + vqf_real_t restThAcc = 0.5; /** * @brief Time constant for current norm/dip value in magnetic disturbance detection @@ -217,7 +211,7 @@ struct VQFParams { * * Default value: 0.05 s */ - vqf_real_t magCurrentTau; + vqf_real_t magCurrentTau = 0.05; /** * @brief Time constant for the adjustment of the magnetic field reference (in * seconds). @@ -227,7 +221,7 @@ struct VQFParams { * * Default value: 20.0 s */ - vqf_real_t magRefTau; + vqf_real_t magRefTau = 20.0; /** * @brief Relative threshold for the magnetic field strength for magnetic * disturbance detection. @@ -236,14 +230,14 @@ struct VQFParams { * * Default value: 0.1 (10%) */ - vqf_real_t magNormTh; + vqf_real_t magNormTh = 0.1; /** * @brief Threshold for the magnetic field dip angle for magnetic disturbance * detection (in degrees). * * Default vaule: 10 ° */ - vqf_real_t magDipTh; + vqf_real_t magDipTh = 10.0; /** * @brief Duration after which to accept a different homogeneous magnetic field (in * seconds). @@ -255,7 +249,7 @@ struct VQFParams { * * Default value: 20.0 */ - vqf_real_t magNewTime; + vqf_real_t magNewTime = 20.0; /** * @brief Duration after which to accept a homogeneous magnetic field for the first * time (in seconds). @@ -265,7 +259,7 @@ struct VQFParams { * * Default value: 5.0 */ - vqf_real_t magNewFirstTime; + vqf_real_t magNewFirstTime = 5.0; /** * @brief Minimum angular velocity needed in order to count time for new magnetic * field acceptance (in °/s). @@ -275,14 +269,14 @@ struct VQFParams { * * Default value: 20.0 °/s */ - vqf_real_t magNewMinGyr; + vqf_real_t magNewMinGyr = 20.0; /** * @brief Minimum duration within thresholds after which to regard the field as * undisturbed again (in seconds). * * Default value: 0.5 s */ - vqf_real_t magMinUndisturbedTime; + vqf_real_t magMinUndisturbedTime = 0.5; /** * @brief Maximum duration of full magnetic disturbance rejection (in seconds). * @@ -294,7 +288,7 @@ struct VQFParams { * * Default value: 60.0 s */ - vqf_real_t magMaxRejectionTime; + vqf_real_t magMaxRejectionTime = 60.0; /** * @brief Factor by which to slow the heading correction during long disturbed * phases. @@ -309,7 +303,7 @@ struct VQFParams { * * Default value: 2.0 */ - vqf_real_t magRejectionFactor; + vqf_real_t magRejectionFactor = 2.0; }; /** diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index 52060c24..7e82164c 100644 --- a/src/sensors/SensorFusion.h +++ b/src/sensors/SensorFusion.h @@ -38,23 +38,18 @@ namespace SlimeVR { namespace Sensors { #if SENSOR_USE_VQF -struct SensorVQFParams : VQFParams { - SensorVQFParams() - : VQFParams() { -#ifndef VQF_NO_MOTION_BIAS_ESTIMATION - motionBiasEstEnabled = true; -#endif - tauAcc = 2.0f; - restMinT = 2.0f; - restThGyr = 0.6f; // 400 norm - restThAcc = 0.06f; // 100 norm - } +constexpr VQFParams DefaultVQFParams = VQFParams{ + .tauAcc = 2.0f, + .restMinT = 2.0f, + .restThGyr = 0.6f, + .restThAcc = 0.06f, }; #endif class SensorFusion { public: SensorFusion( + VQFParams vqfParams, sensor_real_t gyrTs, sensor_real_t accTs = -1.0, sensor_real_t magTs = -1.0 @@ -62,12 +57,13 @@ class SensorFusion { : gyrTs(gyrTs) , accTs((accTs < 0) ? gyrTs : accTs) , magTs((magTs < 0) ? gyrTs : magTs) + , vqfParams(vqfParams) #if SENSOR_USE_MAHONY #elif SENSOR_USE_MADGWICK #elif SENSOR_USE_BASICVQF , basicvqf(gyrTs, ((accTs < 0) ? gyrTs : accTs), ((magTs < 0) ? gyrTs : magTs)) #elif SENSOR_USE_VQF - , vqf(vqfParams, + , vqf(this->vqfParams, gyrTs, ((accTs < 0) ? gyrTs : accTs), ((magTs < 0) ? gyrTs : magTs)) @@ -75,6 +71,13 @@ class SensorFusion { { } + explicit SensorFusion( + sensor_real_t gyrTs, + sensor_real_t accTs = -1.0, + sensor_real_t magTs = -1.0 + ) + : SensorFusion(DefaultVQFParams, gyrTs, accTs, magTs) {} + void update6D( sensor_real_t Axyz[3], sensor_real_t Gxyz[3], @@ -122,7 +125,7 @@ class SensorFusion { #elif SENSOR_USE_BASICVQF BasicVQF basicvqf; #elif SENSOR_USE_VQF - SensorVQFParams vqfParams{}; + VQFParams vqfParams; VQF vqf; #endif diff --git a/src/sensors/SensorFusionRestDetect.h b/src/sensors/SensorFusionRestDetect.h index 95c39637..5cf62309 100644 --- a/src/sensors/SensorFusionRestDetect.h +++ b/src/sensors/SensorFusionRestDetect.h @@ -33,6 +33,21 @@ class SensorFusionRestDetect : public SensorFusion { { } +#if SENSOR_USE_VQF + SensorFusionRestDetect( + VQFParams vqfParams, + float gyrTs, + float accTs = -1.0, + float magTs = -1.0 + ) + : SensorFusion(vqfParams, gyrTs, accTs, magTs) +#if !SENSOR_FUSION_WITH_RESTDETECT + , restDetection(restDetectionParams, gyrTs, (accTs < 0) ? gyrTs : accTs) +#endif + { + } +#endif + bool getRestDetected(); #if !SENSOR_FUSION_WITH_RESTDETECT diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index 531098d4..542f6977 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -30,6 +30,7 @@ #include #include "bmi270fw.h" +#include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -54,6 +55,14 @@ struct BMI270 { static constexpr float TemperatureZROChange = 6.667f; + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 0.5f, + .biasClip = 1.0f, + .restThGyr = 0.5f, + .restThAcc = 0.196f, + }; + struct MotionlessCalibrationData { bool valid; uint8_t x, y, z; diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index 0ca1c725..ef62d26a 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -56,6 +56,14 @@ struct ICM42688 { static constexpr float TemperatureZROChange = 20.0f; + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 0.5f, + .biasClip = 1.0f, + .restThGyr = 0.5f, + .restThAcc = 0.196f, + }; + I2CImpl i2c; SlimeVR::Logging::Logger& logger; ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger) diff --git a/src/sensors/softfusion/drivers/icm45605.h b/src/sensors/softfusion/drivers/icm45605.h index 0c01d1d6..a6a922c2 100644 --- a/src/sensors/softfusion/drivers/icm45605.h +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -39,6 +39,14 @@ struct ICM45605 : public ICM45Base { static constexpr auto Name = "ICM-45605"; static constexpr auto Type = ImuID::ICM45605; + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 0.3f, + .biasClip = 0.6f, + .restThGyr = 0.3f, + .restThAcc = 0.0098f, + }; + ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger) : ICM45Base{i2c, logger} {} diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h index adeed49a..5a3dc9a3 100644 --- a/src/sensors/softfusion/drivers/icm45686.h +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -24,6 +24,7 @@ #pragma once #include "icm45base.h" +#include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -39,6 +40,14 @@ struct ICM45686 : public ICM45Base { static constexpr auto Name = "ICM-45686"; static constexpr auto Type = ImuID::ICM45686; + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 0.5f, + .biasClip = 1.0f, + .restThGyr = 0.5f, + .restThAcc = 0.196f, + }; + ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger) : ICM45Base{i2c, logger} {} diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index 8d1d5b00..5195a15e 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -27,6 +27,8 @@ #include #include +#include "vqf.h" + namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 8g @@ -54,6 +56,14 @@ struct LSM6DS3TRC { static constexpr float TemperatureZROChange = 2.0f; + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 3.0f, + .biasClip = 6.0f, + .restThGyr = 3.0f, + .restThAcc = 0.392f, + }; + I2CImpl i2c; SlimeVR::Logging::Logger logger; LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger& logger) diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index 0e8c6931..d568f104 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -28,6 +28,7 @@ #include #include "lsm6ds-common.h" +#include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -59,6 +60,14 @@ struct LSM6DSO : LSM6DSOutputHandler { static constexpr float TemperatureZROChange = 10.0; + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 1.0f, + .biasClip = 2.0f, + .restThGyr = 1.0f, + .restThAcc = 0.192f, + }; + using LSM6DSOutputHandler::i2c; struct Regs { diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h index 174febab..754ffa41 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -59,6 +59,14 @@ struct LSM6DSR : LSM6DSOutputHandler { static constexpr float TemperatureZROChange = 20.0f; + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 1.0f, + .biasClip = 2.0f, + .restThGyr = 1.0f, + .restThAcc = 0.192f, + }; + using LSM6DSOutputHandler::i2c; struct Regs { diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index 8fb112f2..8c084a6a 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -28,6 +28,7 @@ #include #include "lsm6ds-common.h" +#include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -59,6 +60,14 @@ struct LSM6DSV : LSM6DSOutputHandler { static constexpr float TemperatureZROChange = 16.667f; + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 1.0f, + .biasClip = 2.0f, + .restThGyr = 1.0f, + .restThAcc = 0.192f, + }; + using LSM6DSOutputHandler::i2c; struct Regs { diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 184b7ab2..de6bbab5 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -117,6 +117,7 @@ class SoftFusionSensor : public Sensor { void recalcFusion() { m_fusion = SensorFusionRestDetect( + imu::SensorVQFParams, m_calibration.G_Ts, m_calibration.A_Ts, m_calibration.M_Ts @@ -238,7 +239,7 @@ class SoftFusionSensor : public Sensor { uint8_t ) : Sensor(imu::Name, imu::Type, id, i2cAddress, rotation, sclPin, sdaPin) - , m_fusion(imu::GyrTs, imu::AccTs, imu::MagTs) + , m_fusion(imu::SensorVQFParams, imu::GyrTs, imu::AccTs, imu::MagTs) , m_sensor(I2CImpl(i2cAddress), m_Logger) {} ~SoftFusionSensor() {} From ae66451265cfc81d6a5beaea8e81e12f0795a2eb Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sun, 5 Jan 2025 06:46:48 +0100 Subject: [PATCH 04/21] Split out classic softfusion calibration --- src/configuration/SensorConfig.h | 23 + src/debug.h | 5 + src/sensors/SensorManager.cpp | 44 +- .../AccelBiasCalibrationStep.h | 154 ++++++ .../nonblockingcalibration/CalibrationStep.h | 68 +++ .../GyroBiasCalibrationStep.h | 196 ++++++++ .../MotionlessCalibrationStep.h | 99 ++++ .../NonBlockingCalibration.h | 342 +++++++++++++ .../NullCalibrationStep.h | 46 ++ .../SampleRateCalibrationStep.h | 95 ++++ .../softfusion/SoftfusionCalibration.h | 459 ++++++++++++++++++ src/sensors/softfusion/drivers/icm42688.h | 2 + src/sensors/softfusion/drivers/icm45605.h | 1 + src/sensors/softfusion/drivers/mpu6050.h | 10 + src/sensors/softfusion/softfusionsensor.h | 417 +++------------- 15 files changed, 1588 insertions(+), 373 deletions(-) create mode 100644 src/sensors/nonblockingcalibration/AccelBiasCalibrationStep.h create mode 100644 src/sensors/nonblockingcalibration/CalibrationStep.h create mode 100644 src/sensors/nonblockingcalibration/GyroBiasCalibrationStep.h create mode 100644 src/sensors/nonblockingcalibration/MotionlessCalibrationStep.h create mode 100644 src/sensors/nonblockingcalibration/NonBlockingCalibration.h create mode 100644 src/sensors/nonblockingcalibration/NullCalibrationStep.h create mode 100644 src/sensors/nonblockingcalibration/SampleRateCalibrationStep.h create mode 100644 src/sensors/softfusion/SoftfusionCalibration.h diff --git a/src/configuration/SensorConfig.h b/src/configuration/SensorConfig.h index 0e5977be..b111c5e2 100644 --- a/src/configuration/SensorConfig.h +++ b/src/configuration/SensorConfig.h @@ -78,6 +78,29 @@ struct SoftFusionSensorConfig { float T_Ts; }; +struct NonBlockingSensorConfig { + ImuID ImuType; + uint16_t MotionlessDataLen; + + bool sensorTimestepsCalibrated; + float A_Ts; + float G_Ts; + float M_Ts; + float T_Ts; + + bool motionlessCalibrated; + uint8_t MotionlessData[60]; + + uint8_t gyroPointsCalibrated; + float gyroMeasurementTemperature1; + float G_off1[3]; + float gyroMeasurementTemperature2; + float G_off2[3]; + + bool accelCalibrated[3]; + float A_off[3]; +}; + struct MPU6050SensorConfig { // accelerometer offsets and correction matrix float A_B[3]; diff --git a/src/debug.h b/src/debug.h index 79cc6ec5..227a78d0 100644 --- a/src/debug.h +++ b/src/debug.h @@ -100,4 +100,9 @@ #define FIRMWARE_VERSION "UNKNOWN" #endif +#define USE_NONBLOCKING_CALIBRATION false +#ifndef USE_NONBLOCKING_CALIBRATION +#define USE_NONBLOCKING_CALIBRATION true +#endif + #endif // SLIMEVR_DEBUG_H_ diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 9cc2a621..6ba19cde 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -21,14 +21,17 @@ THE SOFTWARE. */ -#include "SensorManager.h" +#include +#include "SensorManager.h" #include "bmi160sensor.h" #include "bno055sensor.h" #include "bno080sensor.h" +#include "debug.h" #include "icm20948sensor.h" #include "mpu6050sensor.h" #include "mpu9250sensor.h" +#include "sensors/softfusion/SoftfusionCalibration.h" #include "softfusion/drivers/bmi270.h" #include "softfusion/drivers/icm42688.h" #include "softfusion/drivers/icm45605.h" @@ -45,26 +48,39 @@ #include "driver/i2c.h" #endif +#if USE_NONBLOCKING_CALIBRATION +#else +#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator +#endif + namespace SlimeVR { namespace Sensors { -using SoftFusionLSM6DS3TRC - = SoftFusionSensor; -using SoftFusionICM42688 - = SoftFusionSensor; +using SoftFusionLSM6DS3TRC = SoftFusionSensor< + SoftFusion::Drivers::LSM6DS3TRC, + SoftFusion::I2CImpl, + SFCALIBRATOR>; +using SoftFusionICM42688 = SoftFusionSensor< + SoftFusion::Drivers::ICM42688, + SoftFusion::I2CImpl, + SFCALIBRATOR>; using SoftFusionBMI270 - = SoftFusionSensor; + = SoftFusionSensor; using SoftFusionLSM6DSV - = SoftFusionSensor; + = SoftFusionSensor; using SoftFusionLSM6DSO - = SoftFusionSensor; + = SoftFusionSensor; using SoftFusionLSM6DSR - = SoftFusionSensor; + = SoftFusionSensor; using SoftFusionMPU6050 - = SoftFusionSensor; -using SoftFusionICM45686 - = SoftFusionSensor; -using SoftFusionICM45605 - = SoftFusionSensor; + = SoftFusionSensor; +using SoftFusionICM45686 = SoftFusionSensor< + SoftFusion::Drivers::ICM45686, + SoftFusion::I2CImpl, + SFCALIBRATOR>; +using SoftFusionICM45605 = SoftFusionSensor< + SoftFusion::Drivers::ICM45605, + SoftFusion::I2CImpl, + SFCALIBRATOR>; // TODO Make it more generic in the future and move another place (abstract sensor // interface) diff --git a/src/sensors/nonblockingcalibration/AccelBiasCalibrationStep.h b/src/sensors/nonblockingcalibration/AccelBiasCalibrationStep.h new file mode 100644 index 00000000..031b261b --- /dev/null +++ b/src/sensors/nonblockingcalibration/AccelBiasCalibrationStep.h @@ -0,0 +1,154 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include +#include + +#include "../../consts.h" +#include "CalibrationStep.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class AccelBiasCalibrationStep : public CalibrationStep { + using CalibrationStep::sensorConfig; + using typename CalibrationStep::TickResult; + +public: + AccelBiasCalibrationStep( + SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig, + float accelScale + ) + : CalibrationStep{sensorConfig} + , accelScale{accelScale} {} + + void start() override final { + CalibrationStep::start(); + calibrationData = CalibrationData{}; + } + + TickResult tick() override final { + if (!calibrationData.value().axisDetermined) { + return TickResult::CONTINUE; + } + + if (calibrationData.value().largestAxis == -1) { + return TickResult::SKIP; + } + + if (calibrationData.value().sampleCount < accelBiasCalibrationSampleCount) { + return TickResult::CONTINUE; + } + + float accelAverage = calibrationData.value().accelSum + / static_cast(calibrationData.value().sampleCount); + + float expected = accelAverage > 0 ? CONST_EARTH_GRAVITY : -CONST_EARTH_GRAVITY; + + float accelOffset = accelAverage * accelScale - expected; + + sensorConfig.A_off[calibrationData.value().largestAxis] = accelOffset; + sensorConfig.accelCalibrated[calibrationData.value().largestAxis] = true; + + return TickResult::DONE; + } + + void cancel() override final { calibrationData.reset(); } + void processAccelSample(const SensorRawT accelSample[3]) override final { + if (calibrationData.value().axisDetermined) { + calibrationData.value().accelSum + += accelSample[calibrationData.value().largestAxis]; + + calibrationData.value().sampleCount++; + return; + } + + float absAxes[3]{ + std::abs(static_cast(accelSample[0])), + std::abs(static_cast(accelSample[1])), + std::abs(static_cast(accelSample[2])), + }; + + size_t largestAxis; + if (absAxes[0] > absAxes[1] && absAxes[0] > absAxes[2]) { + largestAxis = 0; + } else if (absAxes[1] > absAxes[2]) { + largestAxis = 1; + } else { + largestAxis = 2; + } + + if (sensorConfig.accelCalibrated[largestAxis]) { + calibrationData.value().axisDetermined = true; + calibrationData.value().largestAxis = -1; + return; + } + + float smallAxisPercentage1 + = absAxes[(largestAxis + 1) % 3] / absAxes[largestAxis]; + float smallAxisPercentage2 + = absAxes[(largestAxis + 2) % 3] / absAxes[largestAxis]; + + if (smallAxisPercentage1 > allowableVerticalAxisPercentage + || smallAxisPercentage2 > allowableVerticalAxisPercentage) { + calibrationData.value().axisDetermined = true; + calibrationData.value().largestAxis = -1; + return; + } + + calibrationData.value().axisDetermined = true; + calibrationData.value().largestAxis = largestAxis; + + calibrationData.value().currentAxis[0] = accelSample[0]; + calibrationData.value().currentAxis[1] = accelSample[1]; + calibrationData.value().currentAxis[2] = accelSample[2]; + } + + bool allAxesCalibrated() { + return sensorConfig.accelCalibrated[0] && sensorConfig.accelCalibrated[1] + && sensorConfig.accelCalibrated[2]; + } + bool anyAxesCalibrated() { + return sensorConfig.accelCalibrated[0] || sensorConfig.accelCalibrated[1] + || sensorConfig.accelCalibrated[2]; + } + +private: + static constexpr size_t accelBiasCalibrationSampleCount = 96; + static constexpr float allowableVerticalAxisPercentage = 0.05; + + struct CalibrationData { + bool axisDetermined = false; + int16_t currentAxis[3]{0, 0, 0}; + int32_t largestAxis = -1; + int32_t accelSum = 0; + size_t sampleCount = 0; + }; + + std::optional calibrationData; + float accelScale; +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/nonblockingcalibration/CalibrationStep.h b/src/sensors/nonblockingcalibration/CalibrationStep.h new file mode 100644 index 00000000..4555ce89 --- /dev/null +++ b/src/sensors/nonblockingcalibration/CalibrationStep.h @@ -0,0 +1,68 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include "../../configuration/Configuration.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class CalibrationStep { +public: + enum class TickResult { + CONTINUE, + SKIP, + DONE, + }; + + CalibrationStep(SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig) + : sensorConfig{sensorConfig} {} + + virtual ~CalibrationStep() = default; + + virtual void start() { restDetectionDelayStartMillis = millis(); } + + virtual TickResult tick() = 0; + virtual void cancel() = 0; + + virtual bool requiresRest() { return true; } + virtual void processAccelSample(const SensorRawT accelSample[3]) {} + virtual void processGyroSample(const SensorRawT accelSample[3]) {} + virtual void processTempSample(float tempSample) {} + + bool restDetectionDelayElapsed() { + return (millis() - restDetectionDelayStartMillis) + >= restDetectionDelaySeconds * 1e3; + } + +protected: + SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig; + + float restDetectionDelaySeconds = 5.0f; + +private: + uint32_t restDetectionDelayStartMillis; +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/nonblockingcalibration/GyroBiasCalibrationStep.h b/src/sensors/nonblockingcalibration/GyroBiasCalibrationStep.h new file mode 100644 index 00000000..fcf958bc --- /dev/null +++ b/src/sensors/nonblockingcalibration/GyroBiasCalibrationStep.h @@ -0,0 +1,196 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include +#include + +#include "CalibrationStep.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class GyroBiasCalibrationStep : public CalibrationStep { + using CalibrationStep::sensorConfig; + using typename CalibrationStep::TickResult; + +public: + GyroBiasCalibrationStep( + SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig + ) + : CalibrationStep{sensorConfig} {} + + void start() override final { + CalibrationStep::start(); + calibrationData = {millis()}; + } + + TickResult tick() override final { + if (millis() - calibrationData.value().startMillis + < gyroBiasCalibrationSeconds * 1e3) { + return TickResult::CONTINUE; + } + + float gyroOffsetX = calibrationData.value().gyroSums[0] + / static_cast(calibrationData.value().sampleCount); + float gyroOffsetY = calibrationData.value().gyroSums[1] + / static_cast(calibrationData.value().sampleCount); + float gyroOffsetZ = calibrationData.value().gyroSums[2] + / static_cast(calibrationData.value().sampleCount); + + if (sensorConfig.gyroPointsCalibrated == 0) { + sensorConfig.G_off1[0] = gyroOffsetX; + sensorConfig.G_off1[1] = gyroOffsetY; + sensorConfig.G_off1[2] = gyroOffsetZ; + sensorConfig.gyroPointsCalibrated = 1; + sensorConfig.gyroMeasurementTemperature1 + = calibrationData.value().temperature; + + return TickResult::DONE; + } + + if (sensorConfig.gyroPointsCalibrated == 1) { + if (calibrationData.value().temperature + > sensorConfig.gyroMeasurementTemperature1) { + sensorConfig.G_off2[0] = gyroOffsetX; + sensorConfig.G_off2[1] = gyroOffsetY; + sensorConfig.G_off2[2] = gyroOffsetZ; + sensorConfig.gyroMeasurementTemperature2 + = calibrationData.value().temperature; + } else { + sensorConfig.G_off2[0] = sensorConfig.G_off1[0]; + sensorConfig.G_off2[1] = sensorConfig.G_off1[1]; + sensorConfig.G_off2[2] = sensorConfig.G_off1[2]; + sensorConfig.gyroMeasurementTemperature2 + = sensorConfig.gyroMeasurementTemperature1; + + sensorConfig.G_off1[0] = gyroOffsetX; + sensorConfig.G_off1[1] = gyroOffsetY; + sensorConfig.G_off1[2] = gyroOffsetZ; + sensorConfig.gyroMeasurementTemperature1 + = calibrationData.value().temperature; + } + + sensorConfig.gyroPointsCalibrated = 2; + + return TickResult::DONE; + } + + if (calibrationData.value().temperature + < sensorConfig.gyroMeasurementTemperature1) { + sensorConfig.G_off1[0] = gyroOffsetX; + sensorConfig.G_off1[1] = gyroOffsetY; + sensorConfig.G_off1[2] = gyroOffsetZ; + sensorConfig.gyroMeasurementTemperature1 + = calibrationData.value().temperature; + } else { + sensorConfig.G_off2[0] = gyroOffsetX; + sensorConfig.G_off2[1] = gyroOffsetY; + sensorConfig.G_off2[2] = gyroOffsetZ; + sensorConfig.gyroMeasurementTemperature2 + = calibrationData.value().temperature; + } + + return TickResult::DONE; + } + void cancel() override final { calibrationData.reset(); } + + void processGyroSample(const SensorRawT gyroSample[3]) override final { + calibrationData.value().gyroSums[0] += gyroSample[0]; + calibrationData.value().gyroSums[1] += gyroSample[1]; + calibrationData.value().gyroSums[2] += gyroSample[2]; + calibrationData.value().sampleCount++; + } + + void processTempSample(float tempSample) override final { + calibrationData.value().temperature = tempSample; + + if (sensorConfig.gyroPointsCalibrated == 0) { + return; + } + + if (sensorConfig.gyroPointsCalibrated == 1) { + float tempDiff + = std::abs(sensorConfig.gyroMeasurementTemperature1 - tempSample); + + if (tempDiff < gyroBiasTemperatureDifference) { + calibrationData.value().gyroSums[0] = 0; + calibrationData.value().gyroSums[1] = 0; + calibrationData.value().gyroSums[2] = 0; + calibrationData.value().sampleCount = 0; + calibrationData.value().startMillis = millis(); + } + + return; + } + + if (tempSample >= sensorConfig.gyroMeasurementTemperature1 + && tempSample <= sensorConfig.gyroMeasurementTemperature2) { + calibrationData.value().gyroSums[0] = 0; + calibrationData.value().gyroSums[1] = 0; + calibrationData.value().gyroSums[2] = 0; + calibrationData.value().sampleCount = 0; + calibrationData.value().startMillis = millis(); + } + } + + void swapCalibrationIfNecessary() { + if (sensorConfig.gyroPointsCalibrated == 2 + && sensorConfig.gyroMeasurementTemperature1 + > sensorConfig.gyroMeasurementTemperature2) { + float tempG_off[3]{ + sensorConfig.G_off1[0], + sensorConfig.G_off1[1], + sensorConfig.G_off1[2], + }; + float tempGTemperature = sensorConfig.gyroMeasurementTemperature1; + + sensorConfig.G_off1[0] = sensorConfig.G_off2[0]; + sensorConfig.G_off1[1] = sensorConfig.G_off2[1]; + sensorConfig.G_off1[2] = sensorConfig.G_off2[2]; + sensorConfig.gyroMeasurementTemperature1 + = sensorConfig.gyroMeasurementTemperature2; + + sensorConfig.G_off2[0] = tempG_off[0]; + sensorConfig.G_off2[1] = tempG_off[1]; + sensorConfig.G_off2[2] = tempG_off[2]; + sensorConfig.gyroMeasurementTemperature2 = tempGTemperature; + } + } + +private: + static constexpr float gyroBiasCalibrationSeconds = 5; + static constexpr float gyroBiasTemperatureDifference = 5; + + struct CalibrationData { + uint64_t startMillis = 0; + float temperature = 0; + int32_t gyroSums[3]{0, 0, 0}; + size_t sampleCount = 0; + }; + + std::optional calibrationData; +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/nonblockingcalibration/MotionlessCalibrationStep.h b/src/sensors/nonblockingcalibration/MotionlessCalibrationStep.h new file mode 100644 index 00000000..1ac1fa92 --- /dev/null +++ b/src/sensors/nonblockingcalibration/MotionlessCalibrationStep.h @@ -0,0 +1,99 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include +#include + +#include "CalibrationStep.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class MotionlessCalibrationStep : public CalibrationStep { + using CalibrationStep::sensorConfig; + using typename CalibrationStep::TickResult; + +public: + MotionlessCalibrationStep( + SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig, + IMU& imu + ) + : CalibrationStep{sensorConfig} + , imu{imu} {} + + void start() override final { + CalibrationStep::start(); + calibrationData = {millis()}; + } + + TickResult tick() override final { + if constexpr (HasMotionlessCalib) { + if (millis() - calibrationData.value().startMillis + < motionlessCalibrationDelay * 1e3) { + return TickResult::CONTINUE; + } + + typename IMU::MotionlessCalibrationData motionlessCalibrationData; + if (!imu.motionlessCalibration(motionlessCalibrationData)) { + return TickResult::CONTINUE; + } + + std::memcpy( + sensorConfig.MotionlessData, + &motionlessCalibrationData, + sizeof(motionlessCalibrationData) + ); + sensorConfig.motionlessCalibrated = true; + + return TickResult::DONE; + } else { + return TickResult::DONE; + } + } + + void cancel() override final { calibrationData.reset(); } + +private: + static constexpr float motionlessCalibrationDelay = 5; + + static constexpr bool HasMotionlessCalib + = requires(IMU& i) { typename IMU::MotionlessCalibrationData; }; + static constexpr size_t MotionlessCalibDataSize() { + if constexpr (HasMotionlessCalib) { + return sizeof(typename IMU::MotionlessCalibrationData); + } else { + return 0; + } + } + + struct CalibrationData { + uint64_t startMillis = 0; + }; + + std::optional calibrationData; + IMU& imu; +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/nonblockingcalibration/NonBlockingCalibration.h b/src/sensors/nonblockingcalibration/NonBlockingCalibration.h new file mode 100644 index 00000000..8627b31d --- /dev/null +++ b/src/sensors/nonblockingcalibration/NonBlockingCalibration.h @@ -0,0 +1,342 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include + +#include +#include +#include + +#include "../../GlobalVars.h" +#include "../../configuration/Configuration.h" +#include "../../utils.h" +#include "../SensorFusionRestDetect.h" +#include "AccelBiasCalibrationStep.h" +#include "GyroBiasCalibrationStep.h" +#include "MotionlessCalibrationStep.h" +#include "NullCalibrationStep.h" +#include "SampleRateCalibrationStep.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class NonBlockingCalibrator { +public: + mutable SlimeVR::Logging::Logger logger{"DynamicCalibration"}; + + NonBlockingCalibrator( + SensorFusionRestDetect& fusion, + float accelScale, + IMU& imu, + uint8_t sensorId + ) + : fusion{fusion} + , imu{imu} + , accelScale{accelScale} + , sensorId{sensorId} {} + + void setup(Configuration::NonBlockingSensorConfig& baseCalibrationConfig) { + sensorConfig = baseCalibrationConfig; + startupMillis = millis(); + + gyroBiasCalibrationStep.swapCalibrationIfNecessary(); + + computeNextCalibrationStep(); + + printCalibration(); + } + + void tick() { + if (fusion.getRestDetected()) { + ledManager.on(); + } else { + ledManager.off(); + } + if (skippedAStep && !lastTickRest && fusion.getRestDetected()) { + computeNextCalibrationStep(); + skippedAStep = false; + } + + if (millis() - startupMillis < initialStartupDelaySeconds * 1e3) { + return; + } + + if (!fusion.getRestDetected() && currentStep->requiresRest()) { + if (isCalibrating) { + currentStep->cancel(); + isCalibrating = false; + } + + lastTickRest = fusion.getRestDetected(); + return; + } + + if (!isCalibrating) { + isCalibrating = true; + currentStep->start(); + } + + if (currentStep->requiresRest() && !currentStep->restDetectionDelayElapsed()) { + lastTickRest = fusion.getRestDetected(); + return; + } + + auto result = currentStep->tick(); + + switch (result) { + case CalibrationStep::TickResult::DONE: + stepCalibrationForward(); + break; + case CalibrationStep::TickResult::SKIP: + stepCalibrationForward(false); + break; + case CalibrationStep::TickResult::CONTINUE: + break; + } + + lastTickRest = fusion.getRestDetected(); + } + + void provideAccelSample(const SensorRawT accelSample[3]) { + if (isCalibrating) { + currentStep->processAccelSample(accelSample); + } + } + + void provideGyroSample(const SensorRawT gyroSample[3]) { + if (isCalibrating) { + currentStep->processGyroSample(gyroSample); + } + } + + void provideTempSample(float tempSample) { + if (isCalibrating) { + currentStep->processTempSample(tempSample); + } + } + +private: + enum class CalibrationStepEnum { + NONE, + SAMPLING_RATE, + MOTIONLESS, + GYRO_BIAS, + ACCEL_BIAS, + }; + + void computeNextCalibrationStep() { + if (!sensorConfig.sensorTimestepsCalibrated) { + nextCalibrationStep = CalibrationStepEnum::SAMPLING_RATE; + currentStep = &sampleRateCalibrationStep; + } else if (!sensorConfig.motionlessCalibrated && HasMotionlessCalib) { + nextCalibrationStep = CalibrationStepEnum::MOTIONLESS; + currentStep = &motionlessCalibrationStep; + } else if (sensorConfig.gyroPointsCalibrated == 0) { + nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS; + currentStep = &gyroBiasCalibrationStep; + // } else if (!accelBiasCalibrationStep.allAxesCalibrated()) { + // nextCalibrationStep = CalibrationStepEnum::ACCEL_BIAS; + // currentStep = &accelBiasCalibrationStep; + } else { + nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS; + currentStep = &gyroBiasCalibrationStep; + } + } + + void stepCalibrationForward(bool save = true) { + currentStep->cancel(); + switch (nextCalibrationStep) { + case CalibrationStepEnum::NONE: + return; + case CalibrationStepEnum::SAMPLING_RATE: + nextCalibrationStep = CalibrationStepEnum::MOTIONLESS; + currentStep = &motionlessCalibrationStep; + if (save) { + printCalibration(CalibrationPrintFlags::TIMESTEPS); + } + break; + case CalibrationStepEnum::MOTIONLESS: + nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS; + currentStep = &gyroBiasCalibrationStep; + if (save) { + printCalibration(CalibrationPrintFlags::MOTIONLESS); + } + break; + case CalibrationStepEnum::GYRO_BIAS: + if (sensorConfig.gyroPointsCalibrated == 1) { + // nextCalibrationStep = CalibrationStepEnum::ACCEL_BIAS; + // currentStep = &accelBiasCalibrationStep; + nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS; + currentStep = &gyroBiasCalibrationStep; + } + + if (save) { + printCalibration(CalibrationPrintFlags::GYRO_BIAS); + } + break; + case CalibrationStepEnum::ACCEL_BIAS: + nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS; + currentStep = &gyroBiasCalibrationStep; + + if (save) { + printCalibration(CalibrationPrintFlags::ACCEL_BIAS); + } + + if (!accelBiasCalibrationStep.allAxesCalibrated()) { + skippedAStep = true; + } + break; + } + + isCalibrating = false; + + if (save) { + saveCalibration(); + } + } + + void saveCalibration() { + SlimeVR::Configuration::SensorConfig calibration; + calibration.type = SlimeVR::Configuration::SensorConfigType::NONBLOCKING; + calibration.data.nonblocking = sensorConfig; + configuration.setSensor(sensorId, calibration); + configuration.save(); + + ledManager.blink(100); + } + + enum class CalibrationPrintFlags { + TIMESTEPS = 1, + MOTIONLESS = 2, + GYRO_BIAS = 4, + ACCEL_BIAS = 8, + }; + + static constexpr CalibrationPrintFlags PrintAllCalibration + = CalibrationPrintFlags::TIMESTEPS | CalibrationPrintFlags::MOTIONLESS + | CalibrationPrintFlags::GYRO_BIAS | CalibrationPrintFlags::ACCEL_BIAS; + + void printCalibration(CalibrationPrintFlags toPrint = PrintAllCalibration) { + if (any(toPrint & CalibrationPrintFlags::TIMESTEPS)) { + if (sensorConfig.sensorTimestepsCalibrated) { + logger.info( + "Calibrated timesteps: Accel %f, Gyro %f, Temperature %f", + sensorConfig.A_Ts, + sensorConfig.G_Ts, + sensorConfig.T_Ts + ); + } else { + logger.info("Sensor timesteps not calibrated"); + } + } + + if (HasMotionlessCalib && any(toPrint & CalibrationPrintFlags::MOTIONLESS)) { + if (sensorConfig.motionlessCalibrated) { + logger.info("Motionless calibration done"); + } else { + logger.info("Motionless calibration not done"); + } + } + + if (any(toPrint & CalibrationPrintFlags::GYRO_BIAS)) { + if (sensorConfig.gyroPointsCalibrated != 0) { + logger.info( + "Calibrated gyro bias at %fC: %f %f %f", + sensorConfig.gyroMeasurementTemperature1, + sensorConfig.G_off1[0], + sensorConfig.G_off1[1], + sensorConfig.G_off1[2] + ); + } else { + logger.info("Gyro bias not calibrated"); + } + + if (sensorConfig.gyroPointsCalibrated == 2) { + logger.info( + "Calibrated gyro bias at %fC: %f %f %f", + sensorConfig.gyroMeasurementTemperature2, + sensorConfig.G_off2[0], + sensorConfig.G_off2[1], + sensorConfig.G_off2[2] + ); + } + } + + if (any(toPrint & CalibrationPrintFlags::ACCEL_BIAS)) { + if (accelBiasCalibrationStep.allAxesCalibrated()) { + logger.info( + "Calibrated accel bias: %f %f %f", + sensorConfig.A_off[0], + sensorConfig.A_off[1], + sensorConfig.A_off[2] + ); + } else if (accelBiasCalibrationStep.anyAxesCalibrated()) { + logger.info( + "Partially calibrated accel bias: %f %f %f", + sensorConfig.A_off[0], + sensorConfig.A_off[1], + sensorConfig.A_off[2] + ); + } else { + logger.info("Accel bias not calibrated"); + } + } + } + + static constexpr bool HasMotionlessCalib + = requires(IMU& i) { typename IMU::MotionlessCalibrationData; }; + + SensorFusionRestDetect& fusion; + IMU& imu; + float accelScale; + uint8_t sensorId; + + CalibrationStepEnum nextCalibrationStep = CalibrationStepEnum::MOTIONLESS; + + static constexpr float initialStartupDelaySeconds = 5; + uint64_t startupMillis; + + SlimeVR::Configuration::NonBlockingSensorConfig sensorConfig; + + SampleRateCalibrationStep sampleRateCalibrationStep{sensorConfig}; + MotionlessCalibrationStep motionlessCalibrationStep{ + sensorConfig, + imu + }; + GyroBiasCalibrationStep gyroBiasCalibrationStep{sensorConfig}; + AccelBiasCalibrationStep accelBiasCalibrationStep{ + sensorConfig, + accelScale + }; + NullCalibrationStep nullCalibrationStep{sensorConfig}; + + CalibrationStep* currentStep = &nullCalibrationStep; + + bool isCalibrating = false; + bool skippedAStep = false; + bool lastTickRest = false; +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/nonblockingcalibration/NullCalibrationStep.h b/src/sensors/nonblockingcalibration/NullCalibrationStep.h new file mode 100644 index 00000000..11b52b23 --- /dev/null +++ b/src/sensors/nonblockingcalibration/NullCalibrationStep.h @@ -0,0 +1,46 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include "CalibrationStep.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class NullCalibrationStep : public CalibrationStep { + using CalibrationStep::sensorConfig; + using typename CalibrationStep::TickResult; + +public: + NullCalibrationStep(SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig) + : CalibrationStep{sensorConfig} {} + + void start() override final { CalibrationStep::start(); } + + TickResult tick() override final { return TickResult::CONTINUE; } + + void cancel() override final {} +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/nonblockingcalibration/SampleRateCalibrationStep.h b/src/sensors/nonblockingcalibration/SampleRateCalibrationStep.h new file mode 100644 index 00000000..85c7d034 --- /dev/null +++ b/src/sensors/nonblockingcalibration/SampleRateCalibrationStep.h @@ -0,0 +1,95 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include + +#include "CalibrationStep.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class SampleRateCalibrationStep : public CalibrationStep { + using CalibrationStep::sensorConfig; + using typename CalibrationStep::TickResult; + +public: + SampleRateCalibrationStep( + SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig + ) + : CalibrationStep{sensorConfig} {} + + void start() override final { + CalibrationStep::start(); + calibrationData = {millis()}; + } + + TickResult tick() override final { + float elapsedTime = (millis() - calibrationData.value().startMillis) / 1e3f; + + if (elapsedTime < samplingRateCalibrationSeconds) { + return TickResult::CONTINUE; + } + + float accelTimestep = elapsedTime / calibrationData.value().accelSamples; + float gyroTimestep = elapsedTime / calibrationData.value().gyroSamples; + float tempTimestep = elapsedTime / calibrationData.value().tempSamples; + + sensorConfig.A_Ts = accelTimestep; + sensorConfig.G_Ts = gyroTimestep; + sensorConfig.T_Ts = tempTimestep; + sensorConfig.sensorTimestepsCalibrated = true; + + return TickResult::DONE; + } + + void cancel() override final { calibrationData.reset(); } + bool requiresRest() override final { return false; } + + void processAccelSample(const SensorRawT accelSample[3]) override final { + calibrationData.value().accelSamples++; + } + + void processGyroSample(const SensorRawT GyroSample[3]) override final { + calibrationData.value().gyroSamples++; + } + + void processTempSample(float tempSample) override final { + calibrationData.value().tempSamples++; + } + +private: + static constexpr float samplingRateCalibrationSeconds = 5; + + struct CalibrationData { + uint64_t startMillis = 0; + uint64_t accelSamples = 0; + uint64_t gyroSamples = 0; + uint64_t tempSamples = 0; + }; + + std::optional calibrationData; +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/softfusion/SoftfusionCalibration.h b/src/sensors/softfusion/SoftfusionCalibration.h new file mode 100644 index 00000000..99e024ed --- /dev/null +++ b/src/sensors/softfusion/SoftfusionCalibration.h @@ -0,0 +1,459 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include +#include +#include + +#include "GlobalVars.h" +#include "configuration/SensorConfig.h" +#include "logging/Logger.h" +#include "motionprocessing/RestDetection.h" +#include "motionprocessing/types.h" + +namespace SlimeVR::Sensor { + +template < + typename IMU, + float TempTs, + double AScale, + double GScale, + typename RawSensorT, + typename RawVectorT> +class SoftfusionCalibrator { +public: + static constexpr bool HasMotionlessCalib + = requires(IMU& i) { typename IMU::MotionlessCalibrationData; }; + static constexpr size_t MotionlessCalibDataSize() { + if constexpr (HasMotionlessCalib) { + return sizeof(typename IMU::MotionlessCalibrationData); + } else { + return 0; + } + } + + SoftfusionCalibrator( + IMU& sensor, + uint8_t sensorId, + SlimeVR::Logging::Logger& m_Logger + ) + : m_sensor{sensor} + , sensorId{sensorId} + , m_Logger{m_Logger} {} + + const Configuration::SoftFusionSensorConfig& getCalibration() { + return m_calibration; + } + + void startCalibration( + int calibrationType, + const std::function& eatSamplesForSeconds, + const std::function(const uint32_t + )>& eatSamplesReturnLast, + const std::function& recalcFusion + ) { + if (calibrationType == 0) { + // ALL + calibrateSampleRate(eatSamplesForSeconds, recalcFusion); + if constexpr (HasMotionlessCalib) { + typename IMU::MotionlessCalibrationData calibData; + m_sensor.motionlessCalibration(calibData); + std::memcpy( + m_calibration.MotionlessData, + &calibData, + sizeof(calibData) + ); + } + // Gryoscope offset calibration can only happen after any motionless + // gyroscope calibration, otherwise we are calculating the offset based + // on an incorrect starting point + calibrateGyroOffset(eatSamplesReturnLast); + calibrateAccel(eatSamplesForSeconds); + } else if (calibrationType == 1) { + calibrateSampleRate(eatSamplesForSeconds, recalcFusion); + } else if (calibrationType == 2) { + calibrateGyroOffset(eatSamplesReturnLast); + } else if (calibrationType == 3) { + calibrateAccel(eatSamplesForSeconds); + } else if (calibrationType == 4) { + if constexpr (HasMotionlessCalib) { + typename IMU::MotionlessCalibrationData calibData; + m_sensor.motionlessCalibration(calibData); + std::memcpy( + m_calibration.MotionlessData, + &calibData, + sizeof(calibData) + ); + } else { + m_Logger.info("Sensor doesn't provide any custom motionless calibration" + ); + } + } + + saveCalibration(); + } + + bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration) { + return sensorCalibration.type + == SlimeVR::Configuration::SensorConfigType::SFUSION + && (sensorCalibration.data.sfusion.ImuType == IMU::Type) + && (sensorCalibration.data.sfusion.MotionlessDataLen + == MotionlessCalibDataSize()); + } + + void assignCalibration(const Configuration::SensorConfig& sensorCalibration) { + m_calibration = sensorCalibration.data.sfusion; + } + + void scaleAccelSample(sensor_real_t accelSample[3]) { + float tmp[3]; + for (uint8_t i = 0; i < 3; i++) { + tmp[i] = (accelSample[i] - m_calibration.A_B[i]); + } + + accelSample[0] + = (m_calibration.A_Ainv[0][0] * tmp[0] + m_calibration.A_Ainv[0][1] * tmp[1] + + m_calibration.A_Ainv[0][2] * tmp[2]) + * AScale; + accelSample[1] + = (m_calibration.A_Ainv[1][0] * tmp[0] + m_calibration.A_Ainv[1][1] * tmp[1] + + m_calibration.A_Ainv[1][2] * tmp[2]) + * AScale; + accelSample[2] + = (m_calibration.A_Ainv[2][0] * tmp[0] + m_calibration.A_Ainv[2][1] * tmp[1] + + m_calibration.A_Ainv[2][2] * tmp[2]) + * AScale; + } + + float getAccelTimestep() { return m_calibration.A_Ts; } + + void scaleGyroSample(sensor_real_t gyroSample[3]) { + gyroSample[0] = static_cast( + GScale * (gyroSample[0] - m_calibration.G_off[0]) + ); + gyroSample[1] = static_cast( + GScale * (gyroSample[1] - m_calibration.G_off[1]) + ); + gyroSample[2] = static_cast( + GScale * (gyroSample[2] - m_calibration.G_off[2]) + ); + } + + float getGyroTimestep() { return m_calibration.G_Ts; } + + float getTempTimestep() { return m_calibration.T_Ts; } + +private: + static constexpr auto GyroCalibDelaySeconds = 5; + static constexpr auto GyroCalibSeconds = 5; + + static constexpr auto SampleRateCalibDelaySeconds = 1; + static constexpr auto SampleRateCalibSeconds = 5; + + static constexpr auto AccelCalibDelaySeconds = 3; + static constexpr auto AccelCalibRestSeconds = 3; + + void saveCalibration() { + m_Logger.debug("Saving the calibration data"); + SlimeVR::Configuration::SensorConfig calibration{}; + calibration.type = SlimeVR::Configuration::SensorConfigType::SFUSION; + calibration.data.sfusion = m_calibration; + configuration.setSensor(sensorId, calibration); + configuration.save(); + } + + void calibrateGyroOffset( + const std::function(const uint32_t + )>& eatSamplesReturnLast + ) { + // Wait for sensor to calm down before calibration + m_Logger.info( + "Put down the device and wait for baseline gyro reading calibration (%d " + "seconds)", + GyroCalibDelaySeconds + ); + ledManager.on(); + auto lastSamples = eatSamplesReturnLast(GyroCalibDelaySeconds); + ledManager.off(); + + m_calibration.temperature + = std::get<2>(lastSamples) / IMU::TemperatureSensitivity + + IMU::TemperatureBias; + m_Logger.trace("Calibration temperature: %f", m_calibration.temperature); + + ledManager.pattern(100, 100, 3); + ledManager.on(); + m_Logger.info("Gyro calibration started..."); + + int32_t sumXYZ[3] = {0}; + const auto targetCalib = millis() + 1000 * GyroCalibSeconds; + uint32_t sampleCount = 0; + + while (millis() < targetCalib) { +#ifdef ESP8266 + ESP.wdtFeed(); +#endif + m_sensor.bulkRead( + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [&sumXYZ, + &sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + sumXYZ[0] += xyz[0]; + sumXYZ[1] += xyz[1]; + sumXYZ[2] += xyz[2]; + ++sampleCount; + }, + [](const int16_t rawTemp, const sensor_real_t timeDelta) {} + ); + } + + ledManager.off(); + m_calibration.G_off[0] + = static_cast(sumXYZ[0]) / static_cast(sampleCount); + m_calibration.G_off[1] + = static_cast(sumXYZ[1]) / static_cast(sampleCount); + m_calibration.G_off[2] + = static_cast(sumXYZ[2]) / static_cast(sampleCount); + + m_Logger.info( + "Gyro offset after %d samples: %f %f %f", + sampleCount, + UNPACK_VECTOR_ARRAY(m_calibration.G_off) + ); + } + + void calibrateAccel(const std::function& eatSamplesForSeconds + ) { + auto magneto = std::make_unique(); + m_Logger.info( + "Put the device into 6 unique orientations (all sides), leave it still and " + "do not hold/touch for %d seconds each", + AccelCalibRestSeconds + ); + ledManager.on(); + eatSamplesForSeconds(AccelCalibDelaySeconds); + ledManager.off(); + + RestDetectionParams calibrationRestDetectionParams; + calibrationRestDetectionParams.restMinTime = AccelCalibRestSeconds; + calibrationRestDetectionParams.restThAcc = 0.25f; + + RestDetection calibrationRestDetection( + calibrationRestDetectionParams, + IMU::GyrTs, + IMU::AccTs + ); + + constexpr uint16_t expectedPositions = 6; + constexpr uint16_t numSamplesPerPosition = 96; + + uint16_t numPositionsRecorded = 0; + uint16_t numCurrentPositionSamples = 0; + bool waitForMotion = true; + + std::vector accelCalibrationChunk; + accelCalibrationChunk.resize(numSamplesPerPosition * 3); + ledManager.pattern(100, 100, 6); + ledManager.on(); + m_Logger.info("Gathering accelerometer data..."); + m_Logger.info( + "Waiting for position %i, you can leave the device as is...", + numPositionsRecorded + 1 + ); + bool samplesGathered = false; + while (!samplesGathered) { +#ifdef ESP8266 + ESP.wdtFeed(); +#endif + m_sensor.bulkRead( + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + const sensor_real_t scaledData[] + = {static_cast( + AScale * static_cast(xyz[0]) + ), + static_cast( + AScale * static_cast(xyz[1]) + ), + static_cast( + AScale * static_cast(xyz[2]) + )}; + + calibrationRestDetection.updateAcc(IMU::AccTs, scaledData); + if (waitForMotion) { + if (!calibrationRestDetection.getRestDetected()) { + waitForMotion = false; + } + return; + } + + if (calibrationRestDetection.getRestDetected()) { + const uint16_t i = numCurrentPositionSamples * 3; + accelCalibrationChunk[i + 0] = xyz[0]; + accelCalibrationChunk[i + 1] = xyz[1]; + accelCalibrationChunk[i + 2] = xyz[2]; + numCurrentPositionSamples++; + + if (numCurrentPositionSamples >= numSamplesPerPosition) { + for (int i = 0; i < numSamplesPerPosition; i++) { + magneto->sample( + accelCalibrationChunk[i * 3 + 0], + accelCalibrationChunk[i * 3 + 1], + accelCalibrationChunk[i * 3 + 2] + ); + } + numPositionsRecorded++; + numCurrentPositionSamples = 0; + if (numPositionsRecorded < expectedPositions) { + ledManager.pattern(50, 50, 2); + ledManager.on(); + m_Logger.info( + "Recorded, waiting for position %i...", + numPositionsRecorded + 1 + ); + waitForMotion = true; + } + } + } else { + numCurrentPositionSamples = 0; + } + + if (numPositionsRecorded >= expectedPositions) { + samplesGathered = true; + } + }, + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [](const int16_t rawTemp, const sensor_real_t timeDelta) {} + ); + } + ledManager.off(); + m_Logger.debug("Calculating accelerometer calibration data..."); + accelCalibrationChunk.resize(0); + + float A_BAinv[4][3]; + magneto->current_calibration(A_BAinv); + + m_Logger.debug("Finished calculating accelerometer calibration"); + m_Logger.debug("Accelerometer calibration matrix:"); + m_Logger.debug("{"); + for (int i = 0; i < 3; i++) { + m_calibration.A_B[i] = A_BAinv[0][i]; + m_calibration.A_Ainv[0][i] = A_BAinv[1][i]; + m_calibration.A_Ainv[1][i] = A_BAinv[2][i]; + m_calibration.A_Ainv[2][i] = A_BAinv[3][i]; + m_Logger.debug( + " %f, %f, %f, %f", + A_BAinv[0][i], + A_BAinv[1][i], + A_BAinv[2][i], + A_BAinv[3][i] + ); + } + m_Logger.debug("}"); + } + + void calibrateSampleRate( + const std::function& eatSamplesForSeconds, + const std::function& recalcFusion + ) { + m_Logger.debug( + "Calibrating IMU sample rate in %d second(s)...", + SampleRateCalibDelaySeconds + ); + ledManager.on(); + eatSamplesForSeconds(SampleRateCalibDelaySeconds); + + uint32_t accelSamples = 0; + uint32_t gyroSamples = 0; + uint32_t tempSamples = 0; + + const auto calibTarget = millis() + 1000 * SampleRateCalibSeconds; + m_Logger.debug("Counting samples now..."); + uint32_t currentTime; + while ((currentTime = millis()) < calibTarget) { + m_sensor.bulkRead( + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + accelSamples++; + }, + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + gyroSamples++; + }, + [&](const int16_t rawTemp, const sensor_real_t timeDelta) { + tempSamples++; + } + ); + yield(); + } + + const auto millisFromStart = static_cast( + currentTime - (calibTarget - 1000 * SampleRateCalibSeconds) + ); + m_Logger.debug( + "Collected %d gyro, %d acc samples during %d ms", + gyroSamples, + accelSamples, + millisFromStart + ); + m_calibration.A_Ts + = millisFromStart / (static_cast(accelSamples) * 1000.0f); + m_calibration.G_Ts + = millisFromStart / (static_cast(gyroSamples) * 1000.0f); + m_calibration.T_Ts + = millisFromStart / (static_cast(tempSamples) * 1000.0f); + + m_Logger.debug( + "Gyro frequency %fHz, accel frequency: %fHz, temperature frequency: %fHz", + 1.0 / m_calibration.G_Ts, + 1.0 / m_calibration.A_Ts, + 1.0 / m_calibration.T_Ts + ); + ledManager.off(); + + // fusion needs to be recalculated + recalcFusion(); + } + + IMU& m_sensor; + SlimeVR::Logging::Logger& m_Logger; + uint8_t sensorId; + + SlimeVR::Configuration::SoftFusionSensorConfig m_calibration = { + // let's create here transparent calibration that doesn't affect input data + .ImuType = {IMU::Type}, + .MotionlessDataLen = {MotionlessCalibDataSize()}, + .A_B = {0.0, 0.0, 0.0}, + .A_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + .M_B = {0.0, 0.0, 0.0}, + .M_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + .G_off = {0.0, 0.0, 0.0}, + .temperature = 0.0, + .A_Ts = IMU::AccTs, + .G_Ts = IMU::GyrTs, + .M_Ts = IMU::MagTs, + .G_Sens = {1.0, 1.0, 1.0}, + .MotionlessData = {}, + .T_Ts = TempTs, + }; +}; + +} // namespace SlimeVR::Sensor diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index ef62d26a..496c29e8 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -27,6 +27,8 @@ #include #include +#include "vqf.h" + namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 8g diff --git a/src/sensors/softfusion/drivers/icm45605.h b/src/sensors/softfusion/drivers/icm45605.h index a6a922c2..32537e1a 100644 --- a/src/sensors/softfusion/drivers/icm45605.h +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -24,6 +24,7 @@ #pragma once #include "icm45base.h" +#include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index e5d88310..80ef8533 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -29,6 +29,8 @@ #include #include +#include "vqf.h" + namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 8g @@ -67,6 +69,14 @@ struct MPU6050 { static constexpr float TemperatureZROChange = 1.6f; + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 20.0f, + .biasClip = 40.0f, + .restThGyr = 20.0f, + .restThAcc = 0.784f, + }; + I2CImpl i2c; SlimeVR::Logging::Logger& logger; MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger& logger) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index de6bbab5..0037fb9e 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -24,6 +24,7 @@ #pragma once #include +#include #include #include "../SensorFusionRestDetect.h" @@ -34,10 +35,23 @@ namespace SlimeVR::Sensors { -template