github/SlimeVR-Tracker-ESP
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git synced 2026-04-05 17:51:57 +02:00
Code Issues 76 Packages Projects Releases Wiki Activity

Optimized VQFParams 2: Electric Boogaloo (#472)

Browse Source 
* Adjust IMU parameters

* Format

* Forgot a comment

* Use v2 optimized VQFParams GLOBALLY

* Always calibrate sensor timesteps on startup

* Formatting

---------

Co-authored-by: gorbit99 <gorbitgames@gmail.com>
This commit is contained in:
Meia
2025-09-29 19:55:48 +03:00
committed by GitHub
parent 1ddbcd4855
commit 96b6b7acec
13 changed files with 77 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
lib/vqf/vqf.h
View File

@@ -56,7 +56,7 @@ struct VQFParams {
*
* Default value: 3.0 s
*/
vqf_real_t tauAcc = 3.0;
vqf_real_t tauAcc = 4.337983;
/**
* @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds.
*
@@ -106,7 +106,7 @@ struct VQFParams {
*
* Default value: 0.5 °/s
*/
vqf_real_t biasSigmaInit = 0.5;
vqf_real_t biasSigmaInit = 3.219453;
/**
* @brief Time in which the bias estimation uncertainty increases from 0 °/s to 0.1
* °/s (in seconds).
@@ -115,7 +115,7 @@ struct VQFParams {
*
* Default value: 100.0 s
*/
vqf_real_t biasForgettingTime = 100.0;
vqf_real_t biasForgettingTime = 136.579346;
/**
* @brief Maximum expected gyroscope bias (in degrees per second).
*
@@ -126,7 +126,7 @@ struct VQFParams {
*
* Default value: 2.0 °/s
*/
vqf_real_t biasClip = 2.0;
vqf_real_t biasClip = 5.0;
#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
/**
* @brief Standard deviation of the converged bias estimation uncertainty during
@@ -137,7 +137,7 @@ struct VQFParams {
*
* Default value: 0.1 °/s
*/
vqf_real_t biasSigmaMotion = 0.1;
vqf_real_t biasSigmaMotion = 0.348501;
/**
* @brief Forgetting factor for unobservable bias in vertical direction during
* motion.
@@ -149,7 +149,7 @@ struct VQFParams {
*
* Default value: 0.0001
*/
vqf_real_t biasVerticalForgettingFactor = 0.0001;
vqf_real_t biasVerticalForgettingFactor = 0.007056;
#endif
/**
* @brief Standard deviation of the converged bias estimation uncertainty during
@@ -160,7 +160,7 @@ struct VQFParams {
*
* Default value: 0.03 °
*/
vqf_real_t biasSigmaRest = 0.03;
vqf_real_t biasSigmaRest = 0.063616;
/**
* @brief Time threshold for rest detection (in seconds).
@@ -170,7 +170,7 @@ struct VQFParams {
*
* Default value: 1.5 s
*/
vqf_real_t restMinT = 1.5;
vqf_real_t restMinT = 2.586910;
/**
* @brief Time constant for the low-pass filter used in rest detection (in seconds).
*
@@ -179,7 +179,7 @@ struct VQFParams {
*
* Default value: 0.5 s
*/
vqf_real_t restFilterTau = 0.5;
vqf_real_t restFilterTau = 1.114532;
/**
* @brief Angular velocity threshold for rest detection (in °/s).
*
@@ -189,7 +189,7 @@ struct VQFParams {
*
* Default value: 2.0 °/s
*/
vqf_real_t restThGyr = 2.0;
vqf_real_t restThGyr = 1.399189;
/**
* @brief Acceleration threshold for rest detection (in m/s²).
*
@@ -198,7 +198,7 @@ struct VQFParams {
*
* Default value: 0.5 m/s²
*/
vqf_real_t restThAcc = 0.5;
vqf_real_t restThAcc = 1.418598;
/**
* @brief Time constant for current norm/dip value in magnetic disturbance detection

15
src/sensors/softfusion/drivers/bmi160.h
View File

@@ -37,7 +37,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 4G
// and gyroscope range at 1000DPS
// Gyroscope ODR = 400Hz, accel ODR = 100Hz
// Gyroscope ODR = 200Hz, accel ODR = 100Hz
// Timestamps reading are not used
// Sensorhub to be implemented
@@ -47,7 +47,7 @@ struct BMI160 {
static constexpr auto Name = "BMI160";
static constexpr auto Type = SensorTypeID::BMI160;
static constexpr float GyrTs = 1.0 / 400.0;
static constexpr float GyrTs = 1.0 / 200.0;
static constexpr float AccTs = 1.0 / 100.0;
static constexpr float MagTs = 1.0 / 100;
@@ -58,14 +58,7 @@ struct BMI160 {
static constexpr float TemperatureZROChange
= 2.0f; // wow maybe BMI270 isn't that bad actually
static constexpr VQFParams SensorVQFParams{
// need to be refined, this IMU sucks
.motionBiasEstEnabled = true,
.biasSigmaInit = 0.5f,
.biasClip = 2.0f,
.restThGyr = 0.5f,
.restThAcc = 0.196f,
};
static constexpr VQFParams SensorVQFParams{};
RegisterInterface& m_RegisterInterface;
SlimeVR::Logging::Logger& m_Logger;
@@ -102,7 +95,7 @@ struct BMI160 {
struct GyrConf {
static constexpr uint8_t reg = 0x42;
static constexpr uint8_t value = 0b0101010; // 400Hz, filter mode normal
static constexpr uint8_t value = 0b0101001; // 200Hz, filter mode normal
};
struct GyrRange {

14
src/sensors/softfusion/drivers/bmi270.h
View File

@@ -38,7 +38,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 16g
// and gyroscope range at 1000dps
// Gyroscope ODR = 400Hz, accel ODR = 100Hz
// Gyroscope ODR = 200Hz, accel ODR = 100Hz
// Timestamps reading are not used
struct BMI270 {
@@ -46,7 +46,7 @@ struct BMI270 {
static constexpr auto Name = "BMI270";
static constexpr auto Type = SensorTypeID::BMI270;
static constexpr float GyrTs = 1.0 / 400.0;
static constexpr float GyrTs = 1.0 / 200.0;
static constexpr float AccTs = 1.0 / 100.0;
static constexpr float MagTs = 1.0 / 100;
@@ -56,13 +56,7 @@ 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,
};
static constexpr VQFParams SensorVQFParams{};
struct MotionlessCalibrationData {
bool valid;
@@ -138,7 +132,7 @@ struct BMI270 {
static constexpr uint8_t filterHighPerfMode = 1 << 7;
static constexpr uint8_t value
= rate400Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode;
= rate200Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode;
};
struct GyrRange {

14
src/sensors/softfusion/drivers/icm42688.h
View File

@@ -34,7 +34,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
// and gyroscope range at 1000dps
// Gyroscope ODR = 500Hz, accel ODR = 100Hz
// Gyroscope ODR = 200Hz, accel ODR = 100Hz
// Timestamps reading not used, as they're useless (constant predefined increment)
struct ICM42688 {
@@ -42,7 +42,7 @@ struct ICM42688 {
static constexpr auto Name = "ICM-42688";
static constexpr auto Type = SensorTypeID::ICM42688;
static constexpr float GyrTs = 1.0 / 500.0;
static constexpr float GyrTs = 1.0 / 200.0;
static constexpr float AccTs = 1.0 / 100.0;
static constexpr float TempTs = 1.0 / 500.0;
@@ -56,13 +56,7 @@ 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,
};
static constexpr VQFParams SensorVQFParams{};
RegisterInterface& m_RegisterInterface;
SlimeVR::Logging::Logger& m_Logger;
@@ -99,7 +93,7 @@ struct ICM42688 {
struct GyroConfig {
static constexpr uint8_t reg = 0x4f;
static constexpr uint8_t value
= (0b001 << 5) | 0b1111; // 1000dps, odr=500Hz
= (0b001 << 5) | 0b0111; // 1000dps, odr=200Hz
};
struct AccelConfig {
static constexpr uint8_t reg = 0x50;

10
src/sensors/softfusion/drivers/icm45605.h
View File

@@ -32,20 +32,14 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// and gyroscope range at 4000dps
// using high resolution mode
// Uses 32.768kHz clock
// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz
// Gyroscope ODR = 204.8Hz, accel ODR = 102.4Hz
// Timestamps reading not used, as they're useless (constant predefined increment)
struct ICM45605 : public ICM45Base {
static constexpr auto Name = "ICM-45605";
static constexpr auto Type = SensorTypeID::ICM45605;
static constexpr VQFParams SensorVQFParams{
.motionBiasEstEnabled = true,
.biasSigmaInit = 0.3f,
.biasClip = 0.6f,
.restThGyr = 0.3f,
.restThAcc = 0.0098f,
};
static constexpr VQFParams SensorVQFParams{};
ICM45605(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: ICM45Base{registerInterface, logger} {}

16
src/sensors/softfusion/drivers/icm45686.h
View File

@@ -32,26 +32,14 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// and gyroscope range at 4000dps
// using high resolution mode
// Uses 32.768kHz clock
// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz
// Gyroscope ODR = 204.8Hz, accel ODR = 102.4Hz
// Timestamps reading not used, as they're useless (constant predefined increment)
struct ICM45686 : public ICM45Base {
static constexpr auto Name = "ICM-45686";
static constexpr auto Type = SensorTypeID::ICM45686;
static constexpr VQFParams SensorVQFParams{
.tauAcc = 7.171490,
.biasSigmaInit = 0.337976,
.biasForgettingTime = 352.235500,
.biasClip = 5.0,
.biasSigmaMotion = 0.985346,
.biasVerticalForgettingFactor = 0.007959,
.biasSigmaRest = 0.028897,
.restMinT = 4.648680,
.restFilterTau = 1.900166,
.restThGyr = 2.620598,
.restThAcc = 2.142593,
};
static constexpr VQFParams SensorVQFParams{};
ICM45686(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
: ICM45Base{registerInterface, logger} {}

6
src/sensors/softfusion/drivers/icm45base.h
View File

@@ -34,13 +34,13 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// and gyroscope range at 4000dps
// using high resolution mode
// Uses 32.768kHz clock
// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz
// Gyroscope ODR = 204.8Hz, accel ODR = 102.4Hz
// Timestamps reading not used, as they're useless (constant predefined increment)
struct ICM45Base {
static constexpr uint8_t Address = 0x68;
static constexpr float GyrTs = 1.0 / 409.6;
static constexpr float GyrTs = 1.0 / 204.8;
static constexpr float AccTs = 1.0 / 102.4;
static constexpr float TempTs = 1.0 / 409.6;
@@ -71,7 +71,7 @@ struct ICM45Base {
struct GyroConfig {
static constexpr uint8_t reg = 0x1c;
static constexpr uint8_t value
= (0b0000 << 4) | 0b0111; // 4000dps, odr=409.6Hz
= (0b0000 << 4) | 0b1000; // 4000dps, odr=204.8Hz
};
struct AccelConfig {

32
src/sensors/softfusion/drivers/lsm6ds3trc.h
View File

@@ -33,21 +33,25 @@
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
// Driver uses acceleration range at 4g
// and gyroscope range at 1000dps
// Gyroscope ODR = 416Hz, accel ODR = 416Hz
// Gyroscope ODR = 208Hz, accel ODR = 104Hz
struct LSM6DS3TRC {
static constexpr uint8_t Address = 0x6a;
static constexpr auto Name = "LSM6DS3TR-C";
static constexpr auto Type = SensorTypeID::LSM6DS3TRC;
static constexpr float Freq = 416;
static constexpr float GyrFreq = 208.0f;
static constexpr float AccFreq = 104.0f;
static constexpr float MagFreq = 100.0f;
static constexpr float TempFreq
= 416.0f; // I guess it's just output at the FIFO ODR?
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 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 = 28.571428571f;
static constexpr float AccelSensitivity = 4098.360655738f;
@@ -57,13 +61,7 @@ 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,
};
static constexpr VQFParams SensorVQFParams{};
RegisterInterface& m_RegisterInterface;
SlimeVR::Logging::Logger m_Logger;
@@ -79,12 +77,12 @@ struct LSM6DS3TRC {
};
struct Ctrl1XL {
static constexpr uint8_t reg = 0x10;
static constexpr uint8_t value = (0b11 << 2) | (0b0110 << 4); // 8g, 416Hz
static constexpr uint8_t value = (0b10 << 2) | (0b0100 << 4); // 4g, 104Hz
};
struct Ctrl2G {
static constexpr uint8_t reg = 0x11;
static constexpr uint8_t value
= (0b10 << 2) | (0b0110 << 4); // 1000dps, 416Hz
= (0b10 << 2) | (0b0101 << 4); // 1000dps, 208Hz
};
struct Ctrl3C {
static constexpr uint8_t reg = 0x12;
@@ -104,7 +102,7 @@ struct LSM6DS3TRC {
struct FifoCtrl5 {
static constexpr uint8_t reg = 0x0a;
static constexpr uint8_t value
= 0b110 | (0b0111 << 3); // continuous mode, odr = 833Hz
= 0b110 | (0b0110 << 3); // continuous mode, odr = 416Hz
};
static constexpr uint8_t FifoStatus = 0x3a;

18
src/sensors/softfusion/drivers/lsm6dso.h
View File

@@ -33,7 +33,7 @@
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
// Driver uses acceleration range at 4g
// and gyroscope range at 1000dps
// Gyroscope ODR = 416Hz, accel ODR = 104Hz
@@ -42,7 +42,7 @@ struct LSM6DSO : LSM6DSOutputHandler {
static constexpr auto Name = "LSM6DSO";
static constexpr auto Type = SensorTypeID::LSM6DSO;
static constexpr float GyrFreq = 416;
static constexpr float GyrFreq = 208;
static constexpr float AccFreq = 104;
static constexpr float MagFreq = 120;
static constexpr float TempFreq = 52;
@@ -60,13 +60,7 @@ 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,
};
static constexpr VQFParams SensorVQFParams{};
struct Regs {
struct WhoAmI {
@@ -75,11 +69,11 @@ struct LSM6DSO : LSM6DSOutputHandler {
};
struct Ctrl1XL {
static constexpr uint8_t reg = 0x10;
static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS
static constexpr uint8_t value = (0b01001000); // XL at 104 Hz, 4g FS
};
struct Ctrl2GY {
static constexpr uint8_t reg = 0x11;
static constexpr uint8_t value = (0b01101000); // GY at 416 Hz, 1000dps FS
static constexpr uint8_t value = (0b01011000); // GY at 208 Hz, 1000dps FS
};
struct Ctrl3C {
static constexpr uint8_t reg = 0x12;
@@ -90,7 +84,7 @@ struct LSM6DSO : LSM6DSOutputHandler {
struct FifoCtrl3BDR {
static constexpr uint8_t reg = 0x09;
static constexpr uint8_t value
= (0b0110) | (0b0110 << 4); // gyro and accel batched at 417Hz
= 0b01010100; // Gyroscope batched into FIFO at 208Hz, Accel at 104Hz
};
struct FifoCtrl4Mode {
static constexpr uint8_t reg = 0x0a;

20
src/sensors/softfusion/drivers/lsm6dsr.h
View File

@@ -31,16 +31,16 @@
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
// Driver uses acceleration range at 4g
// and gyroscope range at 1000dps
// Gyroscope ODR = 416Hz, accel ODR = 104Hz
// Gyroscope ODR = 208Hz, accel ODR = 104Hz
struct LSM6DSR : LSM6DSOutputHandler {
static constexpr uint8_t Address = 0x6a;
static constexpr auto Name = "LSM6DSR";
static constexpr auto Type = SensorTypeID::LSM6DSR;
static constexpr float GyrFreq = 416;
static constexpr float GyrFreq = 208;
static constexpr float AccFreq = 104;
static constexpr float MagFreq = 120;
static constexpr float TempFreq = 52;
@@ -58,13 +58,7 @@ 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,
};
static constexpr VQFParams SensorVQFParams{};
struct Regs {
struct WhoAmI {
@@ -73,11 +67,11 @@ struct LSM6DSR : LSM6DSOutputHandler {
};
struct Ctrl1XL {
static constexpr uint8_t reg = 0x10;
static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS
static constexpr uint8_t value = (0b01001000); // XL at 104 Hz, 4g FS
};
struct Ctrl2GY {
static constexpr uint8_t reg = 0x11;
static constexpr uint8_t value = (0b01101000); // GY at 416 Hz, 1000dps FS
static constexpr uint8_t value = (0b01011000); // GY at 208 Hz, 1000dps FS
};
struct Ctrl3C {
static constexpr uint8_t reg = 0x12;
@@ -88,7 +82,7 @@ struct LSM6DSR : LSM6DSOutputHandler {
struct FifoCtrl3BDR {
static constexpr uint8_t reg = 0x09;
static constexpr uint8_t value
= (0b0110) | (0b0110 << 4); // gyro and accel batched at 417Hz
= 0b01010100; // Gyroscope batched into FIFO at 208Hz, Accel at 104Hz
};
struct FifoCtrl4Mode {
static constexpr uint8_t reg = 0x0a;

20
src/sensors/softfusion/drivers/lsm6dsv.h
View File

@@ -32,16 +32,16 @@
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
// Driver uses acceleration range at 4g
// and gyroscope range at 1000dps
// Gyroscope ODR = 480Hz, accel ODR = 120Hz
// Gyroscope ODR = 240Hz, accel ODR = 120Hz
struct LSM6DSV : LSM6DSOutputHandler {
static constexpr uint8_t Address = 0x6a;
static constexpr auto Name = "LSM6DSV";
static constexpr auto Type = SensorTypeID::LSM6DSV;
static constexpr float GyrFreq = 480;
static constexpr float GyrFreq = 240;
static constexpr float AccFreq = 120;
static constexpr float MagFreq = 120;
static constexpr float TempFreq = 60;
@@ -59,13 +59,7 @@ 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,
};
static constexpr VQFParams SensorVQFParams{};
struct Regs {
struct WhoAmI {
@@ -82,7 +76,7 @@ struct LSM6DSV : LSM6DSOutputHandler {
};
struct Ctrl2GODR {
static constexpr uint8_t reg = 0x11;
static constexpr uint8_t value = (0b0011000); // 480Hz, HAODR
static constexpr uint8_t value = (0b0010111); // 240Hz, HAODR
};
struct Ctrl3C {
static constexpr uint8_t reg = 0x12;
@@ -96,12 +90,12 @@ struct LSM6DSV : LSM6DSOutputHandler {
};
struct Ctrl8XLFS {
static constexpr uint8_t reg = 0x17;
static constexpr uint8_t value = (0b10); // 8g
static constexpr uint8_t value = (0b01); // 4g
};
struct FifoCtrl3BDR {
static constexpr uint8_t reg = 0x09;
static constexpr uint8_t value
= (0b1000) | (0b1000 << 4); // gyro and accel batched at 480Hz
= 0b01110110; // Gyroscope batched into FIFO at 240Hz, Accel at 120Hz
};
struct FifoCtrl4Mode {
static constexpr uint8_t reg = 0x0a;

8
src/sensors/softfusion/drivers/mpu6050.h
View File

@@ -70,13 +70,7 @@ 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,
};
static constexpr VQFParams SensorVQFParams{};
RegisterInterface& m_RegisterInterface;
SlimeVR::Logging::Logger& m_Logger;

22
src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h
View File

@@ -99,7 +99,10 @@ public:
gyroBiasCalibrationStep.swapCalibrationIfNecessary();
computeNextCalibrationStep();
currentStep = &sampleRateCalibrationStep;
currentStep->start();
nextCalibrationStep = CalibrationStepEnum::SAMPLING_RATE;
calculateZROChange();
printCalibration();
@@ -229,10 +232,7 @@ private:
};
void computeNextCalibrationStep() {
if (!calibration.sensorTimestepsCalibrated) {
nextCalibrationStep = CalibrationStepEnum::SAMPLING_RATE;
currentStep = &sampleRateCalibrationStep;
} else if (!calibration.motionlessCalibrated && Base::HasMotionlessCalib) {
if (!calibration.motionlessCalibrated && Base::HasMotionlessCalib) {
nextCalibrationStep = CalibrationStepEnum::MOTIONLESS;
currentStep = &motionlessCalibrationStep;
} else if (calibration.gyroPointsCalibrated == 0) {
@@ -323,12 +323,12 @@ private:
void printCalibration(CalibrationPrintFlags toPrint = PrintAllCalibration) {
if (any(toPrint & CalibrationPrintFlags::TIMESTEPS)) {
if (calibration.sensorTimestepsCalibrated) {
if (activeCalibration.sensorTimestepsCalibrated) {
logger.info(
"Calibrated timesteps: Accel %f, Gyro %f, Temperature %f",
calibration.A_Ts,
calibration.G_Ts,
calibration.T_Ts
activeCalibration.A_Ts,
activeCalibration.G_Ts,
activeCalibration.T_Ts
);
} else {
logger.info("Sensor timesteps not calibrated");
@@ -389,12 +389,12 @@ private:
}
}
CalibrationStepEnum nextCalibrationStep = CalibrationStepEnum::MOTIONLESS;
CalibrationStepEnum nextCalibrationStep = CalibrationStepEnum::SAMPLING_RATE;
static constexpr float initialStartupDelaySeconds = 5;
uint64_t startupMillis = millis();
SampleRateCalibrationStep<RawSensorT> sampleRateCalibrationStep{calibration};
SampleRateCalibrationStep<RawSensorT> sampleRateCalibrationStep{activeCalibration};
MotionlessCalibrationStep<IMU, RawSensorT> motionlessCalibrationStep{
calibration,
sensor