diff --git a/.github/workflows/actions.yml b/.github/workflows/actions.yml index 03f844834..c2737319c 100644 --- a/.github/workflows/actions.yml +++ b/.github/workflows/actions.yml @@ -15,7 +15,7 @@ jobs: - uses: actions/checkout@v4 - uses: jidicula/clang-format-action@v4.13.0 with: - clang-format-version: "17" + clang-format-version: "18" fallback-style: google # Disable clang-tidy for now # - name: Get clang-tidy diff --git a/lib/vqf/vqf.cpp b/lib/vqf/vqf.cpp index 3ecd9bdee..a5edd4aca 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; @@ -917,16 +882,8 @@ 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 +898,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; +} diff --git a/lib/vqf/vqf.h b/lib/vqf/vqf.h index cda5cc105..7db2df9d0 100644 --- a/lib/vqf/vqf.h +++ b/lib/vqf/vqf.h @@ -11,7 +11,7 @@ #include #define VQF_SINGLE_PRECISION -#define VQF_NO_MOTION_BIAS_ESTIMATION +// #define VQF_NO_MOTION_BIAS_ESTIMATION #define M_PI 3.14159265358979323846 #define M_SQRT2 1.41421356237309504880 @@ -41,11 +41,6 @@ typedef float vqf_real_t; */ 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. * @@ -57,7 +52,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. * @@ -70,7 +65,7 @@ struct VQFParams * * Default value: 9.0 s */ - vqf_real_t tauMag; + vqf_real_t tauMag = 9.0; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION /** @@ -79,7 +74,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. @@ -87,7 +82,7 @@ struct VQFParams * 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; + bool restBiasEstEnabled = true; /** * @brief Enables magnetic disturbance detection and magnetic disturbance rejection. * @@ -96,14 +91,14 @@ struct VQFParams * the disturbances 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 per second). * * 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). * @@ -111,7 +106,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). * @@ -121,7 +116,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 motion (in degrees per second). @@ -130,7 +125,7 @@ struct VQFParams * * Default value: 0.1 °/s */ - vqf_real_t biasSigmaMotion; + vqf_real_t biasSigmaMotion = 0.1125; /** * @brief Forgetting factor for unobservable bias in vertical direction during motion. * @@ -140,7 +135,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 rest (in degrees per second). @@ -149,7 +144,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). @@ -158,7 +153,7 @@ struct VQFParams * * Default value: 1.5 s */ - vqf_real_t restMinT; + vqf_real_t restMinT = 0.5; /** * @brief Time constant for the low-pass filter used in rest detection (in seconds). * @@ -167,7 +162,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). * @@ -176,7 +171,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²). * @@ -185,7 +180,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 (in seconds). @@ -196,7 +191,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). * @@ -204,7 +199,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. * @@ -212,13 +207,13 @@ 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). * @@ -228,7 +223,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). * @@ -237,7 +232,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). * @@ -245,13 +240,13 @@ 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). * @@ -262,7 +257,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. * @@ -274,7 +269,7 @@ struct VQFParams * * Default value: 2.0 */ - vqf_real_t magRejectionFactor; + vqf_real_t magRejectionFactor = 2.0; }; /** @@ -987,6 +982,8 @@ class VQF 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. diff --git a/platformio-tools.ini b/platformio-tools.ini index f77b312b5..01d1947ba 100644 --- a/platformio-tools.ini +++ b/platformio-tools.ini @@ -23,18 +23,22 @@ board = esp12e [env:BOARD_NODEMCU] platform = espressif8266 @ 4.2.1 board = esp12e +board_build.f_cpu = 160000000L [env:BOARD_WEMOSD1MINI] platform = espressif8266 @ 4.2.1 board = esp12e +board_build.f_cpu = 160000000L [env:BOARD_TTGO_TBASE] platform = espressif8266 @ 4.2.1 board = esp12e +board_build.f_cpu = 160000000L [env:BOARD_WEMOSWROOM02] platform = espressif8266 @ 4.2.1 board = esp12e +board_build.f_cpu = 160000000L [env:BOARD_WROOM32] platform = espressif32 @ 6.7.0 diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index c4ca47c1e..4d575cc37 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -160,6 +160,21 @@ void Configuration::setSensor(size_t sensorID, const SensorConfig& config) { m_Sensors[sensorID] = config; } +void Configuration::eraseSensors() { + m_Sensors.clear(); + + SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) { + char path[17]; + sprintf(path, DIR_CALIBRATIONS "/%s", f.name()); + + f.close(); + + LittleFS.remove(path); + }); + + save(); +} + void Configuration::loadSensors() { SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) { SensorConfig sensorConfig; @@ -377,6 +392,61 @@ void Configuration::print() { m_Logger.info(" magEnabled: %d", c.data.bno0XX.magEnabled); break; + + case SensorConfigType::NONBLOCKING: + if (c.data.nonblocking.sensorTimestepsCalibrated) { + m_Logger.info( + " Calibrated timesteps: Accel %f, Gyro %f, " + "Temperature %f", + c.data.nonblocking.A_Ts, + c.data.nonblocking.G_Ts, + c.data.nonblocking.T_Ts + ); + } else { + m_Logger.info(" Sensor timesteps not calibrated"); + } + + if (c.data.nonblocking.motionlessCalibrated) { + m_Logger.info(" Motionless calibration done"); + } else { + m_Logger.info(" Motionless calibration not done"); + } + + if (c.data.nonblocking.gyroPointsCalibrated != 0) { + m_Logger.info( + " Calibrated gyro bias at %fC: %f %f %f", + c.data.nonblocking.gyroMeasurementTemperature1, + c.data.nonblocking.G_off1[0], + c.data.nonblocking.G_off1[1], + c.data.nonblocking.G_off1[2] + ); + } else { + m_Logger.info(" Gyro bias not calibrated"); + } + + if (c.data.nonblocking.gyroPointsCalibrated == 2) { + m_Logger.info( + " Calibrated gyro bias at %fC: %f %f %f", + c.data.nonblocking.gyroMeasurementTemperature2, + c.data.nonblocking.G_off2[0], + c.data.nonblocking.G_off2[1], + c.data.nonblocking.G_off2[2] + ); + } + + if (c.data.nonblocking.accelCalibrated[0] + || c.data.nonblocking.accelCalibrated[1] + || c.data.nonblocking.accelCalibrated[2]) { + m_Logger.info( + " Calibrated accel bias: %f %f %f", + c.data.nonblocking.A_off[0], + c.data.nonblocking.A_off[1], + c.data.nonblocking.A_off[2] + ); + } else { + m_Logger.info(" Accel bias not calibrated"); + } + break; } } } diff --git a/src/configuration/Configuration.h b/src/configuration/Configuration.h index 00cd4c78b..f246b2234 100644 --- a/src/configuration/Configuration.h +++ b/src/configuration/Configuration.h @@ -46,6 +46,7 @@ class Configuration { size_t getSensorCount() const; SensorConfig getSensor(size_t sensorID) const; void setSensor(size_t sensorID, const SensorConfig& config); + void eraseSensors(); bool loadTemperatureCalibration( uint8_t sensorId, diff --git a/src/configuration/SensorConfig.cpp b/src/configuration/SensorConfig.cpp index 28af04d44..4d4eb3daf 100644 --- a/src/configuration/SensorConfig.cpp +++ b/src/configuration/SensorConfig.cpp @@ -39,6 +39,8 @@ const char* calibrationConfigTypeToString(SensorConfigType type) { return "ICM20948"; case SensorConfigType::SFUSION: return "SoftFusion (common)"; + case SensorConfigType::NONBLOCKING: + return "NonBlocking (common)"; case SensorConfigType::BNO0XX: return "BNO0XX"; default: diff --git a/src/configuration/SensorConfig.h b/src/configuration/SensorConfig.h index 24b9477c1..5847d1f8f 100644 --- a/src/configuration/SensorConfig.h +++ b/src/configuration/SensorConfig.h @@ -75,6 +75,29 @@ struct SoftFusionSensorConfig { uint8_t MotionlessData[60]; }; +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]; @@ -131,6 +154,7 @@ enum class SensorConfigType { MPU9250, ICM20948, SFUSION, + NONBLOCKING, BNO0XX }; @@ -142,6 +166,7 @@ struct SensorConfig { union { BMI160SensorConfig bmi160; SoftFusionSensorConfig sfusion; + NonBlockingSensorConfig nonblocking; MPU6050SensorConfig mpu6050; MPU9250SensorConfig mpu9250; ICM20948SensorConfig icm20948; diff --git a/src/consts.h b/src/consts.h index 04e57698d..720e41fbc 100644 --- a/src/consts.h +++ b/src/consts.h @@ -42,6 +42,9 @@ enum class ImuID { LSM6DSV, LSM6DSO, LSM6DSR, + LSM6DS3, + ICM45686, + ICM45605, Empty = 255 }; @@ -62,6 +65,9 @@ enum class ImuID { #define IMU_LSM6DSO SoftFusionLSM6DSO #define IMU_LSM6DSR SoftFusionLSM6DSR #define IMU_MPU6050_SF SoftFusionMPU6050 +#define IMU_LSM6DS3 SoftFusionLSM6DS3 +#define IMU_ICM45686 SoftFusionICM45686 +#define IMU_ICM45605 SoftFusionICM45605 #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware diff --git a/src/debug.h b/src/debug.h index 79cc6ec52..63ad12e40 100644 --- a/src/debug.h +++ b/src/debug.h @@ -38,7 +38,7 @@ false // monitor accel for (triple) tap events and send them. Uses more cpu, // disable if problems. Server does nothing with value so disabled atm #define SEND_ACCELERATION true // send linear acceleration to the server - +#define USE_NONBLOCKING_CALIBRATION true // Debug information #define LOG_LEVEL LOG_LEVEL_DEBUG diff --git a/src/network/wifihandler.cpp b/src/network/wifihandler.cpp index 2bf2984ea..4ab3c0512 100644 --- a/src/network/wifihandler.cpp +++ b/src/network/wifihandler.cpp @@ -151,9 +151,9 @@ void WiFiNetwork::upkeep() { case SLIME_WIFI_SAVED_ATTEMPT: // Couldn't connect with first set of // credentials #if ESP8266 - // Try again but with 11G but only if there are credentials, - // otherwise we just waste time before switching to hardcoded - // credentials. + // Try again but with 11G + // But only if there are credentials, otherwise we just waste time + // before switching to hardcoded credentials. if (WiFi.SSID().length() > 0) { #if USE_ATTENUATION WiFi.setOutputPower(20.0 - ATTENUATION_G); @@ -180,7 +180,7 @@ void WiFiNetwork::upkeep() { case SLIME_WIFI_SAVED_G_ATTEMPT: // Couldn't connect with first set of // credentials with PHY Mode G #if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) - // Try hardcoded credentials now + // Try hardcoded credentials now #if ESP8266 #if USE_ATTENUATION WiFi.setOutputPower(20.0 - ATTENUATION_N); @@ -201,8 +201,8 @@ void WiFiNetwork::upkeep() { case SLIME_WIFI_HARDCODE_ATTEMPT: // Couldn't connect with second set // of credentials #if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) && ESP8266 - // Try hardcoded credentials again, - // but with PHY Mode G + // Try hardcoded credentials again, but with PHY + // Mode G #if USE_ATTENUATION WiFi.setOutputPower(20.0 - ATTENUATION_G); #endif @@ -223,7 +223,7 @@ void WiFiNetwork::upkeep() { case SLIME_WIFI_SERVER_CRED_ATTEMPT: // Couldn't connect with // server-sent credentials. #if ESP8266 - // Try again silently but with 11G + // Try again silently but with 11G #if USE_ATTENUATION WiFi.setOutputPower(20.0 - ATTENUATION_G); #endif diff --git a/src/sensors/EmptySensor.h b/src/sensors/EmptySensor.h index 5da697990..398eb2637 100644 --- a/src/sensors/EmptySensor.h +++ b/src/sensors/EmptySensor.h @@ -34,10 +34,10 @@ class EmptySensor : public Sensor { : Sensor("EmptySensor", ImuID::Empty, id, 0, 0.0){}; ~EmptySensor(){}; - void motionSetup() override final{}; - void motionLoop() override final{}; - void sendData() override final{}; - void startCalibration(int calibrationType) override final{}; + void motionSetup() override final {}; + void motionLoop() override final {}; + void sendData() override final {}; + void startCalibration(int calibrationType) override final {}; SensorStatus getSensorState() override final { return SensorStatus::SENSOR_OFFLINE; }; diff --git a/src/sensors/ErroneousSensor.h b/src/sensors/ErroneousSensor.h index e20d0bd64..6cf4a64b0 100644 --- a/src/sensors/ErroneousSensor.h +++ b/src/sensors/ErroneousSensor.h @@ -36,9 +36,9 @@ class ErroneousSensor : public Sensor { ~ErroneousSensor(){}; void motionSetup() override; - void motionLoop() override final{}; - void sendData() override{}; - void startCalibration(int calibrationType) override final{}; + void motionLoop() override final {}; + void sendData() override {}; + void startCalibration(int calibrationType) override final {}; SensorStatus getSensorState() override final; private: diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp index c608d488f..c553e183e 100644 --- a/src/sensors/SensorFusion.cpp +++ b/src/sensors/SensorFusion.cpp @@ -211,5 +211,15 @@ 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); +} + +void SensorFusion::updateRestDetectionParams(float restThGyr, float restThAcc) { + vqf.setRestDetectionThresholds(restThGyr, restThAcc); +} +#endif } // namespace Sensors } // namespace SlimeVR diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index c60b02f2d..4fdf402b7 100644 --- a/src/sensors/SensorFusion.h +++ b/src/sensors/SensorFusion.h @@ -37,20 +37,20 @@ 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 - } -}; -#endif + +// Previous VQF params: +// motionBiasEstEnabled = true; +// tauAcc = 3.0f; +// biasSigmaInit = 1.0f; +// biasForgettingTime = 60.0f; +// biasClip = 2.0f; +// biasSigmaMotion = 0.1175f; +// biasVerticalForgettingFactor = 10 - 0.03f; +// biasSigmaRest = 0.007f; +// restMinT = 1.5f; +// restFilterTau = 0.5f; +// restThGyr = 1.0f; // 400 norm +// restThAcc = 0.196f; // 100 norm class SensorFusion { public: @@ -75,6 +75,23 @@ class SensorFusion { { } +#if SENSOR_USE_VQF + SensorFusion( + VQFParams vqfParams, + sensor_real_t gyrTs, + sensor_real_t accTs = -1.0, + sensor_real_t magTs = -1.0 + ) + : gyrTs(gyrTs) + , accTs((accTs < 0) ? gyrTs : accTs) + , magTs((magTs < 0) ? gyrTs : magTs) + , vqfParams(vqfParams) + , vqf(vqfParams, + gyrTs, + ((accTs < 0) ? gyrTs : accTs), + ((magTs < 0) ? gyrTs : magTs)) {} +#endif + void update6D( sensor_real_t Axyz[3], sensor_real_t Gxyz[3], @@ -106,6 +123,12 @@ class SensorFusion { sensor_real_t accout[3] ); +#if SENSOR_USE_VQF + void updateBiasForgettingTime(float biasForgettingTime); + + void updateRestDetectionParams(float restThGyr, float restThAcc); +#endif + protected: sensor_real_t gyrTs; sensor_real_t accTs; @@ -118,7 +141,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 95c396372..5cf62309a 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/SensorManager.cpp b/src/sensors/SensorManager.cpp index 09262d035..700c36640 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -32,6 +32,9 @@ #include "sensoraddresses.h" #include "softfusion/drivers/bmi270.h" #include "softfusion/drivers/icm42688.h" +#include "softfusion/drivers/icm45605.h" +#include "softfusion/drivers/icm45686.h" +#include "softfusion/drivers/lsm6ds3.h" #include "softfusion/drivers/lsm6ds3trc.h" #include "softfusion/drivers/lsm6dso.h" #include "softfusion/drivers/lsm6dsr.h" @@ -60,6 +63,12 @@ using SoftFusionLSM6DSR = SoftFusionSensor; using SoftFusionMPU6050 = SoftFusionSensor; +using SoftFusionLSM6DS3 + = SoftFusionSensor; +using SoftFusionICM45686 + = SoftFusionSensor; +using SoftFusionICM45605 + = SoftFusionSensor; // TODO Make it more generic in the future and move another place (abstract sensor // interface) @@ -97,13 +106,12 @@ void SensorManager::setup() { activeSDA = PIN_IMU_SDA; uint8_t sensorID = 0; - uint8_t activeSensorCount = 0; #define IMU_DESC_ENTRY(ImuType, ...) \ { \ auto sensor = buildSensor(sensorID, __VA_ARGS__); \ if (sensor->isWorking()) { \ m_Logger.info("Sensor %d configured", sensorID + 1); \ - activeSensorCount++; \ + m_ActiveSensorCount++; \ } \ m_Sensors.push_back(std::move(sensor)); \ sensorID++; \ @@ -112,11 +120,12 @@ void SensorManager::setup() { IMU_DESC_LIST; #undef IMU_DESC_ENTRY - m_Logger.info("%d sensor(s) configured", activeSensorCount); + m_Logger.info("%d sensor(s) configured", m_ActiveSensorCount); // Check and scan i2c if no sensors active - if (activeSensorCount == 0) { + if (m_ActiveSensorCount == 0) { m_Logger.error( - "Can't find I2C device on provided addresses, scanning for all I2C devices " + "Can't find I2C device on provided addresses, scanning for all I2C " + "devices " "and returning" ); I2CSCAN::scani2cports(); diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index 9a8980672..8e64c6106 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -53,10 +53,13 @@ class SensorManager { return ImuID::Unknown; } + uint8_t getActiveSensorCount() { return m_ActiveSensorCount; } + private: SlimeVR::Logging::Logger m_Logger; std::vector> m_Sensors; + uint8_t m_ActiveSensorCount = 0; template std::unique_ptr buildSensor( diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index 1b1cf11e9..a40c95dc4 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -145,14 +145,14 @@ class BMI160Sensor : public Sensor { int axisRemapParam ) : Sensor( - "BMI160Sensor", - ImuID::BMI160, - id, - Address + addrSuppl, - rotation, - sclPin, - sdaPin - ) + "BMI160Sensor", + ImuID::BMI160, + id, + Address + addrSuppl, + rotation, + sclPin, + sdaPin + ) , sfusion( BMI160_ODR_GYR_MICROS / 1e6f, BMI160_ODR_ACC_MICROS / 1e6f, diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 1e348bfd7..581c7029e 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -42,14 +42,14 @@ class BNO055Sensor : public Sensor { uint8_t ) : Sensor( - "BNO055Sensor", - ImuID::BNO055, - id, - Address + addrSuppl, - rotation, - sclPin, - sdaPin - ){}; + "BNO055Sensor", + ImuID::BNO055, + id, + Address + addrSuppl, + rotation, + sclPin, + sdaPin + ){}; ~BNO055Sensor(){}; void motionSetup() override final; void motionLoop() override final; diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 57f797b79..e224eb6eb 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -44,14 +44,14 @@ class BNO080Sensor : public Sensor { uint8_t intPin ) : Sensor( - "BNO080Sensor", - ImuID::BNO080, - id, - Address + addrSuppl, - rotation, - sclPin, - sdaPin - ) + "BNO080Sensor", + ImuID::BNO080, + id, + Address + addrSuppl, + rotation, + sclPin, + sdaPin + ) , m_IntPin(intPin){}; ~BNO080Sensor(){}; void motionSetup() override final; @@ -109,15 +109,15 @@ class BNO085Sensor : public BNO080Sensor { uint8_t intPin ) : BNO080Sensor( - "BNO085Sensor", - ImuID::BNO085, - id, - address, - rotation, - sclPin, - sdaPin, - intPin - ){}; + "BNO085Sensor", + ImuID::BNO085, + id, + address, + rotation, + sclPin, + sdaPin, + intPin + ){}; }; class BNO086Sensor : public BNO080Sensor { @@ -132,15 +132,15 @@ class BNO086Sensor : public BNO080Sensor { uint8_t intPin ) : BNO080Sensor( - "BNO086Sensor", - ImuID::BNO086, - id, - address, - rotation, - sclPin, - sdaPin, - intPin - ){}; + "BNO086Sensor", + ImuID::BNO086, + id, + address, + rotation, + sclPin, + sdaPin, + intPin + ){}; }; #endif diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index 5e938b346..7f9ed1ade 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -42,14 +42,14 @@ class ICM20948Sensor : public Sensor { uint8_t ) : Sensor( - "ICM20948Sensor", - ImuID::ICM20948, - id, - Address + addrSuppl, - rotation, - sclPin, - sdaPin - ) {} + "ICM20948Sensor", + ImuID::ICM20948, + id, + Address + addrSuppl, + rotation, + sclPin, + sdaPin + ) {} ~ICM20948Sensor() override = default; void motionSetup() override final; void postSetup() override { this->lastData = millis(); } diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index ad46a3999..31b0ed847 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -43,14 +43,14 @@ class MPU6050Sensor : public Sensor { uint8_t ) : Sensor( - "MPU6050Sensor", - ImuID::MPU6050, - id, - Address + addrSuppl, - rotation, - sclPin, - sdaPin - ){}; + "MPU6050Sensor", + ImuID::MPU6050, + id, + Address + addrSuppl, + rotation, + sclPin, + sdaPin + ){}; ~MPU6050Sensor(){}; void motionSetup() override final; void motionLoop() override final; diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index d6d0b727d..0ad5e6442 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -56,14 +56,14 @@ class MPU9250Sensor : public Sensor { uint8_t ) : Sensor( - "MPU9250Sensor", - ImuID::MPU9250, - id, - Address + addrSuppl, - rotation, - sclPin, - sdaPin - ) + "MPU9250Sensor", + ImuID::MPU9250, + id, + Address + addrSuppl, + rotation, + sclPin, + sdaPin + ) #if !MPU_USE_DMPMAG , sfusion(MPU9250_ODR_TS) #endif diff --git a/src/sensors/nonblockingcalibration/AccelBiasCalibrationStep.h b/src/sensors/nonblockingcalibration/AccelBiasCalibrationStep.h new file mode 100644 index 000000000..031b261b1 --- /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 000000000..4555ce89c --- /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 000000000..fcf958bcd --- /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 000000000..1ac1fa929 --- /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 000000000..8627b31df --- /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 000000000..11b52b23d --- /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 000000000..85c7d0344 --- /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/sensor.cpp b/src/sensors/sensor.cpp index db9573daa..cd819443f 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -128,6 +128,12 @@ const char* getIMUNameByType(ImuID imuType) { return "LSM6DSO"; case ImuID::LSM6DSR: return "LSM6DSR"; + case ImuID::LSM6DS3: + return "LSM6DS3"; + case ImuID::ICM45686: + return "ICM45686"; + case ImuID::ICM45605: + return "ICM45605"; case ImuID::Unknown: case ImuID::Empty: return "UNKNOWN"; diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 6b52177b4..e4df40844 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -72,19 +72,19 @@ class Sensor { } virtual ~Sensor(){}; - virtual void motionSetup(){}; - virtual void postSetup(){}; - virtual void motionLoop(){}; + virtual void motionSetup() {}; + virtual void postSetup() {}; + virtual void motionLoop() {}; virtual void sendData(); virtual void setAcceleration(Vector3 a); virtual void setFusedRotation(Quat r); - virtual void startCalibration(int calibrationType){}; + virtual void startCalibration(int calibrationType) {}; virtual SensorStatus getSensorState(); virtual void printTemperatureCalibrationState(); virtual void printDebugTemperatureCalibrationState(); virtual void resetTemperatureCalibrationState(); virtual void saveTemperatureCalibration(); - virtual void setFlag(uint16_t flagId, bool state){}; + virtual void setFlag(uint16_t flagId, bool state) {}; virtual uint16_t getSensorConfigData(); bool isWorking() { return working; }; bool getHadData() const { return hadData; }; diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index 7938e0ca8..6756d1d86 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -33,9 +33,9 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { -// Driver uses acceleration range at 16g +// Driver uses acceleration range at 4g // and gyroscope range at 1000dps -// Gyroscope ODR = 400Hz, accel ODR = 100Hz +// Gyroscope ODR = 400Hz, accel ODR = 200Hz // Timestamps reading are not used template @@ -45,12 +45,30 @@ struct BMI270 { static constexpr auto Type = ImuID::BMI270; static constexpr float GyrTs = 1.0 / 400.0; - static constexpr float AccTs = 1.0 / 100.0; + static constexpr float AccTs = 1.0 / 200.0; + static constexpr float TempTs = 1.0 / 15.0; static constexpr float MagTs = 1.0 / 100; static constexpr float GyroSensitivity = 32.768f; - static constexpr float AccelSensitivity = 2048.0f; + static constexpr float AccelSensitivity = 8192.0f; + + // Temperature stability constant - how many degrees of temperature for the bias to + // change by 0.01 Though I don't know if it should be 0.1 or 0.01, this is a guess + // and seems to work better than 0.1 + static constexpr float TemperatureZROChange = 6.667f; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 0.5f, + .biasClip = 1.0f, + .restThGyr = 0.5f, + .restThAcc = 0.196f, + }; struct MotionlessCalibrationData { bool valid; @@ -165,7 +183,7 @@ struct BMI270 { static constexpr uint8_t filterHighPerfMode = 1 << 7; static constexpr uint8_t value - = rate100Hz | DLPFModeAvg4 | filterHighPerfMode; + = rate200Hz | DLPFModeAvg4 | filterHighPerfMode; }; struct AccRange { @@ -176,7 +194,7 @@ struct BMI270 { static constexpr uint8_t range8G = 2; static constexpr uint8_t range16G = 3; - static constexpr uint8_t value = range16G; + static constexpr uint8_t value = range4G; }; struct FifoConfig0 { @@ -314,7 +332,7 @@ struct BMI270 { return true; } - void motionlessCalibration(MotionlessCalibrationData& gyroSensitivity) { + bool motionlessCalibration(MotionlessCalibrationData& gyroSensitivity) { // perfrom gyroscope motionless sensitivity calibration (CRT) // need to start from clean state according to spec restartAndInit(); @@ -364,6 +382,8 @@ struct BMI270 { } setNormalConfig(gyroSensitivity); + + return status == 0; } float getDirectTemp() const { @@ -386,8 +406,12 @@ struct BMI270 { return to_ret; } - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TemperatureCall&& processTemperatureSample + ) { const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); const auto bytes_to_read = std::min( @@ -401,6 +425,9 @@ struct BMI270 { if ((header & Fifo::ModeMask) == Fifo::SkipFrame && (i - bytes_to_read) >= 1) { getFromFifo(i, read_buffer); // skip 1 byte + logger.error( + "FIFO OVERRUN! This occuring during normal usage is an issue." + ); } else if ((header & Fifo::ModeMask) == Fifo::DataFrame) { const uint8_t required_length = (((header & Fifo::GyrDataBit) >> Fifo::GyrDataBit) @@ -441,4 +468,4 @@ struct BMI270 { } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index 16088994d..faa7741b4 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -29,7 +29,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 = 500Hz, accel ODR = 100Hz // Timestamps reading not used, as they're useless (constant predefined increment) @@ -42,11 +42,34 @@ 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 float AccelSensitivity = 8192.0f; + + static constexpr bool Uses32BitSensorData = true; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 2.07f; + + // Temperature stability constant - how many degrees of temperature for the bias to + // change by 0.01 Though I don't know if it should be 0.1 or 0.01, this is a guess + // and seems to work better than 0.1 + static constexpr float TemperatureZROChange = 20.0f; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 0.5f, + .biasClip = 1.0f, + .restThGyr = 0.5f, + .restThAcc = 0.196f, + }; I2CImpl i2c; SlimeVR::Logging::Logger& logger; @@ -78,8 +101,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) | (0b0 << 2) + | (0b0 << 4); // fifo accel en=1, gyro=1, temp=0, hires=1 }; struct GyroConfig { static constexpr uint8_t reg = 0x4f; @@ -88,7 +111,7 @@ struct ICM42688 { }; struct AccelConfig { static constexpr uint8_t reg = 0x50; - static constexpr uint8_t value = (0b001 << 5) | 0b1000; // 8g, odr = 100Hz + static constexpr uint8_t value = (0b010 << 5) | 0b0111; // 4g, odr = 200Hz }; struct PwrMgmt { static constexpr uint8_t reg = 0x4e; @@ -111,15 +134,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 +163,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, + TemperatureCall&& processTemperatureSample + ) { const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); std::array read_buffer; // max 8 readings @@ -161,13 +185,29 @@ 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); } + + 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 new file mode 100644 index 000000000..badfa67d1 --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -0,0 +1,70 @@ +/* + 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 "icm45base.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45605 : public ICM45Base { + static constexpr auto Name = "ICM-45605"; + static constexpr auto Type = ImuID::ICM45605; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + 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} {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x72; + static constexpr uint8_t value = 0xe5; + }; + }; + + bool initialize() { + ICM45Base::softResetIMU(); + return ICM45Base::initializeBase(); + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h new file mode 100644 index 000000000..e101964f9 --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -0,0 +1,84 @@ +/* + 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 "icm45base.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45686 : public ICM45Base { + static constexpr auto Name = "ICM-45686"; + static constexpr auto Type = ImuID::ICM45686; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 0.3f, + .biasClip = 0.6f, + .restThGyr = 0.3f, + .restThAcc = 0.0098f, + }; + + ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : ICM45Base{i2c, logger} {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x72; + static constexpr uint8_t value = 0xe9; + }; + + struct Pin9Config { + static constexpr uint8_t reg = 0x31; + static constexpr uint8_t value = 0b00000110; // pin 9 to clkin + }; + + struct RtcConfig { + static constexpr uint8_t reg = 0x26; + static constexpr uint8_t value = 0b00100011; // enable RTC + }; + }; + + using ICM45Base::i2c; + + bool initialize() { + ICM45Base::softResetIMU(); + i2c.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value); + i2c.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value); + return ICM45Base::initializeBase(); + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h new file mode 100644 index 000000000..4c4ef6b11 --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -0,0 +1,216 @@ +/* + 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 + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45Base { + static constexpr uint8_t Address = 0x68; + + static constexpr float GyrTs = 1.0 / 409.6; + static constexpr float AccTs = 1.0 / 204.8; + static constexpr float TempTs = 1.0 / 409.6; + + static constexpr float MagTs = 1.0 / 100; + + static constexpr float GyroSensitivity = 131.072f; + static constexpr float AccelSensitivity = 16384.0f; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 128.0f; + + static constexpr bool Uses32BitSensorData = true; + + // Temperature stability constant - how many degrees of temperature for the bias to + // change by 0.01 Though I don't know if it should be 0.1 or 0.01, this is a guess + // and seems to work better than 0.1 + static constexpr float TemperatureZROChange = 20.0f; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 0.3f, + .biasClip = 0.6f, + .restThGyr = 0.3f, + .restThAcc = 0.0098f, + }; + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + struct BaseRegs { + static constexpr uint8_t TempData = 0x0c; + + struct DeviceConfig { + static constexpr uint8_t reg = 0x7f; + static constexpr uint8_t valueSwReset = 0b11; + }; + + struct GyroConfig { + static constexpr uint8_t reg = 0x1c; + static constexpr uint8_t value + = (0b0000 << 4) | 0b0111; // 4000dps, odr=409.6Hz + }; + + struct AccelConfig { + static constexpr uint8_t reg = 0x1b; + static constexpr uint8_t value + = (0b000 << 4) | 0b1000; // 32g, odr = 204.8Hz + }; + + struct FifoConfig0 { + static constexpr uint8_t reg = 0x1d; + static constexpr uint8_t value + = (0b01 << 6) | (0b011111); // stream to FIFO mode, FIFO depth + // 8k bytes <-- this disables all APEX + // features, but we don't need them + }; + + struct FifoConfig3 { + static constexpr uint8_t reg = 0x21; + static constexpr uint8_t value = (0b1 << 0) | (0b1 << 1) | (0b1 << 2) + | (0b1 << 3); // enable FIFO, + // enable accel, + // enable gyro, + // enable hires mode + }; + + struct PwrMgmt0 { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value + = 0b11 | (0b11 << 2); // accel in low noise mode, gyro in low noise + }; + + static constexpr uint8_t FifoCount = 0x12; + static constexpr uint8_t FifoData = 0x14; + }; + +#pragma pack(push, 1) + struct FifoEntryAligned { + union { + struct { + int16_t accel[3]; + int16_t gyro[3]; + uint16_t temp; + uint16_t timestamp; + uint8_t lsb[3]; + } part; + uint8_t raw[19]; + }; + }; +#pragma pack(pop) + + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; + + void softResetIMU() { + i2c.writeReg(BaseRegs::DeviceConfig::reg, BaseRegs::DeviceConfig::valueSwReset); + delay(35); + } + + bool initializeBase() { + // perform initialization step + i2c.writeReg(BaseRegs::GyroConfig::reg, BaseRegs::GyroConfig::value); + i2c.writeReg(BaseRegs::AccelConfig::reg, BaseRegs::AccelConfig::value); + i2c.writeReg(BaseRegs::FifoConfig0::reg, BaseRegs::FifoConfig0::value); + i2c.writeReg(BaseRegs::FifoConfig3::reg, BaseRegs::FifoConfig3::value); + i2c.writeReg(BaseRegs::PwrMgmt0::reg, BaseRegs::PwrMgmt0::value); + delay(1); + + 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, + TemperatureCall&& processTemperatureSample + ) { + const auto fifo_packets = i2c.readReg16(BaseRegs::FifoCount); + const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); + + std::array read_buffer; // max 8 readings + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(fifo_bytes) + ) + / FullFifoEntrySize * FullFifoEntrySize; + i2c.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data()); + for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) { + FifoEntryAligned entry; + memcpy( + entry.raw, + &read_buffer[i + 0x1], + sizeof(FifoEntryAligned) + ); // skip fifo header + const int32_t gyroData[3]{ + static_cast(entry.part.gyro[0]) << 4 + | (entry.part.lsb[0] & 0xf), + static_cast(entry.part.gyro[1]) << 4 + | (entry.part.lsb[1] & 0xf), + static_cast(entry.part.gyro[2]) << 4 + | (entry.part.lsb[2] & 0xf), + }; + processGyroSample(gyroData, GyrTs); + + if (entry.part.accel[0] != -32768) { + const int32_t accelData[3]{ + static_cast(entry.part.accel[0]) << 4 + | (static_cast(entry.part.lsb[0]) & 0xf0 >> 4), + static_cast(entry.part.accel[1]) << 4 + | (static_cast(entry.part.lsb[1]) & 0xf0 >> 4), + static_cast(entry.part.accel[2]) << 4 + | (static_cast(entry.part.lsb[2]) & 0xf0 >> 4), + }; + processAccelSample(accelData, AccTs); + } + + if (entry.part.temp != 0x8000) { + processTemperatureSample(static_cast(entry.part.temp), TempTs); + } + } + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6ds-common.h b/src/sensors/softfusion/drivers/lsm6ds-common.h index 382103b9f..cd100f701 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,18 @@ struct LSM6DSOutputHandler { static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; - template + template < + typename AccelCall, + typename GyroCall, + typename TemperatureCall, + typename Regs> void bulkRead( AccelCall& processAccelSample, GyroCall& processGyroSample, + TemperatureCall& processTemperatureSample, float GyrTs, - float AccTs + float AccTs, + float TempTs ) { constexpr auto FIFO_SAMPLES_MASK = 0x3ff; constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800; @@ -99,9 +97,12 @@ struct LSM6DSOutputHandler { case 0x02: // Accel NC processAccelSample(entry.xyz, AccTs); break; + case 0x03: // Temperature + processTemperatureSample(entry.xyz[0], 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/lsm6ds3.h b/src/sensors/softfusion/drivers/lsm6ds3.h new file mode 100644 index 000000000..7fcc51ae6 --- /dev/null +++ b/src/sensors/softfusion/drivers/lsm6ds3.h @@ -0,0 +1,175 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & 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 + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 4g +// and gyroscope range at 1000dps +// Gyroscope ODR = 416Hz, accel ODR = 208Hz + +template +struct LSM6DS3 { + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DS3"; + static constexpr auto Type = ImuID::LSM6DS3; + + static constexpr float Freq = 416; + + 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 = 8196.72131148f; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + + // Temperature stability constant - how many degrees of temperature for the bias to + // change by 0.01 Though I don't know if it should be 0.1 or 0.01, this is a guess + // and seems to work better than 0.1 + static constexpr float TemperatureZROChange = 2.0f; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 10.0f, + .biasClip = 20.0f, + .restThGyr = 10.0f, + .restThAcc = 0.392f, + }; + + I2CImpl i2c; + SlimeVR::Logging::Logger logger; + LSM6DS3(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x69; + }; + static constexpr uint8_t OutTemp = 0x20; + struct Ctrl1XL { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b10 << 2) | (0b0101 << 4); // 4g, 208Hz + }; + struct Ctrl2G { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value + = (0b10 << 2) | (0b0110 << 4); // 1000dps, 416Hz + }; + struct Ctrl3C { + static constexpr uint8_t reg = 0x12; + static constexpr uint8_t valueSwReset = 1; + 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 + = 0b001 | (0b001 << 3); // accel no decimation, gyro no decimation + }; + struct FifoCtrl5 { + static constexpr uint8_t reg = 0x0a; + static constexpr uint8_t value + = 0b110 | (0b0111 << 3); // continuous mode, odr = 833Hz + }; + + static constexpr uint8_t FifoStatus = 0x3a; + static constexpr uint8_t FifoData = 0x3e; + }; + + bool initialize() { + // perform initialization step + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + delay(20); + 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::FifoCtrl3::reg, Regs::FifoCtrl3::value); + i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); + return true; + } + + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TemperatureCall&& processTemperatureSample + ) { + const auto read_result = i2c.readReg16(Regs::FifoStatus); + if (read_result & 0x4000) { // overrun! + // disable and re-enable fifo to clear it + logger.debug("Fifo overrun, resetting..."); + i2c.writeReg(Regs::FifoCtrl5::reg, 0); + i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); + return; + } + const auto unread_entries = read_result & 0x7ff; + constexpr auto single_measurement_words = 12; + constexpr auto single_measurement_bytes + = sizeof(uint16_t) * single_measurement_words; + + std::array + read_buffer; // max 10 packages of 12 16bit values of data form fifo + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(unread_entries) + ) + * sizeof(uint16_t) / single_measurement_bytes + * single_measurement_bytes; + + i2c.readBytes( + Regs::FifoData, + bytes_to_read, + reinterpret_cast(read_buffer.data()) + ); + for (uint16_t i = 0; i < bytes_to_read / sizeof(uint16_t); + i += single_measurement_words) { + processGyroSample(reinterpret_cast(&read_buffer[i]), GyrTs); + processAccelSample( + reinterpret_cast(&read_buffer[i + 3]), + AccTs + ); + processTemperatureSample(read_buffer[i + 9], AccTs); + } + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index 75f036e4a..1a0aabe75 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -29,9 +29,9 @@ 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 = 416Hz, accel ODR = 208Hz template struct LSM6DS3TRC { @@ -44,9 +44,30 @@ 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 AccelSensitivity = 8196.72131148f; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + + // Temperature stability constant - how many degrees of temperature for the bias to + // change by 0.01 Though I don't know if it should be 0.1 or 0.01, this is a guess + // and seems to work better than 0.1 + static constexpr float TemperatureZROChange = 2.0f; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 3.0f, + .biasClip = 6.0f, + .restThGyr = 3.0f, + .restThAcc = 0.392f, + }; I2CImpl i2c; SlimeVR::Logging::Logger logger; @@ -62,7 +83,7 @@ struct LSM6DS3TRC { 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 + static constexpr uint8_t value = (0b10 << 2) | (0b0101 << 4); // 4g, 208Hz }; struct Ctrl2G { static constexpr uint8_t reg = 0x11; @@ -75,6 +96,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 @@ -102,15 +127,12 @@ struct LSM6DS3TRC { 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, + TemperatureCall&& processTemperatureSample + ) { const auto read_result = i2c.readReg16(Regs::FifoStatus); if (read_result & 0x4000) { // overrun! // disable and re-enable fifo to clear it @@ -120,12 +142,12 @@ struct LSM6DS3TRC { return; } const auto unread_entries = read_result & 0x7ff; - constexpr auto single_measurement_words = 6; + constexpr auto single_measurement_words = 12; constexpr auto single_measurement_bytes = sizeof(uint16_t) * single_measurement_words; - std::array - read_buffer; // max 10 packages of 6 16bit values of data form fifo + std::array + read_buffer; // max 10 packages of 12 16bit values of data form fifo const auto bytes_to_read = std::min( static_cast(read_buffer.size()), static_cast(unread_entries) @@ -145,8 +167,9 @@ struct LSM6DS3TRC { reinterpret_cast(&read_buffer[i + 3]), AccTs ); + processTemperatureSample(read_buffer[i + 9], AccTs); } } }; -} // 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 7c26c85bc..9d3250eb3 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -31,9 +31,9 @@ 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 = 416Hz, accel ODR = 208Hz template struct LSM6DSO : LSM6DSOutputHandler { @@ -42,15 +42,37 @@ struct LSM6DSO : LSM6DSOutputHandler { static constexpr auto Type = ImuID::LSM6DSO; static constexpr float GyrFreq = 416; - static constexpr float AccFreq = 104; + static constexpr float AccFreq = 208; 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 AccelSensitivity = 1000 / 0.122f; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + + // Temperature stability constant - how many degrees of temperature for the bias to + // change by 0.01 Though I don't know if it should be 0.1 or 0.01, this is a guess + // and seems to work better than 0.1 + static constexpr float TemperatureZROChange = 10.0f; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 1.0f, + .biasClip = 2.0f, + .restThGyr = 1.0f, + .restThAcc = 0.192f, + }; using LSM6DSOutputHandler::i2c; @@ -62,7 +84,7 @@ struct LSM6DSO : LSM6DSOutputHandler { 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 + static constexpr uint8_t value = (0b01011000); // XL at 208 Hz, 4g FS }; struct Ctrl2GY { static constexpr uint8_t reg = 0x11; @@ -76,12 +98,13 @@ 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 + static constexpr uint8_t value = (0b01100101 + ); // gyro and accel batched at 416Hz and 208hz respectively }; 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 +126,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, + TemperatureCall&& processTemperatureSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTemperatureSample, + 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 248679807..8c3d21627 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -31,7 +31,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 @@ -44,13 +44,35 @@ 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 AccelSensitivity = 1000 / 0.122f; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + + // Temperature stability constant - how many degrees of temperature for the bias to + // change by 0.01 Though I don't know if it should be 0.1 or 0.01, this is a guess + // and seems to work better than 0.1 + static constexpr float TemperatureZROChange = 20.0f; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 1.0f, + .biasClip = 2.0f, + .restThGyr = 1.0f, + .restThAcc = 0.192f, + }; using LSM6DSOutputHandler::i2c; @@ -62,7 +84,7 @@ struct LSM6DSR : LSM6DSOutputHandler { 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 + static constexpr uint8_t value = (0b01001000); // XL at 104 Hz, 4g FS }; struct Ctrl2GY { static constexpr uint8_t reg = 0x11; @@ -81,7 +103,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 +126,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, + TemperatureCall&& processTemperatureSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTemperatureSample, + 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 f0aa11ee7..25cb3502c 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -31,9 +31,9 @@ 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 = 480Hz, accel ODR = 240Hz template struct LSM6DSV : LSM6DSOutputHandler { @@ -42,15 +42,37 @@ struct LSM6DSV : LSM6DSOutputHandler { static constexpr auto Type = ImuID::LSM6DSV; static constexpr float GyrFreq = 480; - static constexpr float AccFreq = 120; + static constexpr float AccFreq = 240; 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 AccelSensitivity = 1000 / 0.122f; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + + // Temperature stability constant - how many degrees of temperature for the bias to + // change by 0.01 Though I don't know if it should be 0.1 or 0.01, this is a guess + // and seems to work better than 0.1 + static constexpr float TemperatureZROChange = 16.667f; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 1.0f, + .biasClip = 2.0f, + .restThGyr = 1.0f, + .restThAcc = 0.192f, + }; using LSM6DSOutputHandler::i2c; @@ -66,7 +88,7 @@ struct LSM6DSV : LSM6DSOutputHandler { }; struct Ctrl1XLODR { static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value = (0b0010110); // 120Hz, HAODR + static constexpr uint8_t value = (0b0010111); // 240Hz, HAODR }; struct Ctrl2GODR { static constexpr uint8_t reg = 0x11; @@ -84,16 +106,17 @@ 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 + static constexpr uint8_t value = (0b10000111 + ); // gyro and accel batched at 480Hz and 240Hz respectively }; 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 +141,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, + TemperatureCall&& processTemperatureSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTemperatureSample, + 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/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index 910d85a92..5d8a95895 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -31,7 +31,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 = accel ODR = 250Hz @@ -63,7 +63,26 @@ struct MPU6050 { static constexpr float MagTs = 1.0 / Freq; static constexpr float GyroSensitivity = 32.8f; - static constexpr float AccelSensitivity = 4096.0f; + static constexpr float AccelSensitivity = 8192.0f; + + // Temperature stability constant - how many degrees of temperature for the bias to + // change by 0.01 Though I don't know if it should be 0.1 or 0.01, this is a guess + // and seems to work better than 0.1 + static constexpr float TemperatureZROChange = 1.6f; + + // VQF parameters + // biasSigmaInit and and restThGyr should be the sensor's typical gyro bias + // biasClip should be 2x the sensor's typical gyro bias + // restThAcc should be the sensor's typical acceleration bias + + // Jesus christ this sensor sucks + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 20.0f, + .biasClip = 40.0f, + .restThGyr = 20.0f, + .restThAcc = 0.784f, + }; I2CImpl i2c; SlimeVR::Logging::Logger& logger; @@ -91,7 +110,7 @@ struct MPU6050 { struct AccelConfig { static constexpr uint8_t reg = 0x1c; - static constexpr uint8_t value = 0b10 << 3; // 8g + static constexpr uint8_t value = 0b01 << 3; // 4g }; static constexpr uint8_t OutTemp = MPU6050_RA_TEMP_OUT_H; @@ -166,8 +185,12 @@ struct MPU6050 { return result; } - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TemperatureCall&& processTemperatureSample + ) { const auto status = i2c.readReg(Regs::IntStatus); if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) { diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index cb1709b86..164a724eb 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -23,7 +23,12 @@ #pragma once +#include +#include +#include + #include "../SensorFusionRestDetect.h" +#include "../nonblockingcalibration/NonBlockingCalibration.h" #include "../sensor.h" #include "GlobalVars.h" @@ -32,8 +37,15 @@ namespace SlimeVR::Sensors { template