From 5cc67f20565de628c60b21f9edb04dea0d7dc758 Mon Sep 17 00:00:00 2001 From: kounocom Date: Sun, 14 Jul 2024 03:02:55 +0300 Subject: [PATCH 01/86] sfusion: Use MBE --- lib/vqf/vqf.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/vqf/vqf.h b/lib/vqf/vqf.h index cda5cc105..ecee90a72 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 From 9c3bb73f0244535d4cf70776f6ed923d6b2432f1 Mon Sep 17 00:00:00 2001 From: kounocom Date: Sun, 14 Jul 2024 03:04:07 +0300 Subject: [PATCH 02/86] sfusion: Tune MBE parameters --- src/sensors/SensorFusion.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index cc0116205..59c21c4c5 100644 --- a/src/sensors/SensorFusion.h +++ b/src/sensors/SensorFusion.h @@ -50,6 +50,8 @@ namespace SlimeVR restMinT = 2.0f; restThGyr = 0.6f; // 400 norm restThAcc = 0.06f; // 100 norm + biasSigmaMotion = 0.1175f; + biasVerticalForgettingFactor = 0.0001; } }; #endif From bfa3df936105d2e5f2823989a6293b962af47ea1 Mon Sep 17 00:00:00 2001 From: kounocom Date: Wed, 29 May 2024 03:46:05 +0300 Subject: [PATCH 03/86] sfusion: Unify full-scale ranges for all sensors (1000DPS/4G) --- src/sensors/softfusion/drivers/bmi270.h | 4 ++-- src/sensors/softfusion/drivers/icm42688.h | 4 ++-- src/sensors/softfusion/drivers/lsm6ds3trc.h | 4 ++-- src/sensors/softfusion/drivers/lsm6dso.h | 4 ++-- src/sensors/softfusion/drivers/lsm6dsr.h | 4 ++-- src/sensors/softfusion/drivers/lsm6dsv.h | 4 ++-- src/sensors/softfusion/drivers/mpu6050.h | 4 ++-- 7 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index b5f49e52e..082082b1a 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -50,7 +50,7 @@ struct BMI270 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; struct MotionlessCalibrationData { @@ -173,7 +173,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 { diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index e071db55a..5a5e0a9db 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -48,7 +48,7 @@ struct ICM42688 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; I2CImpl i2c; SlimeVR::Logging::Logger &logger; @@ -84,7 +84,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) | 0b1000; //4g, odr = 100Hz }; struct PwrMgmt { static constexpr uint8_t reg = 0x4e; diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index 785a5e325..1c05ad6fe 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -48,7 +48,7 @@ struct LSM6DS3TRC static constexpr float MagTs=1.0/Freq; static constexpr float GyroSensitivity = 28.571428571f; - static constexpr float AccelSensitivity = 4098.360655738f; + static constexpr float AccelSensitivity = 8196.72131148f; I2CImpl i2c; SlimeVR::Logging::Logger logger; @@ -63,7 +63,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) | (0b0110 << 4); //4g, 416Hz }; struct Ctrl2G { static constexpr uint8_t reg = 0x11; diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index 0dc4870f8..dbfff6abd 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -52,7 +52,7 @@ struct LSM6DSO : LSM6DSOutputHandler static constexpr float MagTs=1.0/MagFreq; static constexpr float GyroSensitivity = 1000 / 35.0f; - static constexpr float AccelSensitivity = 1000 / 0.244f; + static constexpr float AccelSensitivity = 1000 / 0.122f; using LSM6DSOutputHandler::i2c; @@ -64,7 +64,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 = (0b01001000); // XL at 104 Hz, 4g FS }; struct Ctrl2GY { static constexpr uint8_t reg = 0x11; diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h index ebef63ca2..5530547cd 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -52,7 +52,7 @@ struct LSM6DSR : LSM6DSOutputHandler static constexpr float MagTs=1.0/MagFreq; static constexpr float GyroSensitivity = 1000 / 35.0f; - static constexpr float AccelSensitivity = 1000 / 0.244f; + static constexpr float AccelSensitivity = 1000 / 0.122f; using LSM6DSOutputHandler::i2c; @@ -64,7 +64,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; diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index ef073ecf6..f3ecd283e 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -52,7 +52,7 @@ struct LSM6DSV : LSM6DSOutputHandler static constexpr float MagTs=1.0/MagFreq; static constexpr float GyroSensitivity = 1000 / 35.0f; - static constexpr float AccelSensitivity = 1000 / 0.244f; + static constexpr float AccelSensitivity = 1000 / 0.122f; using LSM6DSOutputHandler::i2c; @@ -85,7 +85,7 @@ 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; diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index 6546784c8..aebca8201 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -65,7 +65,7 @@ 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; I2CImpl i2c; SlimeVR::Logging::Logger &logger; @@ -90,7 +90,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; From d383d4589c78b85dd56555ff47a18a7f533b5b23 Mon Sep 17 00:00:00 2001 From: kounocom Date: Sun, 14 Jul 2024 03:06:13 +0300 Subject: [PATCH 04/86] sfusion: Perform custom motionless calibration before gyro offset --- src/sensors/softfusion/softfusionsensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index ec076a757..bfbc844f4 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -274,12 +274,12 @@ class SoftFusionSensor : public Sensor if (calibrationType == 0) { // ALL calibrateSampleRate(); - calibrateGyroOffset(); if constexpr(HasMotionlessCalib) { typename imu::MotionlessCalibrationData calibData; m_sensor.motionlessCalibration(calibData); std::memcpy(m_calibration.MotionlessData, &calibData, sizeof(calibData)); } + calibrateGyroOffset(); calibrateAccel(); } else if (calibrationType == 1) From 6b53da1df88ba29bd79fdbe3471ebc557dc8f508 Mon Sep 17 00:00:00 2001 From: kounocom Date: Wed, 21 Aug 2024 19:58:02 +0300 Subject: [PATCH 05/86] LSM6DSV: Increase accel ODR to 240Hz --- src/sensors/softfusion/drivers/lsm6dsv.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index f3ecd283e..7501ef33d 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -32,9 +32,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 @@ -44,7 +44,7 @@ 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 GyrTs=1.0/GyrFreq; @@ -68,7 +68,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; @@ -89,7 +89,7 @@ struct LSM6DSV : LSM6DSOutputHandler }; 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; From 9f52744f602d68369fc63cc9af47def4c088c005 Mon Sep 17 00:00:00 2001 From: kounocom Date: Wed, 21 Aug 2024 19:58:12 +0300 Subject: [PATCH 06/86] LSM6DSO: Increase accel ODR to 208Hz --- src/sensors/softfusion/drivers/lsm6dso.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index dbfff6abd..a42467a5a 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -32,9 +32,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 @@ -44,7 +44,7 @@ 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 GyrTs=1.0/GyrFreq; @@ -64,7 +64,7 @@ struct LSM6DSO : LSM6DSOutputHandler static constexpr uint8_t OutTemp = 0x20; struct Ctrl1XL { static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value = (0b01001000); // XL at 104 Hz, 4g FS + static constexpr uint8_t value = (0b01011000); // XL at 208 Hz, 4g FS }; struct Ctrl2GY { static constexpr uint8_t reg = 0x11; @@ -77,7 +77,7 @@ struct LSM6DSO : LSM6DSOutputHandler }; struct FifoCtrl3BDR { static constexpr uint8_t reg = 0x09; - static constexpr uint8_t value = (0b0110) | (0b0110 << 4); //gyro and accel batched at 417Hz + static constexpr uint8_t value = (0b01100101); //gyro and accel batched at 416Hz and 208hz respectively }; struct FifoCtrl4Mode { static constexpr uint8_t reg = 0x0a; From ea1dfbdb0684a32225d76d5cae54c756227a364b Mon Sep 17 00:00:00 2001 From: kounocom Date: Wed, 21 Aug 2024 19:58:20 +0300 Subject: [PATCH 07/86] BMI270: Increase accel ODR to 200Hz --- src/sensors/softfusion/drivers/bmi270.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index 082082b1a..530891fa0 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -32,9 +32,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,7 +45,7 @@ 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 MagTs=1.0/100; @@ -162,7 +162,7 @@ struct BMI270 static constexpr uint8_t filterHighPerfMode = 1 << 7; - static constexpr uint8_t value = rate100Hz | DLPFModeAvg4 | filterHighPerfMode; + static constexpr uint8_t value = rate200Hz | DLPFModeAvg4 | filterHighPerfMode; }; struct AccRange { From 1e0fc4ac3d74cb96a18ff2b178c5817ce53f7b74 Mon Sep 17 00:00:00 2001 From: kounocom Date: Wed, 21 Aug 2024 20:01:38 +0300 Subject: [PATCH 08/86] sfusion: Adjust VQF params --- src/sensors/SensorFusion.h | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index 59c21c4c5..91167d71f 100644 --- a/src/sensors/SensorFusion.h +++ b/src/sensors/SensorFusion.h @@ -46,12 +46,17 @@ namespace SlimeVR #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 + tauAcc = 3.0f; + biasSigmaInit = 1.0f; + biasForgettingTime = 60.0f; + biasClip = 2.0f; biasSigmaMotion = 0.1175f; - biasVerticalForgettingFactor = 0.0001; + biasVerticalForgettingFactor = 0; + biasSigmaRest = 0.007f; + restMinT = 1.5f; + restFilterTau = 0.5f; + restThGyr = 1.0f; // 400 norm + restThAcc = 0.196f; // 100 norm } }; #endif From 04895aa00a962466a8edaf9126e1cf853fc66e72 Mon Sep 17 00:00:00 2001 From: kounocom Date: Wed, 21 Aug 2024 20:01:53 +0300 Subject: [PATCH 09/86] sfusion: Disable calibration entirely --- src/sensors/softfusion/softfusionsensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index bfbc844f4..20bcd58ee 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -36,7 +36,7 @@ class SoftFusionSensor : public Sensor { using imu = T; using RawVectorT = std::array; - static constexpr auto UpsideDownCalibrationInit = true; + static constexpr auto UpsideDownCalibrationInit = false; static constexpr auto GyroCalibDelaySeconds = 5; static constexpr auto GyroCalibSeconds = 5; static constexpr auto SampleRateCalibDelaySeconds = 1; From 9275f62d295340b40f925d5876f7281c2535a2f9 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sun, 25 Aug 2024 23:30:17 +0200 Subject: [PATCH 10/86] Skip loading sensor calibration --- src/configuration/Configuration.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index 0114c30c0..95e108406 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -156,6 +156,7 @@ namespace SlimeVR { } void Configuration::loadCalibrations() { + return; SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) { CalibrationConfig calibrationConfig; f.read((uint8_t*)&calibrationConfig, sizeof(CalibrationConfig)); From 5c26f04f7396e23a2c8d3f3b3f6d6e1229ddba46 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 11 Sep 2024 03:14:15 +0200 Subject: [PATCH 11/86] Put temperature into the FIFO wherever possible --- src/configuration/CalibrationConfig.h | 257 ++-- src/defines.h | 33 +- src/sensors/softfusion/drivers/bmi270.h | 830 ++++++------ src/sensors/softfusion/drivers/icm42688.h | 309 ++--- .../softfusion/drivers/lsm6ds-common.h | 185 +-- src/sensors/softfusion/drivers/lsm6ds3trc.h | 267 ++-- src/sensors/softfusion/drivers/lsm6dso.h | 216 +-- src/sensors/softfusion/drivers/lsm6dsr.h | 216 +-- src/sensors/softfusion/drivers/lsm6dsv.h | 246 ++-- src/sensors/softfusion/drivers/mpu6050.h | 367 +++--- src/sensors/softfusion/softfusionsensor.h | 1171 +++++++++-------- 11 files changed, 2186 insertions(+), 1911 deletions(-) diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h index f07e0dc2f..1721a96fd 100644 --- a/src/configuration/CalibrationConfig.h +++ b/src/configuration/CalibrationConfig.h @@ -1,141 +1,142 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV - - 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. + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + 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. */ #ifndef SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H #define SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H #include + #include "consts.h" namespace SlimeVR { - namespace Configuration { - struct BMI160CalibrationConfig { - // accelerometer offsets and correction matrix - float A_B[3]; - float A_Ainv[3][3]; - - // magnetometer offsets and correction matrix - float M_B[3]; - float M_Ainv[3][3]; - - // raw offsets, determined from gyro at rest - float G_off[3]; - - // calibration temperature for dynamic compensation - float temperature; - }; - - struct SoftFusionCalibrationConfig { - ImuID ImuType; - uint16_t MotionlessDataLen; - - // accelerometer offsets and correction matrix - float A_B[3]; - float A_Ainv[3][3]; - - // magnetometer offsets and correction matrix - float M_B[3]; - float M_Ainv[3][3]; - - // raw offsets, determined from gyro at rest - float G_off[3]; - - // calibration temperature for dynamic compensation - float temperature; - - // real measured sensor sampling rate - float A_Ts; - float G_Ts; - float M_Ts; - - // gyro sensitivity multiplier - float G_Sens[3]; - - uint8_t MotionlessData[60]; - }; - - - struct MPU6050CalibrationConfig { - // accelerometer offsets and correction matrix - float A_B[3]; - - // raw offsets, determined from gyro at rest - float G_off[3]; - }; - - struct MPU9250CalibrationConfig { - // accelerometer offsets and correction matrix - float A_B[3]; - float A_Ainv[3][3]; - - // magnetometer offsets and correction matrix - float M_B[3]; - float M_Ainv[3][3]; - - // raw offsets, determined from gyro at rest - float G_off[3]; - }; - - struct ICM20948CalibrationConfig { - // gyroscope bias - int32_t G[3]; - - // accelerometer bias - int32_t A[3]; - - // compass bias - int32_t C[3]; - }; - - struct ICM42688CalibrationConfig { - // accelerometer offsets and correction matrix - float A_B[3]; - float A_Ainv[3][3]; - - // magnetometer offsets and correction matrix - float M_B[3]; - float M_Ainv[3][3]; - - // raw offsets, determined from gyro at rest - float G_off[3]; - }; - - enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, SFUSION }; - - const char* calibrationConfigTypeToString(CalibrationConfigType type); - - struct CalibrationConfig { - CalibrationConfigType type; +namespace Configuration { +struct BMI160CalibrationConfig { + // accelerometer offsets and correction matrix + float A_B[3]; + float A_Ainv[3][3]; - union { - BMI160CalibrationConfig bmi160; - SoftFusionCalibrationConfig sfusion; - MPU6050CalibrationConfig mpu6050; - MPU9250CalibrationConfig mpu9250; - ICM20948CalibrationConfig icm20948; - } data; - }; - } -} + // magnetometer offsets and correction matrix + float M_B[3]; + float M_Ainv[3][3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; + + // calibration temperature for dynamic compensation + float temperature; +}; + +struct SoftFusionCalibrationConfig { + ImuID ImuType; + uint16_t MotionlessDataLen; + + // accelerometer offsets and correction matrix + float A_B[3]; + float A_Ainv[3][3]; + + // magnetometer offsets and correction matrix + float M_B[3]; + float M_Ainv[3][3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; + + // calibration temperature for dynamic compensation + float temperature; + + // real measured sensor sampling rate + float A_Ts; + float G_Ts; + float M_Ts; + float T_Ts; + + // gyro sensitivity multiplier + float G_Sens[3]; + + uint8_t MotionlessData[60]; +}; + +struct MPU6050CalibrationConfig { + // accelerometer offsets and correction matrix + float A_B[3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; +}; + +struct MPU9250CalibrationConfig { + // accelerometer offsets and correction matrix + float A_B[3]; + float A_Ainv[3][3]; + + // magnetometer offsets and correction matrix + float M_B[3]; + float M_Ainv[3][3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; +}; + +struct ICM20948CalibrationConfig { + // gyroscope bias + int32_t G[3]; + + // accelerometer bias + int32_t A[3]; + + // compass bias + int32_t C[3]; +}; + +struct ICM42688CalibrationConfig { + // accelerometer offsets and correction matrix + float A_B[3]; + float A_Ainv[3][3]; + + // magnetometer offsets and correction matrix + float M_B[3]; + float M_Ainv[3][3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; +}; + +enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, SFUSION }; + +const char* calibrationConfigTypeToString(CalibrationConfigType type); + +struct CalibrationConfig { + CalibrationConfigType type; + + union { + BMI160CalibrationConfig bmi160; + SoftFusionCalibrationConfig sfusion; + MPU6050CalibrationConfig mpu6050; + MPU9250CalibrationConfig mpu9250; + ICM20948CalibrationConfig icm20948; + } data; +}; +} // namespace Configuration +} // namespace SlimeVR #endif diff --git a/src/defines.h b/src/defines.h index 33296454a..abfc2177a 100644 --- a/src/defines.h +++ b/src/defines.h @@ -26,16 +26,15 @@ // ================================================ // Set parameters of IMU and board used -#define IMU IMU_BNO085 -#define SECOND_IMU IMU -#define BOARD BOARD_SLIMEVR -#define IMU_ROTATION DEG_270 -#define SECOND_IMU_ROTATION DEG_270 +#define IMU IMU_BMI270 +#define BOARD BOARD_CUSTOM +#define IMU_ROTATION DEG_90 #define PRIMARY_IMU_OPTIONAL false -#define SECONDARY_IMU_OPTIONAL true -#define MAX_IMU_COUNT 2 +#define MAX_IMU_COUNT 1 + +#define ON_OFF_BUTTON 1 // Axis mapping example /* @@ -48,8 +47,7 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P #ifndef IMU_DESC_LIST #define IMU_DESC_LIST \ - IMU_DESC_ENTRY(IMU, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, PIN_IMU_INT) \ - IMU_DESC_ENTRY(SECOND_IMU, SECONDARY_IMU_ADDRESS_TWO, SECOND_IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, SECONDARY_IMU_OPTIONAL, PIN_IMU_INT_2) + IMU_DESC_ENTRY(IMU, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, PIN_IMU_INT) #endif // Battery monitoring options (comment to disable): @@ -148,7 +146,22 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P // #define LED_PIN 2 // #define LED_INVERTED false #elif BOARD == BOARD_CUSTOM - // Define pins by the examples above + #define PIN_IMU_SDA 5 + #define PIN_IMU_SCL 6 + #define PIN_IMU_INT 255 + #define PIN_IMU_INT_2 255 + #define PIN_BATTERY_LEVEL 3 + #define LED_PIN 0 + #define LED_INVERTED true + #ifndef BATTERY_SHIELD_RESISTANCE + #define BATTERY_SHIELD_RESISTANCE 0 + #endif + #ifndef BATTERY_SHIELD_R1 + #define BATTERY_SHIELD_R1 150 + #endif + #ifndef BATTERY_SHIELD_R2 + #define BATTERY_SHIELD_R2 150 + #endif #elif BOARD == BOARD_WROOM32 #define PIN_IMU_SDA 21 #define PIN_IMU_SCL 22 diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index 530891fa0..56c33e98a 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -1,36 +1,36 @@ /* - 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. + 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 +#include +#include #include + #include "bmi270fw.h" -namespace SlimeVR::Sensors::SoftFusion::Drivers -{ +namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 4g // and gyroscope range at 1000dps @@ -38,382 +38,410 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers // Timestamps reading are not used template -struct BMI270 -{ - static constexpr uint8_t Address = 0x68; - static constexpr auto Name = "BMI270"; - static constexpr auto Type = ImuID::BMI270; - - static constexpr float GyrTs=1.0/400.0; - static constexpr float AccTs=1.0/200.0; - - static constexpr float MagTs=1.0/100; - - static constexpr float GyroSensitivity = 32.768f; - static constexpr float AccelSensitivity = 8192.0f; - - struct MotionlessCalibrationData - { - bool valid; - uint8_t x, y, z; - }; - - I2CImpl i2c; - SlimeVR::Logging::Logger &logger; - int8_t zxFactor; - BMI270(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : i2c(i2c), logger(logger), zxFactor(0) {} - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x00; - static constexpr uint8_t value = 0x24; - }; - static constexpr uint8_t TempData = 0x22; - - struct Cmd { - static constexpr uint8_t reg = 0x7e; - static constexpr uint8_t valueSwReset = 0xb6; - static constexpr uint8_t valueFifoFlush = 0xb0; - static constexpr uint8_t valueGTrigger = 0x02; - }; - - struct PwrConf { - static constexpr uint8_t reg = 0x7c; - static constexpr uint8_t valueNoPowerSaving = 0x0; - static constexpr uint8_t valueFifoSelfWakeup = 0x2; - }; - - struct PwrCtrl { - static constexpr uint8_t reg = 0x7d; - static constexpr uint8_t valueOff = 0x0; - static constexpr uint8_t valueGyrAccTempOn = 0b1110; // aux off - static constexpr uint8_t valueAccOn = 0b0100; // aux, gyr, temp off - }; - - struct InitCtrl { - static constexpr uint8_t reg = 0x59; - static constexpr uint8_t valueStartInit = 0x00; - static constexpr uint8_t valueEndInit = 0x01; - }; - - static constexpr uint8_t InitAddr = 0x5b; - static constexpr uint8_t InitData = 0x5e; - - struct InternalStatus { - static constexpr uint8_t reg = 0x21; - static constexpr uint8_t initializedBit = 0x01; - }; - - struct GyrConf { - static constexpr uint8_t reg = 0x42; - - static constexpr uint8_t rate25Hz = 6; - static constexpr uint8_t rate50Hz = 7; - static constexpr uint8_t rate100Hz = 8; - static constexpr uint8_t rate200Hz = 9; - static constexpr uint8_t rate400Hz = 10; - static constexpr uint8_t rate800Hz = 11; - static constexpr uint8_t rate1600Hz = 12; - static constexpr uint8_t rate3200Hz = 13; - - static constexpr uint8_t DLPFModeOsr4 = 0 << 4; - static constexpr uint8_t DLPFModeOsr2 = 1 << 4; - static constexpr uint8_t DLPFModeNorm = 2 << 4; - - static constexpr uint8_t noisePerfMode = 1 << 6; - static constexpr uint8_t filterHighPerfMode = 1 << 7; - - static constexpr uint8_t value = rate400Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode; - }; - - struct GyrRange { - static constexpr uint8_t reg = 0x43; - - static constexpr uint8_t range125dps = 4; - static constexpr uint8_t range250dps = 3; - static constexpr uint8_t range500dps = 2; - static constexpr uint8_t range1000dps = 1; - static constexpr uint8_t range2000dps = 0; - - static constexpr uint8_t value = range1000dps; - }; - - struct AccConf { - static constexpr uint8_t reg = 0x40; - - static constexpr uint8_t rate0_78Hz = 1; - static constexpr uint8_t rate1_5Hz = 2; - static constexpr uint8_t rate3_1Hz = 3; - static constexpr uint8_t rate6_25Hz = 4; - static constexpr uint8_t rate12_5Hz = 5; - static constexpr uint8_t rate25Hz = 6; - static constexpr uint8_t rate50Hz = 7; - static constexpr uint8_t rate100Hz = 8; - static constexpr uint8_t rate200Hz = 9; - static constexpr uint8_t rate400Hz = 10; - static constexpr uint8_t rate800Hz = 11; - static constexpr uint8_t rate1600Hz = 12; - - static constexpr uint8_t DLPFModeAvg1 = 0 << 4; - static constexpr uint8_t DLPFModeAvg2 = 1 << 4; - static constexpr uint8_t DLPFModeAvg4 = 2 << 4; - static constexpr uint8_t DLPFModeAvg8 = 3 << 4; - - static constexpr uint8_t filterHighPerfMode = 1 << 7; - - static constexpr uint8_t value = rate200Hz | DLPFModeAvg4 | filterHighPerfMode; - }; - - struct AccRange { - static constexpr uint8_t reg = 0x41; - - static constexpr uint8_t range2G = 0; - static constexpr uint8_t range4G = 1; - static constexpr uint8_t range8G = 2; - static constexpr uint8_t range16G = 3; - - static constexpr uint8_t value = range4G; - }; - - struct FifoConfig0 { - static constexpr uint8_t reg = 0x48; - static constexpr uint8_t value = 0x01; // fifo_stop_on_full=1, fifo_time_en=0 - }; - - struct FifoConfig1 { - static constexpr uint8_t reg = 0x49; - static constexpr uint8_t value = (1 << 4) | (1 << 6) | (1 << 7); // header en, acc en, gyr en - }; - - struct GyrCrtConf { - static constexpr uint8_t reg = 0x69; - static constexpr uint8_t valueRunning = (1 << 2); // crt_running = 1 - static constexpr uint8_t valueStopped = 0x0; // crt_running = 0 - }; - - struct GTrig1 { // on feature page 1! - static constexpr uint8_t reg = 0x32; - static constexpr uint16_t valueTriggerCRT = (1 << 8); // select=crt - }; - - struct GyrGainStatus { // on feature page 0! - static constexpr uint8_t reg = 0x38; - static constexpr uint8_t statusOffset = 3; - }; - - struct Offset6 { // on feature page 0! - static constexpr uint8_t reg = 0x77; - static constexpr uint8_t value = (1 << 7); // gyr_gain_en = 1 - }; - - static constexpr uint8_t FeatPage = 0x2f; - - static constexpr uint8_t GyrUserGain = 0x78; // undocumented reg, got from official bmi270 driver - - static constexpr uint8_t FifoCount = 0x24; - static constexpr uint8_t FifoData = 0x26; - static constexpr uint8_t RaGyrCas = 0x3c; // on feature page 0! - }; - - struct Fifo { - static constexpr uint8_t ModeMask = 0b11000000; - static constexpr uint8_t SkipFrame = 0b01000000; - static constexpr uint8_t DataFrame = 0b10000000; - - static constexpr uint8_t GyrDataBit = 0b00001000; - static constexpr uint8_t AccelDataBit = 0b00000100; - }; - - bool restartAndInit() { - // perform initialization step - i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset); - delay(12); - // disable power saving - i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving); - delay(1); - - // firmware upload - i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit); - for (uint16_t pos=0; pos> 1; // convert current position to words - const uint16_t position = (pos_words & 0x0F) | ((pos_words << 4) & 0xff00); - i2c.writeReg16(Regs::InitAddr, position); - // write actual payload chunk - const uint16_t burstWrite = std::min(sizeof(bmi270_firmware) - pos, I2CImpl::MaxTransactionLength); - i2c.writeBytes(Regs::InitData, burstWrite, const_cast(bmi270_firmware + pos)); - pos += burstWrite; - } - i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit); - delay(140); - - // leave fifo_self_wakeup enabled - i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup); - // check if IMU initialized correctly - if (!(i2c.readReg(Regs::InternalStatus::reg) & Regs::InternalStatus::initializedBit)) - { - // firmware upload fail or sensor not initialized - return false; - } - - // read zx factor used to reduce gyro cross-sensitivity error - const uint8_t zx_factor_reg = i2c.readReg(Regs::RaGyrCas); - const uint8_t sign_byte = (zx_factor_reg << 1) & 0x80; - zxFactor = static_cast(zx_factor_reg | sign_byte); - return true; - } - - void setNormalConfig(MotionlessCalibrationData &gyroSensitivity) - { - i2c.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value); - i2c.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value); - - i2c.writeReg(Regs::AccConf::reg, Regs::AccConf::value); - i2c.writeReg(Regs::AccRange::reg, Regs::AccRange::value); - - if (gyroSensitivity.valid) - { - i2c.writeReg(Regs::Offset6::reg, Regs::Offset6::value); - i2c.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x); - } - - i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); - delay(100); // power up delay - i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); - i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); - - delay(4); - i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush); - delay(2); - } - - bool initialize(MotionlessCalibrationData &gyroSensitivity) - { - if (!restartAndInit()) { - return false; - } - - setNormalConfig(gyroSensitivity); - - return true; - } - - void motionlessCalibration(MotionlessCalibrationData &gyroSensitivity) - { - // perfrom gyroscope motionless sensitivity calibration (CRT) - // need to start from clean state according to spec - restartAndInit(); - // only Accel ON - i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn); - delay(100); - i2c.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning); - i2c.writeReg(Regs::FeatPage, 1); - i2c.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT); - i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger); - delay(200); - - while(i2c.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) { - logger.info("CRT running. Do not move tracker!"); - delay(200); - } - - i2c.writeReg(Regs::FeatPage, 0); - - uint8_t status = i2c.readReg(Regs::GyrGainStatus::reg) >> Regs::GyrGainStatus::statusOffset; - // turn gyroscope back on - i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); - delay(100); - - if (status != 0) { - logger.error("CRT failed with status 0x%x. Recalibrate again to enable CRT.", status); - if (status == 0x03) { - logger.error("Reason: tracker was moved during CRT!"); - } - } - else { - std::array crt_values; - i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data()); - logger.debug("CRT finished successfully, result 0x%x, 0x%x, 0x%x", crt_values[0], crt_values[1], crt_values[2]); - gyroSensitivity.valid = true; - gyroSensitivity.x = crt_values[0]; - gyroSensitivity.y = crt_values[1]; - gyroSensitivity.z = crt_values[2]; - } - - setNormalConfig(gyroSensitivity); - } - - float getDirectTemp() const - { - // middle value is 23 degrees C (0x0000) - // temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9 degrees C (0x7FFF) - constexpr float TempStep = 128. / 65535; - const auto value = static_cast(i2c.readReg16(Regs::TempData)); - return static_cast(value) * TempStep + 23.0f; - } - - using FifoBuffer = std::array; - FifoBuffer read_buffer; - - template - inline T getFromFifo(uint32_t &position, FifoBuffer& fifo) { - T to_ret; - std::memcpy(&to_ret, &fifo[position], sizeof(T)); - position += sizeof(T); - return to_ret; - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); - - const auto bytes_to_read = std::min(static_cast(read_buffer.size()), - static_cast(fifo_bytes)); - i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); - - for (uint32_t i=0u; i(i, read_buffer); - if ((header & Fifo::ModeMask) == Fifo::SkipFrame && (i - bytes_to_read) >= 1) { - getFromFifo(i, read_buffer); // skip 1 byte - } - else if ((header & Fifo::ModeMask) == Fifo::DataFrame) { - const uint8_t required_length = - (((header & Fifo::GyrDataBit) >> Fifo::GyrDataBit) + - ((header & Fifo::AccelDataBit) >> Fifo::AccelDataBit)) * 6; - if (i - bytes_to_read < required_length) { - // incomplete frame, will be re-read next time - break; - } - if (header & Fifo::GyrDataBit) { - int16_t gyro[3]; - gyro[0] = getFromFifo(i, read_buffer); - gyro[1] = getFromFifo(i, read_buffer); - gyro[2] = getFromFifo(i, read_buffer); - using ShortLimit = std::numeric_limits; - // apply zx factor, todo: this awful line should be simplified and validated - gyro[0] = std::clamp(static_cast(gyro[0]) - static_cast((static_cast(zxFactor) * gyro[2]) / 512), - static_cast(ShortLimit::min()), static_cast(ShortLimit::max())); - processGyroSample(gyro, GyrTs); - } - - if (header & Fifo::AccelDataBit) { - int16_t accel[3]; - accel[0] = getFromFifo(i, read_buffer); - accel[1] = getFromFifo(i, read_buffer); - accel[2] = getFromFifo(i, read_buffer); - processAccelSample(accel, AccTs); - } - } - } - } - +struct BMI270 { + static constexpr uint8_t Address = 0x68; + static constexpr auto Name = "BMI270"; + static constexpr auto Type = ImuID::BMI270; + + static constexpr float GyrTs = 1.0 / 400.0; + static constexpr float AccTs = 1.0 / 200.0; + + static constexpr float MagTs = 1.0 / 100; + + static constexpr float GyroSensitivity = 32.768f; + static constexpr float AccelSensitivity = 8192.0f; + + struct MotionlessCalibrationData { + bool valid; + uint8_t x, y, z; + }; + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + int8_t zxFactor; + BMI270(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) + , zxFactor(0) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x00; + static constexpr uint8_t value = 0x24; + }; + static constexpr uint8_t TempData = 0x22; + + struct Cmd { + static constexpr uint8_t reg = 0x7e; + static constexpr uint8_t valueSwReset = 0xb6; + static constexpr uint8_t valueFifoFlush = 0xb0; + static constexpr uint8_t valueGTrigger = 0x02; + }; + + struct PwrConf { + static constexpr uint8_t reg = 0x7c; + static constexpr uint8_t valueNoPowerSaving = 0x0; + static constexpr uint8_t valueFifoSelfWakeup = 0x2; + }; + + struct PwrCtrl { + static constexpr uint8_t reg = 0x7d; + static constexpr uint8_t valueOff = 0x0; + static constexpr uint8_t valueGyrAccTempOn = 0b1110; // aux off + static constexpr uint8_t valueAccOn = 0b0100; // aux, gyr, temp off + }; + + struct InitCtrl { + static constexpr uint8_t reg = 0x59; + static constexpr uint8_t valueStartInit = 0x00; + static constexpr uint8_t valueEndInit = 0x01; + }; + + static constexpr uint8_t InitAddr = 0x5b; + static constexpr uint8_t InitData = 0x5e; + + struct InternalStatus { + static constexpr uint8_t reg = 0x21; + static constexpr uint8_t initializedBit = 0x01; + }; + + struct GyrConf { + static constexpr uint8_t reg = 0x42; + + static constexpr uint8_t rate25Hz = 6; + static constexpr uint8_t rate50Hz = 7; + static constexpr uint8_t rate100Hz = 8; + static constexpr uint8_t rate200Hz = 9; + static constexpr uint8_t rate400Hz = 10; + static constexpr uint8_t rate800Hz = 11; + static constexpr uint8_t rate1600Hz = 12; + static constexpr uint8_t rate3200Hz = 13; + + static constexpr uint8_t DLPFModeOsr4 = 0 << 4; + static constexpr uint8_t DLPFModeOsr2 = 1 << 4; + static constexpr uint8_t DLPFModeNorm = 2 << 4; + + static constexpr uint8_t noisePerfMode = 1 << 6; + static constexpr uint8_t filterHighPerfMode = 1 << 7; + + static constexpr uint8_t value + = rate400Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode; + }; + + struct GyrRange { + static constexpr uint8_t reg = 0x43; + + static constexpr uint8_t range125dps = 4; + static constexpr uint8_t range250dps = 3; + static constexpr uint8_t range500dps = 2; + static constexpr uint8_t range1000dps = 1; + static constexpr uint8_t range2000dps = 0; + + static constexpr uint8_t value = range1000dps; + }; + + struct AccConf { + static constexpr uint8_t reg = 0x40; + + static constexpr uint8_t rate0_78Hz = 1; + static constexpr uint8_t rate1_5Hz = 2; + static constexpr uint8_t rate3_1Hz = 3; + static constexpr uint8_t rate6_25Hz = 4; + static constexpr uint8_t rate12_5Hz = 5; + static constexpr uint8_t rate25Hz = 6; + static constexpr uint8_t rate50Hz = 7; + static constexpr uint8_t rate100Hz = 8; + static constexpr uint8_t rate200Hz = 9; + static constexpr uint8_t rate400Hz = 10; + static constexpr uint8_t rate800Hz = 11; + static constexpr uint8_t rate1600Hz = 12; + + static constexpr uint8_t DLPFModeAvg1 = 0 << 4; + static constexpr uint8_t DLPFModeAvg2 = 1 << 4; + static constexpr uint8_t DLPFModeAvg4 = 2 << 4; + static constexpr uint8_t DLPFModeAvg8 = 3 << 4; + + static constexpr uint8_t filterHighPerfMode = 1 << 7; + + static constexpr uint8_t value + = rate200Hz | DLPFModeAvg4 | filterHighPerfMode; + }; + + struct AccRange { + static constexpr uint8_t reg = 0x41; + + static constexpr uint8_t range2G = 0; + static constexpr uint8_t range4G = 1; + static constexpr uint8_t range8G = 2; + static constexpr uint8_t range16G = 3; + + static constexpr uint8_t value = range4G; + }; + + struct FifoConfig0 { + static constexpr uint8_t reg = 0x48; + static constexpr uint8_t value + = 0x01; // fifo_stop_on_full=1, fifo_time_en=0 + }; + + struct FifoConfig1 { + static constexpr uint8_t reg = 0x49; + static constexpr uint8_t value + = (1 << 4) | (1 << 6) | (1 << 7); // header en, acc en, gyr en + }; + + struct GyrCrtConf { + static constexpr uint8_t reg = 0x69; + static constexpr uint8_t valueRunning = (1 << 2); // crt_running = 1 + static constexpr uint8_t valueStopped = 0x0; // crt_running = 0 + }; + + struct GTrig1 { // on feature page 1! + static constexpr uint8_t reg = 0x32; + static constexpr uint16_t valueTriggerCRT = (1 << 8); // select=crt + }; + + struct GyrGainStatus { // on feature page 0! + static constexpr uint8_t reg = 0x38; + static constexpr uint8_t statusOffset = 3; + }; + + struct Offset6 { // on feature page 0! + static constexpr uint8_t reg = 0x77; + static constexpr uint8_t value = (1 << 7); // gyr_gain_en = 1 + }; + + static constexpr uint8_t FeatPage = 0x2f; + + static constexpr uint8_t GyrUserGain + = 0x78; // undocumented reg, got from official bmi270 driver + + static constexpr uint8_t FifoCount = 0x24; + static constexpr uint8_t FifoData = 0x26; + static constexpr uint8_t RaGyrCas = 0x3c; // on feature page 0! + }; + + struct Fifo { + static constexpr uint8_t ModeMask = 0b11000000; + static constexpr uint8_t SkipFrame = 0b01000000; + static constexpr uint8_t DataFrame = 0b10000000; + + static constexpr uint8_t GyrDataBit = 0b00001000; + static constexpr uint8_t AccelDataBit = 0b00000100; + }; + + bool restartAndInit() { + // perform initialization step + i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset); + delay(12); + // disable power saving + i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving); + delay(1); + + // firmware upload + i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit); + for (uint16_t pos = 0; pos < sizeof(bmi270_firmware);) { + // tell the device current position + + // this thing is little endian, but it requires address in bizzare form + // LSB register is only 4 bits, while MSB register is 8bits + // also value requested is in words (16bit) not in bytes (8bit) + + const uint16_t pos_words = pos >> 1; // convert current position to words + const uint16_t position = (pos_words & 0x0F) | ((pos_words << 4) & 0xff00); + i2c.writeReg16(Regs::InitAddr, position); + // write actual payload chunk + const uint16_t burstWrite = std::min( + sizeof(bmi270_firmware) - pos, + I2CImpl::MaxTransactionLength + ); + i2c.writeBytes( + Regs::InitData, + burstWrite, + const_cast(bmi270_firmware + pos) + ); + pos += burstWrite; + } + i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit); + delay(140); + + // leave fifo_self_wakeup enabled + i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup); + // check if IMU initialized correctly + if (!(i2c.readReg(Regs::InternalStatus::reg) + & Regs::InternalStatus::initializedBit)) { + // firmware upload fail or sensor not initialized + return false; + } + + // read zx factor used to reduce gyro cross-sensitivity error + const uint8_t zx_factor_reg = i2c.readReg(Regs::RaGyrCas); + const uint8_t sign_byte = (zx_factor_reg << 1) & 0x80; + zxFactor = static_cast(zx_factor_reg | sign_byte); + return true; + } + + void setNormalConfig(MotionlessCalibrationData& gyroSensitivity) { + i2c.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value); + i2c.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value); + + i2c.writeReg(Regs::AccConf::reg, Regs::AccConf::value); + i2c.writeReg(Regs::AccRange::reg, Regs::AccRange::value); + + if (gyroSensitivity.valid) { + i2c.writeReg(Regs::Offset6::reg, Regs::Offset6::value); + i2c.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x); + } + + i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); + delay(100); // power up delay + i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); + i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); + + delay(4); + i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush); + delay(2); + } + + bool initialize(MotionlessCalibrationData& gyroSensitivity) { + if (!restartAndInit()) { + return false; + } + + setNormalConfig(gyroSensitivity); + + return true; + } + + void motionlessCalibration(MotionlessCalibrationData& gyroSensitivity) { + // perfrom gyroscope motionless sensitivity calibration (CRT) + // need to start from clean state according to spec + restartAndInit(); + // only Accel ON + i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn); + delay(100); + i2c.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning); + i2c.writeReg(Regs::FeatPage, 1); + i2c.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT); + i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger); + delay(200); + + while (i2c.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) { + logger.info("CRT running. Do not move tracker!"); + delay(200); + } + + i2c.writeReg(Regs::FeatPage, 0); + + uint8_t status = i2c.readReg(Regs::GyrGainStatus::reg) + >> Regs::GyrGainStatus::statusOffset; + // turn gyroscope back on + i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); + delay(100); + + if (status != 0) { + logger.error( + "CRT failed with status 0x%x. Recalibrate again to enable CRT.", + status + ); + if (status == 0x03) { + logger.error("Reason: tracker was moved during CRT!"); + } + } else { + std::array crt_values; + i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data()); + logger.debug( + "CRT finished successfully, result 0x%x, 0x%x, 0x%x", + crt_values[0], + crt_values[1], + crt_values[2] + ); + gyroSensitivity.valid = true; + gyroSensitivity.x = crt_values[0]; + gyroSensitivity.y = crt_values[1]; + gyroSensitivity.z = crt_values[2]; + } + + setNormalConfig(gyroSensitivity); + } + + float getDirectTemp() const { + // middle value is 23 degrees C (0x0000) + // temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9 + // degrees C (0x7FFF) + constexpr float TempStep = 128. / 65535; + const auto value = static_cast(i2c.readReg16(Regs::TempData)); + return static_cast(value) * TempStep + 23.0f; + } + + using FifoBuffer = std::array; + FifoBuffer read_buffer; + + template + inline T getFromFifo(uint32_t& position, FifoBuffer& fifo) { + T to_ret; + std::memcpy(&to_ret, &fifo[position], sizeof(T)); + position += sizeof(T); + return to_ret; + } + + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TemperatureCall&& processTemperatureSample + ) { + const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); + + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(fifo_bytes) + ); + i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); + + for (uint32_t i = 0u; i < bytes_to_read;) { + const uint8_t header = getFromFifo(i, read_buffer); + if ((header & Fifo::ModeMask) == Fifo::SkipFrame + && (i - bytes_to_read) >= 1) { + getFromFifo(i, read_buffer); // skip 1 byte + } else if ((header & Fifo::ModeMask) == Fifo::DataFrame) { + const uint8_t required_length + = (((header & Fifo::GyrDataBit) >> Fifo::GyrDataBit) + + ((header & Fifo::AccelDataBit) >> Fifo::AccelDataBit)) + * 6; + if (i - bytes_to_read < required_length) { + // incomplete frame, will be re-read next time + break; + } + if (header & Fifo::GyrDataBit) { + int16_t gyro[3]; + gyro[0] = getFromFifo(i, read_buffer); + gyro[1] = getFromFifo(i, read_buffer); + gyro[2] = getFromFifo(i, read_buffer); + using ShortLimit = std::numeric_limits; + // apply zx factor, todo: this awful line should be simplified and + // validated + gyro[0] = std::clamp( + static_cast(gyro[0]) + - static_cast( + (static_cast(zxFactor) * gyro[2]) / 512 + ), + static_cast(ShortLimit::min()), + static_cast(ShortLimit::max()) + ); + processGyroSample(gyro, GyrTs); + } + + if (header & Fifo::AccelDataBit) { + int16_t accel[3]; + accel[0] = getFromFifo(i, read_buffer); + accel[1] = getFromFifo(i, read_buffer); + accel[2] = getFromFifo(i, read_buffer); + processAccelSample(accel, AccTs); + } + } + } + } }; -} // namespace \ 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 5a5e0a9db..5088cc691 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -1,34 +1,33 @@ /* - 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. + 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 +#include +#include -namespace SlimeVR::Sensors::SoftFusion::Drivers -{ +namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 8g // and gyroscope range at 1000dps @@ -36,129 +35,143 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers // Timestamps reading not used, as they're useless (constant predefined increment) template -struct ICM42688 -{ - static constexpr uint8_t Address = 0x68; - static constexpr auto Name = "ICM-42688"; - static constexpr auto Type = ImuID::ICM42688; - - static constexpr float GyrTs=1.0/500.0; - static constexpr float AccTs=1.0/100.0; - - static constexpr float MagTs=1.0/100; - - static constexpr float GyroSensitivity = 32.8f; - static constexpr float AccelSensitivity = 8192.0f; - - I2CImpl i2c; - SlimeVR::Logging::Logger &logger; - ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : i2c(i2c), logger(logger) {} - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x75; - static constexpr uint8_t value = 0x47; - }; - static constexpr uint8_t TempData = 0x1d; - - struct DeviceConfig { - static constexpr uint8_t reg = 0x11; - static constexpr uint8_t valueSwReset = 1; - }; - struct IntfConfig0 { - static constexpr uint8_t reg = 0x4c; - static constexpr uint8_t value = (0 << 4) | (0 << 5) | (0 << 6); //fifo count in LE, sensor data in LE, fifo size in bytes - }; - struct FifoConfig0 { - static constexpr uint8_t reg = 0x16; - static constexpr uint8_t value = (0b01 << 6); //stream to FIFO mode - }; - 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 - }; - struct GyroConfig { - static constexpr uint8_t reg = 0x4f; - static constexpr uint8_t value = (0b001 << 5) | 0b1111; //1000dps, odr=500Hz - }; - struct AccelConfig { - static constexpr uint8_t reg = 0x50; - static constexpr uint8_t value = (0b010 << 5) | 0b1000; //4g, odr = 100Hz - }; - struct PwrMgmt { - static constexpr uint8_t reg = 0x4e; - static constexpr uint8_t value = 0b11 | (0b11 << 2); //accel in low noise mode, gyro in low noise - }; - - // TODO: might be worth checking - //GYRO_CONFIG1 - //GYRO_ACCEL_CONFIG0 - //ACCEL_CONFIG1 - - static constexpr uint8_t FifoCount = 0x2e; - static constexpr uint8_t FifoData = 0x30; - }; - - #pragma pack(push, 1) - struct FifoEntryAligned { - union { - struct { - int16_t accel[3]; - int16_t gyro[3]; - uint8_t temp; - uint8_t timestamp[2]; // cannot do uint16_t because it's unaligned - } part; - uint8_t raw[15]; - }; - }; - #pragma pack(pop) - - static constexpr size_t FullFifoEntrySize = 16; - - bool initialize() - { - // perform initialization step - i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); - delay(20); - - i2c.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value); - i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); - i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); - i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); - i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); - i2c.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value); - delay(1); - - 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) { - const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); - - 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(Regs::FifoData, bytes_to_read, read_buffer.data()); - for (auto i=0u; i + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TemperatureCall&& processTemperatureSample + ) { + const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); + + 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(Regs::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 + processGyroSample(entry.part.gyro, GyrTs); + + if (entry.part.accel[0] != -32768) { + processAccelSample(entry.part.accel, AccTs); + } + + processTemperatureSample(static_cast(entry.part.temp), TempTs); + } + } }; -} // namespace \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6ds-common.h b/src/sensors/softfusion/drivers/lsm6ds-common.h index 2615cfb12..cd100f701 100644 --- a/src/sensors/softfusion/drivers/lsm6ds-common.h +++ b/src/sensors/softfusion/drivers/lsm6ds-common.h @@ -1,99 +1,108 @@ /* - 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. + 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 -namespace SlimeVR::Sensors::SoftFusion::Drivers -{ +namespace SlimeVR::Sensors::SoftFusion::Drivers { template -struct LSM6DSOutputHandler -{ - LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : i2c(i2c), logger(logger) - {} - - 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 { - int16_t xyz[3]; - uint8_t raw[6]; - }; - }; - #pragma pack(pop) - - static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; - - template - void bulkRead(AccelCall &processAccelSample, GyroCall &processGyroSample, float GyrTs, float AccTs) { - constexpr auto FIFO_SAMPLES_MASK = 0x3ff; - constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800; - - const auto fifo_status = i2c.readReg16(Regs::FifoStatus); - const auto available_axes = fifo_status & FIFO_SAMPLES_MASK; - const auto fifo_bytes = available_axes * FullFifoEntrySize; - if (fifo_status & FIFO_OVERRUN_LATCHED_MASK) { - // FIFO overrun is expected to happen during startup and calibration - logger.error("FIFO OVERRUN! This occuring during normal usage is an issue."); - } - - 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(Regs::FifoData, bytes_to_read, read_buffer.data()); - for (auto i=0u; i> 3; - memcpy(entry.raw, &read_buffer[i+0x1], sizeof(FifoEntryAligned)); // skip fifo header - - switch (tag) { - case 0x01: // Gyro NC - processGyroSample(entry.xyz, GyrTs); - break; - case 0x02: // Accel NC - processAccelSample(entry.xyz, AccTs); - break; - } - } - } - - +struct LSM6DSOutputHandler { + LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + +#pragma pack(push, 1) + struct FifoEntryAligned { + union { + int16_t xyz[3]; + uint8_t raw[6]; + }; + }; +#pragma pack(pop) + + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; + + template < + typename AccelCall, + typename GyroCall, + typename TemperatureCall, + typename Regs> + void bulkRead( + AccelCall& processAccelSample, + GyroCall& processGyroSample, + TemperatureCall& processTemperatureSample, + float GyrTs, + float AccTs, + float TempTs + ) { + constexpr auto FIFO_SAMPLES_MASK = 0x3ff; + constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800; + + const auto fifo_status = i2c.readReg16(Regs::FifoStatus); + const auto available_axes = fifo_status & FIFO_SAMPLES_MASK; + const auto fifo_bytes = available_axes * FullFifoEntrySize; + if (fifo_status & FIFO_OVERRUN_LATCHED_MASK) { + // FIFO overrun is expected to happen during startup and calibration + logger.error("FIFO OVERRUN! This occuring during normal usage is an issue." + ); + } + + 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(Regs::FifoData, bytes_to_read, read_buffer.data()); + for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) { + FifoEntryAligned entry; + uint8_t tag = read_buffer[i] >> 3; + memcpy( + entry.raw, + &read_buffer[i + 0x1], + sizeof(FifoEntryAligned) + ); // skip fifo header + + switch (tag) { + case 0x01: // Gyro NC + processGyroSample(entry.xyz, GyrTs); + break; + case 0x02: // Accel NC + processAccelSample(entry.xyz, AccTs); + break; + case 0x03: // Temperature + processTemperatureSample(entry.xyz[0], TempTs); + break; + } + } + } }; -} // namespace \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index 1c05ad6fe..63f798271 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -1,139 +1,158 @@ /* - 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. + 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 +#include +#include -namespace SlimeVR::Sensors::SoftFusion::Drivers -{ +namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 8g // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 416Hz template -struct LSM6DS3TRC -{ - static constexpr uint8_t Address = 0x6a; - static constexpr auto Name = "LSM6DS3TR-C"; - static constexpr auto Type = ImuID::LSM6DS3TRC; - - 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 GyroSensitivity = 28.571428571f; - static constexpr float AccelSensitivity = 8196.72131148f; - - I2CImpl i2c; - SlimeVR::Logging::Logger logger; - LSM6DS3TRC(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 = 0x6a; - }; - static constexpr uint8_t OutTemp = 0x20; - struct Ctrl1XL { - static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value = (0b10 << 2) | (0b0110 << 4); //4g, 416Hz - }; - 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 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; - } - - 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) { - 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 = 6; - 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 - 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(&read_buffer[i]), GyrTs); - processAccelSample(reinterpret_cast(&read_buffer[i+3]), AccTs); - } - } - - +struct LSM6DS3TRC { + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DS3TR-C"; + static constexpr auto Type = ImuID::LSM6DS3TRC; + + 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; + + I2CImpl i2c; + SlimeVR::Logging::Logger logger; + LSM6DS3TRC(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 = 0x6a; + }; + static constexpr uint8_t OutTemp = 0x20; + struct Ctrl1XL { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b10 << 2) | (0b0110 << 4); // 4g, 416Hz + }; + 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 \ 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 a42467a5a..649610000 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -1,120 +1,130 @@ /* - 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. + 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 #include "lsm6ds-common.h" -namespace SlimeVR::Sensors::SoftFusion::Drivers -{ +namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 4g // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 208Hz template -struct LSM6DSO : LSM6DSOutputHandler -{ - static constexpr uint8_t Address = 0x6a; - static constexpr auto Name = "LSM6DSO"; - static constexpr auto Type = ImuID::LSM6DSO; - - static constexpr float GyrFreq = 416; - static constexpr float AccFreq = 208; - static constexpr float MagFreq = 120; - - static constexpr float GyrTs=1.0/GyrFreq; - static constexpr float AccTs=1.0/AccFreq; - static constexpr float MagTs=1.0/MagFreq; - - static constexpr float GyroSensitivity = 1000 / 35.0f; - static constexpr float AccelSensitivity = 1000 / 0.122f; - - using LSM6DSOutputHandler::i2c; - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x0f; - static constexpr uint8_t value = 0x6c; - }; - static constexpr uint8_t OutTemp = 0x20; - struct Ctrl1XL { - static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value = (0b01011000); // XL at 208 Hz, 4g FS - }; - struct Ctrl2GY { - static constexpr uint8_t reg = 0x11; - static constexpr uint8_t value = (0b01101000); //GY at 416 Hz, 1000dps FS - }; - 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 FifoCtrl3BDR { - static constexpr uint8_t reg = 0x09; - 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 FifoStatus = 0x3a; - static constexpr uint8_t FifoData = 0x78; - }; - - LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : LSM6DSOutputHandler(i2c, logger) { - } - - 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::Ctrl2GY::reg, Regs::Ctrl2GY::value); - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); - i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); - i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); - return true; - } - - float getDirectTemp() const - { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); - } - +struct LSM6DSO : LSM6DSOutputHandler { + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DSO"; + static constexpr auto Type = ImuID::LSM6DSO; + + static constexpr float GyrFreq = 416; + 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.122f; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + + using LSM6DSOutputHandler::i2c; + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x6c; + }; + static constexpr uint8_t OutTemp = 0x20; + struct Ctrl1XL { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b01011000); // XL at 208 Hz, 4g FS + }; + struct Ctrl2GY { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value = (0b01101000); // GY at 416 Hz, 1000dps FS + }; + 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 FifoCtrl3BDR { + static constexpr uint8_t reg = 0x09; + 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 = (0b110110); // continuous mode, + // temperature at 52Hz + }; + + static constexpr uint8_t FifoStatus = 0x3a; + static constexpr uint8_t FifoData = 0x78; + }; + + LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : LSM6DSOutputHandler(i2c, logger) {} + + 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::Ctrl2GY::reg, Regs::Ctrl2GY::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + return true; + } + + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TemperatureCall&& processTemperatureSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTemperatureSample, + GyrTs, + AccTs, + TempTs + ); + } }; -} // namespace \ 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 5530547cd..0a82e684b 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -1,120 +1,130 @@ /* - 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. + 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 #include "lsm6ds-common.h" -namespace SlimeVR::Sensors::SoftFusion::Drivers -{ +namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 8g // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 104Hz template -struct LSM6DSR : LSM6DSOutputHandler -{ - static constexpr uint8_t Address = 0x6a; - static constexpr auto Name = "LSM6DSR"; - static constexpr auto Type = ImuID::LSM6DSR; - - static constexpr float GyrFreq = 416; - static constexpr float AccFreq = 104; - static constexpr float MagFreq = 120; - - static constexpr float GyrTs=1.0/GyrFreq; - static constexpr float AccTs=1.0/AccFreq; - static constexpr float MagTs=1.0/MagFreq; - - static constexpr float GyroSensitivity = 1000 / 35.0f; - static constexpr float AccelSensitivity = 1000 / 0.122f; - - using LSM6DSOutputHandler::i2c; - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x0f; - static constexpr uint8_t value = 0x6b; - }; - static constexpr uint8_t OutTemp = 0x20; - struct Ctrl1XL { - static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value = (0b01001000); // XL at 104 Hz, 4g FS - }; - struct Ctrl2GY { - static constexpr uint8_t reg = 0x11; - static constexpr uint8_t value = (0b01101000); //GY at 416 Hz, 1000dps FS - }; - 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 FifoCtrl3BDR { - static constexpr uint8_t reg = 0x09; - static constexpr uint8_t value = (0b0110) | (0b0110 << 4); //gyro and accel batched at 417Hz - }; - struct FifoCtrl4Mode { - static constexpr uint8_t reg = 0x0a; - static constexpr uint8_t value = (0b110); //continuous mode - }; - - static constexpr uint8_t FifoStatus = 0x3a; - static constexpr uint8_t FifoData = 0x78; - }; - - LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : LSM6DSOutputHandler(i2c, logger) { - } - - 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::Ctrl2GY::reg, Regs::Ctrl2GY::value); - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); - i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); - i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); - return true; - } - - float getDirectTemp() const - { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); - } - +struct LSM6DSR : LSM6DSOutputHandler { + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DSR"; + static constexpr auto Type = ImuID::LSM6DSR; + + 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.122f; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + + using LSM6DSOutputHandler::i2c; + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x6b; + }; + static constexpr uint8_t OutTemp = 0x20; + struct Ctrl1XL { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b01001000); // XL at 104 Hz, 4g FS + }; + struct Ctrl2GY { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value = (0b01101000); // GY at 416 Hz, 1000dps FS + }; + 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 FifoCtrl3BDR { + static constexpr uint8_t reg = 0x09; + static constexpr uint8_t value + = (0b0110) | (0b0110 << 4); // gyro and accel batched at 417Hz + }; + struct FifoCtrl4Mode { + static constexpr uint8_t reg = 0x0a; + static constexpr uint8_t value = (0b110110); // continuous mode, + // temperature at 52Hz + }; + + static constexpr uint8_t FifoStatus = 0x3a; + static constexpr uint8_t FifoData = 0x78; + }; + + LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : LSM6DSOutputHandler(i2c, logger) {} + + 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::Ctrl2GY::reg, Regs::Ctrl2GY::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + return true; + } + + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TemperatureCall&& processTemperatureSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTemperatureSample, + GyrTs, + AccTs, + TempTs + ); + } }; -} // namespace \ 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 7501ef33d..0d2cdc08f 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -1,135 +1,145 @@ /* - 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. + 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 #include "lsm6ds-common.h" -namespace SlimeVR::Sensors::SoftFusion::Drivers -{ +namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 4g // and gyroscope range at 1000dps // Gyroscope ODR = 480Hz, accel ODR = 240Hz template -struct LSM6DSV : LSM6DSOutputHandler -{ - static constexpr uint8_t Address = 0x6a; - static constexpr auto Name = "LSM6DSV"; - static constexpr auto Type = ImuID::LSM6DSV; - - static constexpr float GyrFreq = 480; - static constexpr float AccFreq = 240; - static constexpr float MagFreq = 120; - - static constexpr float GyrTs=1.0/GyrFreq; - static constexpr float AccTs=1.0/AccFreq; - static constexpr float MagTs=1.0/MagFreq; - - static constexpr float GyroSensitivity = 1000 / 35.0f; - static constexpr float AccelSensitivity = 1000 / 0.122f; - - using LSM6DSOutputHandler::i2c; - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x0f; - static constexpr uint8_t value = 0x70; - }; - static constexpr uint8_t OutTemp = 0x20; - struct HAODRCFG { - static constexpr uint8_t reg = 0x62; - static constexpr uint8_t value = (0b00); //1st ODR table - }; - struct Ctrl1XLODR { - static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value = (0b0010111); //240Hz, HAODR - }; - struct Ctrl2GODR { - static constexpr uint8_t reg = 0x11; - static constexpr uint8_t value = (0b0011000); //480Hz, HAODR - }; - 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 Ctrl6GFS { - static constexpr uint8_t reg = 0x15; - static constexpr uint8_t value = (0b0011); //1000dps - }; - struct Ctrl8XLFS { - static constexpr uint8_t reg = 0x17; - static constexpr uint8_t value = (0b01); //4g - }; - struct FifoCtrl3BDR { - static constexpr uint8_t reg = 0x09; - 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 FifoStatus = 0x1b; - static constexpr uint8_t FifoData = 0x78; - }; - - LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : LSM6DSOutputHandler(i2c, logger) { - } - - bool initialize() - { - // perform initialization step - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); - delay(20); - i2c.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value); - i2c.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value); - i2c.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value); - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); - i2c.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value); - i2c.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value); - i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); - i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); - return true; - } - - float getDirectTemp() const - { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); - } - +struct LSM6DSV : LSM6DSOutputHandler { + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DSV"; + static constexpr auto Type = ImuID::LSM6DSV; + + static constexpr float GyrFreq = 480; + 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.122f; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 256.0f; + + using LSM6DSOutputHandler::i2c; + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x70; + }; + static constexpr uint8_t OutTemp = 0x20; + struct HAODRCFG { + static constexpr uint8_t reg = 0x62; + static constexpr uint8_t value = (0b00); // 1st ODR table + }; + struct Ctrl1XLODR { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b0010111); // 240Hz, HAODR + }; + struct Ctrl2GODR { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value = (0b0011000); // 480Hz, HAODR + }; + 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 Ctrl6GFS { + static constexpr uint8_t reg = 0x15; + static constexpr uint8_t value = (0b0011); // 1000dps + }; + struct Ctrl8XLFS { + static constexpr uint8_t reg = 0x17; + static constexpr uint8_t value = (0b01); // 4g + }; + struct FifoCtrl3BDR { + static constexpr uint8_t reg = 0x09; + 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 = (0b110110); // continuous mode, + // temperature at 60Hz + }; + + static constexpr uint8_t FifoStatus = 0x1b; + static constexpr uint8_t FifoData = 0x78; + }; + + LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : LSM6DSOutputHandler(i2c, logger) {} + + bool initialize() { + // perform initialization step + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + delay(20); + i2c.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value); + i2c.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value); + i2c.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value); + i2c.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value); + i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + return true; + } + + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TemperatureCall&& processTemperatureSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTemperatureSample, + GyrTs, + AccTs, + TempTs + ); + } }; -} // namespace \ 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 aebca8201..1d82c865e 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -1,185 +1,214 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 furrycoding & 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. + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 furrycoding & 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 +#include +#include -namespace SlimeVR::Sensors::SoftFusion::Drivers -{ +namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 8g // and gyroscope range at 1000dps // Gyroscope ODR = accel ODR = 250Hz template -struct MPU6050 -{ - struct FifoSample { - uint8_t accel_x_h, accel_x_l; - uint8_t accel_y_h, accel_y_l; - uint8_t accel_z_h, accel_z_l; - - // We don't need temperature in FIFO - // uint8_t temp_h, temp_l; - - uint8_t gyro_x_h, gyro_x_l; - uint8_t gyro_y_h, gyro_y_l; - uint8_t gyro_z_h, gyro_z_l; - }; - #define MPU6050_FIFO_VALUE(fifo, name) (((int16_t)fifo->name##_h << 8) | ((int16_t)fifo->name##_l)) - - static constexpr uint8_t Address = 0x68; - static constexpr auto Name = "MPU-6050"; - static constexpr auto Type = ImuID::MPU6050; - - static constexpr float Freq = 250; - - static constexpr float GyrTs = 1.0 / Freq; - static constexpr float AccTs = 1.0 / Freq; - static constexpr float MagTs = 1.0 / Freq; - - static constexpr float GyroSensitivity = 32.8f; - static constexpr float AccelSensitivity = 8192.0f; - - I2CImpl i2c; - SlimeVR::Logging::Logger &logger; - MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : i2c(i2c), logger(logger) {} - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x75; - static constexpr uint8_t value = 0x68; - }; - - struct UserCtrl { - static constexpr uint8_t reg = 0x6A; - static constexpr uint8_t fifoResetValue = (1 << MPU6050_USERCTRL_FIFO_EN_BIT) | (1 << MPU6050_USERCTRL_FIFO_RESET_BIT); - }; - - struct GyroConfig { - static constexpr uint8_t reg = 0x1b; - static constexpr uint8_t value = 0b10 << 3; // 1000dps - }; - - struct AccelConfig { - static constexpr uint8_t reg = 0x1c; - static constexpr uint8_t value = 0b01 << 3; // 4g - }; - - static constexpr uint8_t OutTemp = MPU6050_RA_TEMP_OUT_H; - - static constexpr uint8_t IntStatus = MPU6050_RA_INT_STATUS; - - static constexpr uint8_t FifoCount = MPU6050_RA_FIFO_COUNTH; - static constexpr uint8_t FifoData = MPU6050_RA_FIFO_R_W; - }; - - inline uint16_t byteSwap(uint16_t value) const { - // Swap bytes because MPU is big-endian - return (value >> 8) | (value << 8); - } - - void resetFIFO() { - i2c.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue); - } - - bool initialize() - { - // Reset - i2c.writeReg(MPU6050_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1: reset with 100ms delay (also disables sleep) - delay(100); - i2c.writeReg(MPU6050_RA_SIGNAL_PATH_RESET, 0x07); // full SIGNAL_PATH_RESET: with another 100ms delay - delay(100); - - // Configure - i2c.writeReg(MPU6050_RA_PWR_MGMT_1, 0x01); // 0000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro - i2c.writeReg(MPU6050_RA_USER_CTRL, 0x00); // 0000 0000 USER_CTRL: Disable FIFO / I2C master / DMP - i2c.writeReg(MPU6050_RA_INT_ENABLE, 0x10); // 0001 0000 INT_ENABLE: only FIFO overflow interrupt - i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); - i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); - i2c.writeReg(MPU6050_RA_CONFIG, 0x02); // 0000 0010 CONFIG: No EXT_SYNC_SET, DLPF set to 98Hz(also lowers gyro output rate to 1KHz) - i2c.writeReg(MPU6050_RA_SMPLRT_DIV, 0x03); // 0000 0011 SMPLRT_DIV: Divides the internal sample rate 250Hz (Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)) - - i2c.writeReg(MPU6050_RA_FIFO_EN, 0x78); // 0111 1000 FIFO_EN: All gyro axes + Accel - - resetFIFO(); - - return true; - } - - float getDirectTemp() const - { - auto value = byteSwap(i2c.readReg16(Regs::OutTemp)); - float result = (static_cast(value) / 340.0f) + 36.53f; - return result; - } - - template - void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - const auto status = i2c.readReg(Regs::IntStatus); - - if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) { - // Overflows make it so we lose track of which packet is which - // This necessitates a reset - logger.debug("Fifo overrun, resetting..."); - resetFIFO(); - return; - } - - std::array readBuffer; // max 10 packages of 12byte values (sample) of data form fifo - auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount)); - - auto readBytes = min(static_cast(byteCount), readBuffer.size()) / sizeof(FifoSample) * sizeof(FifoSample); - if (!readBytes) { - return; - } - - i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data()); - for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) { - const FifoSample *sample = reinterpret_cast(&readBuffer[i]); - - int16_t xyz[3]; - - xyz[0] = MPU6050_FIFO_VALUE(sample, accel_x); - xyz[1] = MPU6050_FIFO_VALUE(sample, accel_y); - xyz[2] = MPU6050_FIFO_VALUE(sample, accel_z); - processAccelSample(xyz, AccTs); - - xyz[0] = MPU6050_FIFO_VALUE(sample, gyro_x); - xyz[1] = MPU6050_FIFO_VALUE(sample, gyro_y); - xyz[2] = MPU6050_FIFO_VALUE(sample, gyro_z); - processGyroSample(xyz, GyrTs); - } - } - - +struct MPU6050 { + struct FifoSample { + uint8_t accel_x_h, accel_x_l; + uint8_t accel_y_h, accel_y_l; + uint8_t accel_z_h, accel_z_l; + + // We don't need temperature in FIFO + // uint8_t temp_h, temp_l; + + uint8_t gyro_x_h, gyro_x_l; + uint8_t gyro_y_h, gyro_y_l; + uint8_t gyro_z_h, gyro_z_l; + }; +#define MPU6050_FIFO_VALUE(fifo, name) \ + (((int16_t)fifo->name##_h << 8) | ((int16_t)fifo->name##_l)) + + static constexpr uint8_t Address = 0x68; + static constexpr auto Name = "MPU-6050"; + static constexpr auto Type = ImuID::MPU6050; + + static constexpr float Freq = 250; + + static constexpr float GyrTs = 1.0 / Freq; + static constexpr float AccTs = 1.0 / Freq; + static constexpr float MagTs = 1.0 / Freq; + + static constexpr float GyroSensitivity = 32.8f; + static constexpr float AccelSensitivity = 8192.0f; + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x75; + static constexpr uint8_t value = 0x68; + }; + + struct UserCtrl { + static constexpr uint8_t reg = 0x6A; + static constexpr uint8_t fifoResetValue + = (1 << MPU6050_USERCTRL_FIFO_EN_BIT) + | (1 << MPU6050_USERCTRL_FIFO_RESET_BIT); + }; + + struct GyroConfig { + static constexpr uint8_t reg = 0x1b; + static constexpr uint8_t value = 0b10 << 3; // 1000dps + }; + + struct AccelConfig { + static constexpr uint8_t reg = 0x1c; + static constexpr uint8_t value = 0b01 << 3; // 4g + }; + + static constexpr uint8_t OutTemp = MPU6050_RA_TEMP_OUT_H; + + static constexpr uint8_t IntStatus = MPU6050_RA_INT_STATUS; + + static constexpr uint8_t FifoCount = MPU6050_RA_FIFO_COUNTH; + static constexpr uint8_t FifoData = MPU6050_RA_FIFO_R_W; + }; + + inline uint16_t byteSwap(uint16_t value) const { + // Swap bytes because MPU is big-endian + return (value >> 8) | (value << 8); + } + + void resetFIFO() { + i2c.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue); + } + + bool initialize() { + // Reset + i2c.writeReg( + MPU6050_RA_PWR_MGMT_1, + 0x80 + ); // PWR_MGMT_1: reset with 100ms delay (also disables sleep) + delay(100); + i2c.writeReg( + MPU6050_RA_SIGNAL_PATH_RESET, + 0x07 + ); // full SIGNAL_PATH_RESET: with another 100ms delay + delay(100); + + // Configure + i2c.writeReg( + MPU6050_RA_PWR_MGMT_1, + 0x01 + ); // 0000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro + i2c.writeReg( + MPU6050_RA_USER_CTRL, + 0x00 + ); // 0000 0000 USER_CTRL: Disable FIFO / I2C master / DMP + i2c.writeReg( + MPU6050_RA_INT_ENABLE, + 0x10 + ); // 0001 0000 INT_ENABLE: only FIFO overflow interrupt + i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); + i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); + i2c.writeReg( + MPU6050_RA_CONFIG, + 0x02 + ); // 0000 0010 CONFIG: No EXT_SYNC_SET, DLPF set to 98Hz(also lowers gyro + // output rate to 1KHz) + i2c.writeReg( + MPU6050_RA_SMPLRT_DIV, + 0x03 + ); // 0000 0011 SMPLRT_DIV: Divides the internal sample rate 250Hz (Sample Rate + // = Gyroscope Output Rate / (1 + SMPLRT_DIV)) + + i2c.writeReg( + MPU6050_RA_FIFO_EN, + 0x78 + ); // 0111 1000 FIFO_EN: All gyro axes + Accel + + resetFIFO(); + + return true; + } + + float getDirectTemp() const { + auto value = byteSwap(i2c.readReg16(Regs::OutTemp)); + float result = (static_cast(value) / 340.0f) + 36.53f; + return result; + } + + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TemperatureCall&& processTemperatureSample + ) { + const auto status = i2c.readReg(Regs::IntStatus); + + if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) { + // Overflows make it so we lose track of which packet is which + // This necessitates a reset + logger.debug("Fifo overrun, resetting..."); + resetFIFO(); + return; + } + + std::array + readBuffer; // max 10 packages of 12byte values (sample) of data form fifo + auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount)); + + auto readBytes = min(static_cast(byteCount), readBuffer.size()) + / sizeof(FifoSample) * sizeof(FifoSample); + if (!readBytes) { + return; + } + + i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data()); + for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) { + const FifoSample* sample = reinterpret_cast(&readBuffer[i]); + + int16_t xyz[3]; + + xyz[0] = MPU6050_FIFO_VALUE(sample, accel_x); + xyz[1] = MPU6050_FIFO_VALUE(sample, accel_y); + xyz[2] = MPU6050_FIFO_VALUE(sample, accel_z); + processAccelSample(xyz, AccTs); + + xyz[0] = MPU6050_FIFO_VALUE(sample, gyro_x); + xyz[1] = MPU6050_FIFO_VALUE(sample, gyro_y); + xyz[2] = MPU6050_FIFO_VALUE(sample, gyro_z); + processGyroSample(xyz, GyrTs); + } + } }; -} // namespace +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 20bcd58ee..b13248253 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -1,531 +1,664 @@ /* - 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. + 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 "../sensor.h" #include "../SensorFusionRestDetect.h" - +#include "../sensor.h" #include "GlobalVars.h" -namespace SlimeVR::Sensors -{ - -template typename T, typename I2CImpl> -class SoftFusionSensor : public Sensor -{ - using imu = T; - using RawVectorT = std::array; - static constexpr auto UpsideDownCalibrationInit = false; - static constexpr auto GyroCalibDelaySeconds = 5; - static constexpr auto GyroCalibSeconds = 5; - static constexpr auto SampleRateCalibDelaySeconds = 1; - static constexpr auto SampleRateCalibSeconds = 5; - - static constexpr auto AccelCalibDelaySeconds = 3; - static constexpr auto AccelCalibRestSeconds = 3; - - static constexpr double GScale = ((32768. / imu::GyroSensitivity) / 32768.) * (PI / 180.0); - static constexpr double AScale = CONST_EARTH_GRAVITY / imu::AccelSensitivity; - - 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; - } - } - - bool detected() const - { - const auto value = m_sensor.i2c.readReg(imu::Regs::WhoAmI::reg); - if (imu::Regs::WhoAmI::value != value) { - m_Logger.error("Sensor not detected, expected reg 0x%02x = 0x%02x but got 0x%02x", - imu::Regs::WhoAmI::reg, imu::Regs::WhoAmI::value, value); - return false; - } - - return true; - } - - - void sendTempIfNeeded() - { - uint32_t now = micros(); - constexpr float maxSendRateHz = 2.0f; - constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6; - uint32_t elapsed = now - m_lastTemperaturePacketSent; - if (elapsed >= sendInterval) { - const float temperature = m_sensor.getDirectTemp(); - m_lastTemperaturePacketSent = now - (elapsed - sendInterval); - networkConnection.sendTemperature(sensorId, temperature); - } - } - - void recalcFusion() - { - m_fusion = SensorFusionRestDetect(m_calibration.G_Ts, m_calibration.A_Ts, m_calibration.M_Ts); - } - - void processAccelSample(const int16_t xyz[3], const sensor_real_t timeDelta) - { - sensor_real_t accelData[] = { - static_cast(xyz[0]), - static_cast(xyz[1]), - static_cast(xyz[2]) }; - - float tmp[3]; - for (uint8_t i = 0; i < 3; i++) - tmp[i] = (accelData[i] - m_calibration.A_B[i]); - - accelData[0] = (m_calibration.A_Ainv[0][0] * tmp[0] + m_calibration.A_Ainv[0][1] * tmp[1] + m_calibration.A_Ainv[0][2] * tmp[2]) * AScale; - accelData[1] = (m_calibration.A_Ainv[1][0] * tmp[0] + m_calibration.A_Ainv[1][1] * tmp[1] + m_calibration.A_Ainv[1][2] * tmp[2]) * AScale; - accelData[2] = (m_calibration.A_Ainv[2][0] * tmp[0] + m_calibration.A_Ainv[2][1] * tmp[1] + m_calibration.A_Ainv[2][2] * tmp[2]) * AScale; - - m_fusion.updateAcc(accelData, m_calibration.A_Ts); - } - - void processGyroSample(const int16_t xyz[3], const sensor_real_t timeDelta) - { - const sensor_real_t scaledData[] = { - static_cast(GScale * (static_cast(xyz[0]) - m_calibration.G_off[0])), - static_cast(GScale * (static_cast(xyz[1]) - m_calibration.G_off[1])), - static_cast(GScale * (static_cast(xyz[2]) - m_calibration.G_off[2]))}; - m_fusion.updateGyro(scaledData, m_calibration.G_Ts); - } - - void eatSamplesForSeconds(const uint32_t seconds) { - const auto targetDelay = millis() + 1000 * seconds; - auto lastSecondsRemaining = seconds; - while (millis() < targetDelay) - { - #ifdef ESP8266 - ESP.wdtFeed(); - #endif - auto currentSecondsRemaining = (targetDelay - millis()) / 1000; - if (currentSecondsRemaining != lastSecondsRemaining) { - m_Logger.info("%d...", currentSecondsRemaining + 1); - lastSecondsRemaining = currentSecondsRemaining; - } - m_sensor.bulkRead( - [](const int16_t xyz[3], const sensor_real_t timeDelta) { }, - [](const int16_t xyz[3], const sensor_real_t timeDelta) { } - ); - } - } - - std::pair eatSamplesReturnLast(const uint32_t milliseconds) - { - RawVectorT accel = {0}; - RawVectorT gyro = {0}; - const auto targetDelay = millis() + milliseconds; - while (millis() < targetDelay) - { - m_sensor.bulkRead( - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { - accel[0] = xyz[0]; - accel[1] = xyz[1]; - accel[2] = xyz[2]; - }, - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { - gyro[0] = xyz[0]; - gyro[1] = xyz[1]; - gyro[2] = xyz[2]; - } - ); - } - return std::make_pair(accel, gyro); - } +namespace SlimeVR::Sensors { + +template