From 8a63326267c4853fad7ea66d636eed6f99f4b0e8 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 25 Sep 2024 21:09:51 +0200 Subject: [PATCH 1/7] In theory this works? --- src/sensors/softfusion/drivers/icm45686.h | 163 ++++++++++++++++++++++ 1 file changed, 163 insertions(+) create mode 100644 src/sensors/softfusion/drivers/icm45686.h diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h new file mode 100644 index 000000000..b5a7dac3f --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -0,0 +1,163 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include +#include +#include + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 8g +// and gyroscope range at 1000dps +// Gyroscope ODR = 400Hz, accel ODR = 100Hz +// 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-45688"; + static constexpr auto Type = ImuID::ICM45686; + + static constexpr float GyrTs = 1.0 / 400.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 = 4096.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 = 0x72; + static constexpr uint8_t value = 0xe9; + }; + static constexpr uint8_t TempData = 0x0c; + + struct DeviceConfig { + static constexpr uint8_t reg = 0x7f; + static constexpr uint8_t valueSwReset = 1; + }; + struct FifoConfig0 { + static constexpr uint8_t reg = 0x1d; + static constexpr uint8_t value + = (0b01 << 6) | (0b000111); // stream to FIFO mode, FIFO depth + // 2k bytes + }; + struct FifoConfig3 { + static constexpr uint8_t reg = 0x21; + static constexpr uint8_t value + = (0b1 << 1) | (0b1 << 2); // fifo accel en=1, gyro=1, todo: hires + }; + struct GyroConfig { + static constexpr uint8_t reg = 0x1c; + static constexpr uint8_t value + = (0b0010 << 4) | 0b0111; // 1000dps, odr=400Hz + }; + struct AccelConfig { + static constexpr uint8_t reg = 0x1b; + static constexpr uint8_t value = (0b010 << 4) | 0b1001; // 8g, odr = 100Hz + }; + struct PwrMgmt0 { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value + = 0b11 | (0b11 << 2); // accel in low noise mode, gyro in low noise + }; + + static constexpr uint8_t FifoCount = 0x12; + static constexpr uint8_t FifoData = 0x14; + }; + +#pragma pack(push, 1) + struct FifoEntryAligned { + union { + struct { + int16_t accel[3]; + int16_t gyro[3]; + 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::GyroConfig::reg, Regs::GyroConfig::value); + i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); + i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); + i2c.writeReg(Regs::FifoConfig3::reg, Regs::FifoConfig3::value); + i2c.writeReg(Regs::PwrMgmt0::reg, Regs::PwrMgmt0::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_packets = i2c.readReg16(Regs::FifoCount); + const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); + + std::array read_buffer; // max 8 readings + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(fifo_bytes) + ) + / FullFifoEntrySize * FullFifoEntrySize; + i2c.readBytes(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); + } + } + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers From 46802488aacbd1b457187179ba773addd1b9c062 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 2 Oct 2024 09:02:21 +0200 Subject: [PATCH 2/7] ICM45686 implementation --- src/consts.h | 2 + src/sensors/SensorManager.cpp | 2 + src/sensors/sensor.cpp | 2 + src/sensors/softfusion/drivers/icm45686.h | 98 ++++-- src/sensors/softfusion/softfusionsensor.h | 363 ++++++++++++++-------- 5 files changed, 306 insertions(+), 161 deletions(-) diff --git a/src/consts.h b/src/consts.h index a6640879a..989e0a6d9 100644 --- a/src/consts.h +++ b/src/consts.h @@ -42,6 +42,7 @@ enum class ImuID { LSM6DSV, LSM6DSO, LSM6DSR, + ICM45686, Empty = 255 }; @@ -62,6 +63,7 @@ enum class ImuID { #define IMU_LSM6DSO SoftFusionLSM6DSO #define IMU_LSM6DSR SoftFusionLSM6DSR #define IMU_MPU6050_SF SoftFusionMPU6050 +#define IMU_ICM45686 SoftFusionICM45686 #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 1c1dc9cc7..b437d07ee 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -38,6 +38,7 @@ #include "softfusion/drivers/lsm6dso.h" #include "softfusion/drivers/lsm6dsr.h" #include "softfusion/drivers/mpu6050.h" +#include "softfusion/drivers/icm45686.h" #include "softfusion/i2cimpl.h" @@ -56,6 +57,7 @@ namespace SlimeVR using SoftFusionLSM6DSO = SoftFusionSensor; using SoftFusionLSM6DSR = SoftFusionSensor; using SoftFusionMPU6050 = SoftFusionSensor; + using SoftFusionICM45686 = SoftFusionSensor; // TODO Make it more generic in the future and move another place (abstract sensor interface) void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin) diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 94299f12c..60aefe79e 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -101,6 +101,8 @@ const char * getIMUNameByType(ImuID imuType) { return "LSM6DSO"; case ImuID::LSM6DSR: return "LSM6DSR"; + case ImuID::ICM45686: + return "ICM45686"; case ImuID::Unknown: case ImuID::Empty: return "UNKNOWN"; diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h index b5a7dac3f..73e9344dd 100644 --- a/src/sensors/softfusion/drivers/icm45686.h +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -29,28 +29,32 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { -// Driver uses acceleration range at 8g -// and gyroscope range at 1000dps -// Gyroscope ODR = 400Hz, accel ODR = 100Hz +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz // Timestamps reading not used, as they're useless (constant predefined increment) template -struct ICM42688 { +struct ICM45686 { static constexpr uint8_t Address = 0x68; static constexpr auto Name = "ICM-45688"; static constexpr auto Type = ImuID::ICM45686; - static constexpr float GyrTs = 1.0 / 400.0; - static constexpr float AccTs = 1.0 / 100.0; + static constexpr float GyrTs = 1.0 / 409.6; + static constexpr float AccTs = 1.0 / 102.4; static constexpr float MagTs = 1.0 / 100; - static constexpr float GyroSensitivity = 32.8f; - static constexpr float AccelSensitivity = 4096.0f; + static constexpr float GyroSensitivity = 131.072f; + static constexpr float AccelSensitivity = 16384.0f; + + static constexpr bool Uses32BitSensorData = true; I2CImpl i2c; SlimeVR::Logging::Logger& logger; - ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger) : i2c(i2c) , logger(logger) {} @@ -65,26 +69,46 @@ struct ICM42688 { static constexpr uint8_t reg = 0x7f; static constexpr uint8_t valueSwReset = 1; }; - struct FifoConfig0 { - static constexpr uint8_t reg = 0x1d; - static constexpr uint8_t value - = (0b01 << 6) | (0b000111); // stream to FIFO mode, FIFO depth - // 2k bytes + + struct Pin9Config { + static constexpr uint8_t reg = 0x31; + static constexpr uint8_t value = 0b00000110; // pin 9 to clkin }; - struct FifoConfig3 { - static constexpr uint8_t reg = 0x21; - static constexpr uint8_t value - = (0b1 << 1) | (0b1 << 2); // fifo accel en=1, gyro=1, todo: hires + + struct RtcConfig { + static constexpr uint8_t reg = 0x26; + static constexpr uint8_t value = 0b00100011; // enable RTC }; + struct GyroConfig { static constexpr uint8_t reg = 0x1c; static constexpr uint8_t value - = (0b0010 << 4) | 0b0111; // 1000dps, odr=400Hz + = (0b0000 << 4) | 0b0111; // 4000dps, odr=409.6Hz }; + struct AccelConfig { static constexpr uint8_t reg = 0x1b; - static constexpr uint8_t value = (0b010 << 4) | 0b1001; // 8g, odr = 100Hz + static constexpr uint8_t value + = (0b000 << 4) | 0b1001; // 32g, odr = 102.4Hz }; + + struct FifoConfig0 { + static constexpr uint8_t reg = 0x1d; + static constexpr uint8_t value + = (0b01 << 6) | (0b011111); // stream to FIFO mode, FIFO depth + // 8k bytes <-- this disables all APEX + // features, but we don't need them + }; + + struct FifoConfig3 { + static constexpr uint8_t reg = 0x21; + static constexpr uint8_t value = (0b1 << 0) | (0b1 << 1) | (0b1 << 2) + | (0b1 << 3); // enable FIFO, + // enable accel, + // enable gyro, + // enable hires mode + }; + struct PwrMgmt0 { static constexpr uint8_t reg = 0x10; static constexpr uint8_t value @@ -101,21 +125,23 @@ struct ICM42688 { struct { int16_t accel[3]; int16_t gyro[3]; - uint8_t temp; - uint8_t timestamp[2]; // cannot do uint16_t because it's unaligned + uint16_t temp; + uint16_t timestamp; + uint8_t lsb[3]; } part; - uint8_t raw[15]; + uint8_t raw[19]; }; }; #pragma pack(pop) - static constexpr size_t FullFifoEntrySize = 16; + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; bool initialize() { // perform initialization step i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); - delay(20); - + delay(35); + i2c.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value); + i2c.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::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); @@ -151,10 +177,26 @@ struct ICM42688 { &read_buffer[i + 0x1], sizeof(FifoEntryAligned) ); // skip fifo header - processGyroSample(entry.part.gyro, GyrTs); + const int32_t gyroData[3]{ + static_cast(entry.part.gyro[0]) << 4 + | (entry.part.lsb[0] & 0xf), + static_cast(entry.part.gyro[1]) << 4 + | (entry.part.lsb[1] & 0xf), + static_cast(entry.part.gyro[2]) << 4 + | (entry.part.lsb[2] & 0xf), + }; + processGyroSample(gyroData, GyrTs); if (entry.part.accel[0] != -32768) { - processAccelSample(entry.part.accel, AccTs); + const int32_t accelData[3]{ + static_cast(entry.part.accel[0]) << 4 + | (static_cast(entry.part.lsb[0]) & 0xf0 >> 4), + static_cast(entry.part.accel[1]) << 4 + | (static_cast(entry.part.lsb[1]) & 0xf0 >> 4), + static_cast(entry.part.accel[2]) << 4 + | (static_cast(entry.part.lsb[2]) & 0xf0 >> 4), + }; + processAccelSample(accelData, AccTs); } } } diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index ec076a757..4ceafd0a9 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -23,19 +23,26 @@ #pragma once -#include "../sensor.h" -#include "../SensorFusionRestDetect.h" +#include +#include +#include "../SensorFusionRestDetect.h" +#include "../sensor.h" #include "GlobalVars.h" -namespace SlimeVR::Sensors -{ +namespace SlimeVR::Sensors { template typename T, typename I2CImpl> -class SoftFusionSensor : public Sensor -{ +class SoftFusionSensor : public Sensor { using imu = T; - using RawVectorT = std::array; + + static constexpr bool Uses32BitSensorData + = requires(imu& i) { i.Uses32BitSensorData; }; + + using RawSensorT = + typename std::conditional::type; + using RawVectorT = std::array; + static constexpr auto UpsideDownCalibrationInit = true; static constexpr auto GyroCalibDelaySeconds = 5; static constexpr auto GyroCalibSeconds = 5; @@ -45,34 +52,36 @@ class SoftFusionSensor : public Sensor static constexpr auto AccelCalibDelaySeconds = 3; static constexpr auto AccelCalibRestSeconds = 3; - static constexpr double GScale = ((32768. / imu::GyroSensitivity) / 32768.) * (PI / 180.0); + 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 bool HasMotionlessCalib + = requires(imu& i) { typename imu::MotionlessCalibrationData; }; static constexpr size_t MotionlessCalibDataSize() { if constexpr(HasMotionlessCalib) { return sizeof(typename imu::MotionlessCalibrationData); - } - else { + } else { return 0; } } - bool detected() const - { + 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); + 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() - { + void sendTempIfNeeded() { uint32_t now = micros(); constexpr float maxSendRateHz = 2.0f; constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6; @@ -84,43 +93,59 @@ class SoftFusionSensor : public Sensor } } - void recalcFusion() - { - m_fusion = SensorFusionRestDetect(m_calibration.G_Ts, m_calibration.A_Ts, m_calibration.M_Ts); + 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]), + void processAccelSample(const RawSensorT 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++) + 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; + } + + 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) - { + void processGyroSample(const RawSensorT 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]))}; + 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) - { + while (millis() < targetDelay) { #ifdef ESP8266 ESP.wdtFeed(); #endif @@ -129,27 +154,27 @@ class SoftFusionSensor : public Sensor 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) { } + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {} ); } } - std::pair eatSamplesReturnLast(const uint32_t milliseconds) - { + std::pair eatSamplesReturnLast(const uint32_t milliseconds + ) { RawVectorT accel = {0}; RawVectorT gyro = {0}; const auto targetDelay = millis() + milliseconds; - while (millis() < targetDelay) - { + while (millis() < targetDelay) { m_sensor.bulkRead( - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { + [&](const RawSensorT 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) { + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { gyro[0] = xyz[0]; gyro[1] = xyz[1]; gyro[2] = xyz[2]; @@ -163,13 +188,28 @@ class SoftFusionSensor : public Sensor static constexpr auto TypeID = imu::Type; static constexpr uint8_t Address = imu::Address; - SoftFusionSensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t) - : Sensor(imu::Name, imu::Type, id, imu::Address + addrSuppl, rotation, sclPin, sdaPin), - m_fusion(imu::GyrTs, imu::AccTs, imu::MagTs), m_sensor(I2CImpl(imu::Address + addrSuppl), m_Logger) {} + SoftFusionSensor( + uint8_t id, + uint8_t addrSuppl, + float rotation, + uint8_t sclPin, + uint8_t sdaPin, + uint8_t + ) + : Sensor( + imu::Name, + imu::Type, + id, + imu::Address + addrSuppl, + rotation, + sclPin, + sdaPin + ) + , m_fusion(imu::GyrTs, imu::AccTs, imu::MagTs) + , m_sensor(I2CImpl(imu::Address + addrSuppl), m_Logger) {} ~SoftFusionSensor(){} - void motionLoop() override final - { + void motionLoop() override final { sendTempIfNeeded(); // read fifo updating fusion @@ -179,16 +219,21 @@ class SoftFusionSensor : public Sensor if (elapsed >= targetPollIntervalMicros) { m_lastPollTime = now - (elapsed - targetPollIntervalMicros); m_sensor.bulkRead( - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { processAccelSample(xyz, timeDelta); }, - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { processGyroSample(xyz, timeDelta); } + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + processAccelSample(xyz, timeDelta); + }, + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + processGyroSample(xyz, timeDelta); + } ); optimistic_yield(100); - if (!m_fusion.isUpdated()) return; + if (!m_fusion.isUpdated()) { + return; + } hadData = true; m_fusion.clearUpdated(); } - // send new fusion values when time is up now = micros(); constexpr float maxSendRateHz = 120.0f; @@ -203,28 +248,35 @@ class SoftFusionSensor : public Sensor } } - void motionSetup() override final - { + void motionSetup() override final { if (!detected()) { m_status = SensorStatus::SENSOR_ERROR; return; } - SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + SlimeVR::Configuration::CalibrationConfig sensorCalibration + = configuration.getCalibration(sensorId); - // If no compatible calibration data is found, the calibration data will just be zero-ed out - if (sensorCalibration.type == SlimeVR::Configuration::CalibrationConfigType::SFUSION + // If no compatible calibration data is found, the calibration data will just be + // zero-ed out + if (sensorCalibration.type + == SlimeVR::Configuration::CalibrationConfigType::SFUSION && (sensorCalibration.data.sfusion.ImuType == imu::Type) - && (sensorCalibration.data.sfusion.MotionlessDataLen == MotionlessCalibDataSize())) { + && (sensorCalibration.data.sfusion.MotionlessDataLen + == MotionlessCalibDataSize())) { m_calibration = sensorCalibration.data.sfusion; recalcFusion(); - } - else if (sensorCalibration.type == SlimeVR::Configuration::CalibrationConfigType::NONE) { - m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); + } else if (sensorCalibration.type == SlimeVR::Configuration::CalibrationConfigType::NONE) { + m_Logger.warn( + "No calibration data found for sensor %d, ignoring...", + sensorId + ); m_Logger.info("Calibration is advised"); - } - else { - m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); + } else { + m_Logger.warn( + "Incompatible calibration data found for sensor %d, ignoring...", + sensorId + ); m_Logger.info("Please recalibrate"); } @@ -234,8 +286,7 @@ class SoftFusionSensor : public Sensor typename imu::MotionlessCalibrationData calibData; std::memcpy(&calibData, m_calibration.MotionlessData, sizeof(calibData)); initResult = m_sensor.initialize(calibData); - } - else { + } else { initResult = m_sensor.initialize(); } @@ -249,18 +300,24 @@ class SoftFusionSensor : public Sensor working = true; [[maybe_unused]] auto lastRawSample = eatSamplesReturnLast(1000); if constexpr(UpsideDownCalibrationInit) { - auto gravity = static_cast(AScale * static_cast(lastRawSample.first[2])); - m_Logger.info("Gravity read: %.1f (need < -7.5 to start calibration)", gravity); + auto gravity = static_cast( + AScale * static_cast(lastRawSample.first[2]) + ); + m_Logger.info( + "Gravity read: %.1f (need < -7.5 to start calibration)", + gravity + ); if (gravity < -7.5f) { ledManager.on(); m_Logger.info("Flip front in 5 seconds to start calibration"); lastRawSample = eatSamplesReturnLast(5000); - gravity = static_cast(AScale * static_cast(lastRawSample.first[2])); + gravity = static_cast( + AScale * static_cast(lastRawSample.first[2]) + ); if (gravity > 7.5f) { m_Logger.debug("Starting calibration..."); startCalibration(0); - } - else { + } else { m_Logger.info("Flip not detected. Skipping calibration."); } @@ -269,8 +326,7 @@ class SoftFusionSensor : public Sensor } } - void startCalibration(int calibrationType) override final - { + void startCalibration(int calibrationType) override final { if (calibrationType == 0) { // ALL calibrateSampleRate(); @@ -278,37 +334,38 @@ class SoftFusionSensor : public Sensor if constexpr(HasMotionlessCalib) { typename imu::MotionlessCalibrationData calibData; m_sensor.motionlessCalibration(calibData); - std::memcpy(m_calibration.MotionlessData, &calibData, sizeof(calibData)); + std::memcpy( + m_calibration.MotionlessData, + &calibData, + sizeof(calibData) + ); } calibrateAccel(); - } - else if (calibrationType == 1) - { + } else if (calibrationType == 1) { calibrateSampleRate(); - } - else if (calibrationType == 2) - { + } else if (calibrationType == 2) { calibrateGyroOffset(); - } - else if (calibrationType == 3) - { + } else if (calibrationType == 3) { calibrateAccel(); - } - else if (calibrationType == 4) { + } else if (calibrationType == 4) { if constexpr(HasMotionlessCalib) { typename imu::MotionlessCalibrationData calibData; m_sensor.motionlessCalibration(calibData); - std::memcpy(m_calibration.MotionlessData, &calibData, sizeof(calibData)); + std::memcpy( + m_calibration.MotionlessData, + &calibData, + sizeof(calibData) + ); } else { - m_Logger.info("Sensor doesn't provide any custom motionless calibration"); + m_Logger.info("Sensor doesn't provide any custom motionless calibration" + ); } } saveCalibration(); } - void saveCalibration() - { + void saveCalibration() { m_Logger.debug("Saving the calibration data"); SlimeVR::Configuration::CalibrationConfig calibration; calibration.type = SlimeVR::Configuration::CalibrationConfigType::SFUSION; @@ -317,10 +374,13 @@ class SoftFusionSensor : public Sensor configuration.save(); } - void calibrateGyroOffset() - { + void calibrateGyroOffset() { // Wait for sensor to calm down before calibration - m_Logger.info("Put down the device and wait for baseline gyro reading calibration (%d seconds)", GyroCalibDelaySeconds); + m_Logger.info( + "Put down the device and wait for baseline gyro reading calibration (%d " + "seconds)", + GyroCalibDelaySeconds + ); ledManager.on(); eatSamplesForSeconds(GyroCalibDelaySeconds); ledManager.off(); @@ -336,14 +396,14 @@ class SoftFusionSensor : public Sensor const auto targetCalib = millis() + 1000 * GyroCalibSeconds; uint32_t sampleCount = 0; - while (millis() < targetCalib) - { + while (millis() < targetCalib) { #ifdef ESP8266 ESP.wdtFeed(); #endif m_sensor.bulkRead( - [](const int16_t xyz[3], const sensor_real_t timeDelta) { }, - [&sumXYZ, &sampleCount](const int16_t xyz[3], const sensor_real_t timeDelta) { + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [&sumXYZ, + &sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) { sumXYZ[0] += xyz[0]; sumXYZ[1] += xyz[1]; sumXYZ[2] += xyz[2]; @@ -357,13 +417,20 @@ class SoftFusionSensor : public Sensor m_calibration.G_off[1] = ((double)sumXYZ[1]) / sampleCount; m_calibration.G_off[2] = ((double)sumXYZ[2]) / sampleCount; - m_Logger.info("Gyro offset after %d samples: %f %f %f", sampleCount, UNPACK_VECTOR_ARRAY(m_calibration.G_off)); + m_Logger.info( + "Gyro offset after %d samples: %f %f %f", + sampleCount, + UNPACK_VECTOR_ARRAY(m_calibration.G_off) + ); } - void calibrateAccel() - { + void calibrateAccel() { auto magneto = std::make_unique(); - m_Logger.info("Put the device into 6 unique orientations (all sides), leave it still and do not hold/touch for %d seconds each", AccelCalibRestSeconds); + m_Logger.info( + "Put the device into 6 unique orientations (all sides), leave it still and " + "do not hold/touch for %d seconds each", + AccelCalibRestSeconds + ); ledManager.on(); eatSamplesForSeconds(AccelCalibDelaySeconds); ledManager.off(); @@ -385,22 +452,32 @@ class SoftFusionSensor : public Sensor uint16_t numCurrentPositionSamples = 0; bool waitForMotion = true; - auto accelCalibrationChunk = std::make_unique(numSamplesPerPosition * 3); + auto accelCalibrationChunk + = std::make_unique(numSamplesPerPosition * 3); ledManager.pattern(100, 100, 6); ledManager.on(); m_Logger.info("Gathering accelerometer data..."); - m_Logger.info("Waiting for position %i, you can leave the device as is...", numPositionsRecorded + 1); + m_Logger.info( + "Waiting for position %i, you can leave the device as is...", + numPositionsRecorded + 1 + ); bool samplesGathered = false; while (!samplesGathered) { #ifdef ESP8266 ESP.wdtFeed(); #endif m_sensor.bulkRead( - [&](const int16_t xyz[3], const sensor_real_t timeDelta) { - const sensor_real_t scaledData[] = { - static_cast(AScale * static_cast(xyz[0])), - static_cast(AScale * static_cast(xyz[1])), - static_cast(AScale * static_cast(xyz[2]))}; + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + const sensor_real_t scaledData[] + = {static_cast( + AScale * static_cast(xyz[0]) + ), + static_cast( + AScale * static_cast(xyz[1]) + ), + static_cast( + AScale * static_cast(xyz[2]) + )}; calibrationRestDetection.updateAcc(imu::AccTs, scaledData); if (waitForMotion) { @@ -430,7 +507,10 @@ class SoftFusionSensor : public Sensor if (numPositionsRecorded < expectedPositions) { ledManager.pattern(50, 50, 2); ledManager.on(); - m_Logger.info("Recorded, waiting for position %i...", numPositionsRecorded + 1); + m_Logger.info( + "Recorded, waiting for position %i...", + numPositionsRecorded + 1 + ); waitForMotion = true; } } @@ -438,12 +518,11 @@ class SoftFusionSensor : public Sensor numCurrentPositionSamples = 0; } - if (numPositionsRecorded >= expectedPositions) - { + if (numPositionsRecorded >= expectedPositions) { samplesGathered = true; } }, - [](const int16_t xyz[3], const sensor_real_t timeDelta) { } + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {} ); } ledManager.off(); @@ -461,14 +540,22 @@ class SoftFusionSensor : public Sensor m_calibration.A_Ainv[0][i] = A_BAinv[1][i]; m_calibration.A_Ainv[1][i] = A_BAinv[2][i]; m_calibration.A_Ainv[2][i] = A_BAinv[3][i]; - m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); + m_Logger.debug( + " %f, %f, %f, %f", + A_BAinv[0][i], + A_BAinv[1][i], + A_BAinv[2][i], + A_BAinv[3][i] + ); } m_Logger.debug("}"); } - void calibrateSampleRate() - { - m_Logger.debug("Calibrating IMU sample rate in %d second(s)...", SampleRateCalibDelaySeconds); + void calibrateSampleRate() { + m_Logger.debug( + "Calibrating IMU sample rate in %d second(s)...", + SampleRateCalibDelaySeconds + ); ledManager.on(); eatSamplesForSeconds(SampleRateCalibDelaySeconds); @@ -478,35 +565,46 @@ class SoftFusionSensor : public Sensor const auto calibTarget = millis() + 1000 * SampleRateCalibSeconds; m_Logger.debug("Counting samples now..."); uint32_t currentTime; - while ((currentTime = millis()) < calibTarget) - { + while ((currentTime = millis()) < calibTarget) { m_sensor.bulkRead( - [&accelSamples](const int16_t xyz[3], const sensor_real_t timeDelta) { accelSamples++; }, - [&gyroSamples](const int16_t xyz[3], const sensor_real_t timeDelta) { gyroSamples++; } + [&accelSamples]( + const RawSensorT xyz[3], + const sensor_real_t timeDelta + ) { accelSamples++; }, + [&gyroSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + gyroSamples++; + } ); } - const auto millisFromStart = currentTime - (calibTarget - 1000 * SampleRateCalibSeconds); - m_Logger.debug("Collected %d gyro, %d acc samples during %d ms", gyroSamples, accelSamples, millisFromStart); + const auto millisFromStart + = currentTime - (calibTarget - 1000 * SampleRateCalibSeconds); + m_Logger.debug( + "Collected %d gyro, %d acc samples during %d ms", + gyroSamples, + accelSamples, + millisFromStart + ); m_calibration.A_Ts = millisFromStart / (accelSamples * 1000.0); m_calibration.G_Ts = millisFromStart / (gyroSamples * 1000.0) ; - m_Logger.debug("Gyro frequency %fHz, accel frequency: %fHz", 1.0/m_calibration.G_Ts, 1.0/m_calibration.A_Ts); + m_Logger.debug( + "Gyro frequency %fHz, accel frequency: %fHz", + 1.0 / m_calibration.G_Ts, + 1.0 / m_calibration.A_Ts + ); ledManager.off(); //fusion needs to be recalculated recalcFusion(); } - SensorStatus getSensorState() override final - { - return m_status; - } + SensorStatus getSensorState() override final { return m_status; } SensorFusionRestDetect m_fusion; T m_sensor; - SlimeVR::Configuration::SoftFusionCalibrationConfig m_calibration = { - // let's create here transparent calibration that doesn't affect input data + SlimeVR::Configuration::SoftFusionCalibrationConfig m_calibration + = {// let's create here transparent calibration that doesn't affect input data .ImuType = {imu::Type}, .MotionlessDataLen = {MotionlessCalibDataSize()}, .A_B = {0.0, 0.0, 0.0}, @@ -519,8 +617,7 @@ class SoftFusionSensor : public Sensor .G_Ts = imu::GyrTs, .M_Ts = imu::MagTs, .G_Sens = {1.0, 1.0, 1.0}, - .MotionlessData = {} - }; + .MotionlessData = {}}; SensorStatus m_status = SensorStatus::SENSOR_OFFLINE; uint32_t m_lastPollTime = micros(); @@ -528,4 +625,4 @@ class SoftFusionSensor : public Sensor uint32_t m_lastTemperaturePacketSent = 0; }; -} // namespace +} // namespace SlimeVR::Sensors From 4abf06f6996662918d0cee5cb415fedfb0e8fbe9 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sat, 5 Oct 2024 04:46:42 +0200 Subject: [PATCH 3/7] Remove unnecessary changes --- src/sensors/softfusion/softfusionsensor.h | 372 +++++++++------------- 1 file changed, 142 insertions(+), 230 deletions(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 4ceafd0a9..b9773f4d3 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -26,22 +26,23 @@ #include #include -#include "../SensorFusionRestDetect.h" #include "../sensor.h" +#include "../SensorFusionRestDetect.h" + #include "GlobalVars.h" -namespace SlimeVR::Sensors { +namespace SlimeVR::Sensors +{ template typename T, typename I2CImpl> -class SoftFusionSensor : public Sensor { +class SoftFusionSensor : public Sensor +{ using imu = T; - static constexpr bool Uses32BitSensorData - = requires(imu& i) { i.Uses32BitSensorData; }; + static constexpr bool Uses32BitSensorData = requires(imu& i){ i.Uses32BitSensorData; }; - using RawSensorT = - typename std::conditional::type; - using RawVectorT = std::array; + using RawSensorT = typename std::conditional::type; + using RawVectorT = std::array; static constexpr auto UpsideDownCalibrationInit = true; static constexpr auto GyroCalibDelaySeconds = 5; @@ -52,36 +53,34 @@ class SoftFusionSensor : public Sensor { static constexpr auto AccelCalibDelaySeconds = 3; static constexpr auto AccelCalibRestSeconds = 3; - static constexpr double GScale - = ((32768. / imu::GyroSensitivity) / 32768.) * (PI / 180.0); + 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 bool HasMotionlessCalib = requires(imu& i){ typename imu::MotionlessCalibrationData; }; static constexpr size_t MotionlessCalibDataSize() { if constexpr(HasMotionlessCalib) { return sizeof(typename imu::MotionlessCalibrationData); - } else { + } + else { return 0; } } - bool detected() const { + 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 - ); + 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() { + + void sendTempIfNeeded() + { uint32_t now = micros(); constexpr float maxSendRateHz = 2.0f; constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6; @@ -93,62 +92,46 @@ class SoftFusionSensor : public Sensor { } } - void recalcFusion() { - m_fusion = SensorFusionRestDetect( - m_calibration.G_Ts, - m_calibration.A_Ts, - m_calibration.M_Ts - ); + void recalcFusion() + { + m_fusion = SensorFusionRestDetect(m_calibration.G_Ts, m_calibration.A_Ts, m_calibration.M_Ts); } - void processAccelSample(const RawSensorT xyz[3], const sensor_real_t timeDelta) { - sensor_real_t accelData[] - = {static_cast(xyz[0]), + void processAccelSample(const RawSensorT 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++) { + 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; + + 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 RawSensorT xyz[3], const sensor_real_t timeDelta) { + void processGyroSample(const RawSensorT 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]) - )}; + 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) { + while (millis() < targetDelay) + { #ifdef ESP8266 - ESP.wdtFeed(); - #endif + ESP.wdtFeed(); + #endif auto currentSecondsRemaining = (targetDelay - millis()) / 1000; if (currentSecondsRemaining != lastSecondsRemaining) { m_Logger.info("%d...", currentSecondsRemaining + 1); @@ -156,25 +139,26 @@ class SoftFusionSensor : public Sensor { } m_sensor.bulkRead( - [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, - [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {} + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {} ); } } - std::pair eatSamplesReturnLast(const uint32_t milliseconds - ) { + std::pair eatSamplesReturnLast(const uint32_t milliseconds) + { RawVectorT accel = {0}; RawVectorT gyro = {0}; const auto targetDelay = millis() + milliseconds; - while (millis() < targetDelay) { + while (millis() < targetDelay) + { m_sensor.bulkRead( - [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { accel[0] = xyz[0]; accel[1] = xyz[1]; accel[2] = xyz[2]; }, - [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { gyro[0] = xyz[0]; gyro[1] = xyz[1]; gyro[2] = xyz[2]; @@ -188,28 +172,13 @@ class SoftFusionSensor : public Sensor { static constexpr auto TypeID = imu::Type; static constexpr uint8_t Address = imu::Address; - SoftFusionSensor( - uint8_t id, - uint8_t addrSuppl, - float rotation, - uint8_t sclPin, - uint8_t sdaPin, - uint8_t - ) - : Sensor( - imu::Name, - imu::Type, - id, - imu::Address + addrSuppl, - rotation, - sclPin, - sdaPin - ) - , m_fusion(imu::GyrTs, imu::AccTs, imu::MagTs) - , m_sensor(I2CImpl(imu::Address + addrSuppl), m_Logger) {} + SoftFusionSensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t) + : Sensor(imu::Name, imu::Type, id, imu::Address + addrSuppl, rotation, sclPin, sdaPin), + m_fusion(imu::GyrTs, imu::AccTs, imu::MagTs), m_sensor(I2CImpl(imu::Address + addrSuppl), m_Logger) {} ~SoftFusionSensor(){} - void motionLoop() override final { + void motionLoop() override final + { sendTempIfNeeded(); // read fifo updating fusion @@ -219,21 +188,16 @@ class SoftFusionSensor : public Sensor { if (elapsed >= targetPollIntervalMicros) { m_lastPollTime = now - (elapsed - targetPollIntervalMicros); m_sensor.bulkRead( - [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { - processAccelSample(xyz, timeDelta); - }, - [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { - processGyroSample(xyz, timeDelta); - } + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { processAccelSample(xyz, timeDelta); }, + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { processGyroSample(xyz, timeDelta); } ); optimistic_yield(100); - if (!m_fusion.isUpdated()) { - return; - } + if (!m_fusion.isUpdated()) return; hadData = true; m_fusion.clearUpdated(); } + // send new fusion values when time is up now = micros(); constexpr float maxSendRateHz = 120.0f; @@ -248,45 +212,39 @@ class SoftFusionSensor : public Sensor { } } - void motionSetup() override final { + void motionSetup() override final + { if (!detected()) { m_status = SensorStatus::SENSOR_ERROR; return; } - SlimeVR::Configuration::CalibrationConfig sensorCalibration - = configuration.getCalibration(sensorId); + SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); - // If no compatible calibration data is found, the calibration data will just be - // zero-ed out - if (sensorCalibration.type - == SlimeVR::Configuration::CalibrationConfigType::SFUSION + // If no compatible calibration data is found, the calibration data will just be zero-ed out + if (sensorCalibration.type == SlimeVR::Configuration::CalibrationConfigType::SFUSION && (sensorCalibration.data.sfusion.ImuType == imu::Type) - && (sensorCalibration.data.sfusion.MotionlessDataLen - == MotionlessCalibDataSize())) { + && (sensorCalibration.data.sfusion.MotionlessDataLen == MotionlessCalibDataSize())) { m_calibration = sensorCalibration.data.sfusion; recalcFusion(); - } else if (sensorCalibration.type == SlimeVR::Configuration::CalibrationConfigType::NONE) { - m_Logger.warn( - "No calibration data found for sensor %d, ignoring...", - sensorId - ); + } + else if (sensorCalibration.type == SlimeVR::Configuration::CalibrationConfigType::NONE) { + m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); m_Logger.info("Calibration is advised"); - } else { - m_Logger.warn( - "Incompatible calibration data found for sensor %d, ignoring...", - sensorId - ); + } + else { + m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); m_Logger.info("Please recalibrate"); } bool initResult = false; - + if constexpr(HasMotionlessCalib) { typename imu::MotionlessCalibrationData calibData; std::memcpy(&calibData, m_calibration.MotionlessData, sizeof(calibData)); initResult = m_sensor.initialize(calibData); - } else { + } + else { initResult = m_sensor.initialize(); } @@ -300,24 +258,18 @@ class SoftFusionSensor : public Sensor { working = true; [[maybe_unused]] auto lastRawSample = eatSamplesReturnLast(1000); if constexpr(UpsideDownCalibrationInit) { - auto gravity = static_cast( - AScale * static_cast(lastRawSample.first[2]) - ); - m_Logger.info( - "Gravity read: %.1f (need < -7.5 to start calibration)", - gravity - ); + auto gravity = static_cast(AScale * static_cast(lastRawSample.first[2])); + m_Logger.info("Gravity read: %.1f (need < -7.5 to start calibration)", gravity); if (gravity < -7.5f) { ledManager.on(); m_Logger.info("Flip front in 5 seconds to start calibration"); lastRawSample = eatSamplesReturnLast(5000); - gravity = static_cast( - AScale * static_cast(lastRawSample.first[2]) - ); + gravity = static_cast(AScale * static_cast(lastRawSample.first[2])); if (gravity > 7.5f) { m_Logger.debug("Starting calibration..."); startCalibration(0); - } else { + } + else { m_Logger.info("Flip not detected. Skipping calibration."); } @@ -326,7 +278,8 @@ class SoftFusionSensor : public Sensor { } } - void startCalibration(int calibrationType) override final { + void startCalibration(int calibrationType) override final + { if (calibrationType == 0) { // ALL calibrateSampleRate(); @@ -334,38 +287,37 @@ class SoftFusionSensor : public Sensor { if constexpr(HasMotionlessCalib) { typename imu::MotionlessCalibrationData calibData; m_sensor.motionlessCalibration(calibData); - std::memcpy( - m_calibration.MotionlessData, - &calibData, - sizeof(calibData) - ); + std::memcpy(m_calibration.MotionlessData, &calibData, sizeof(calibData)); } calibrateAccel(); - } else if (calibrationType == 1) { + } + else if (calibrationType == 1) + { calibrateSampleRate(); - } else if (calibrationType == 2) { + } + else if (calibrationType == 2) + { calibrateGyroOffset(); - } else if (calibrationType == 3) { + } + else if (calibrationType == 3) + { calibrateAccel(); - } else if (calibrationType == 4) { + } + else if (calibrationType == 4) { if constexpr(HasMotionlessCalib) { typename imu::MotionlessCalibrationData calibData; m_sensor.motionlessCalibration(calibData); - std::memcpy( - m_calibration.MotionlessData, - &calibData, - sizeof(calibData) - ); + std::memcpy(m_calibration.MotionlessData, &calibData, sizeof(calibData)); } else { - m_Logger.info("Sensor doesn't provide any custom motionless calibration" - ); + m_Logger.info("Sensor doesn't provide any custom motionless calibration"); } } saveCalibration(); } - void saveCalibration() { + void saveCalibration() + { m_Logger.debug("Saving the calibration data"); SlimeVR::Configuration::CalibrationConfig calibration; calibration.type = SlimeVR::Configuration::CalibrationConfigType::SFUSION; @@ -374,13 +326,10 @@ class SoftFusionSensor : public Sensor { configuration.save(); } - void calibrateGyroOffset() { + void calibrateGyroOffset() + { // Wait for sensor to calm down before calibration - m_Logger.info( - "Put down the device and wait for baseline gyro reading calibration (%d " - "seconds)", - GyroCalibDelaySeconds - ); + m_Logger.info("Put down the device and wait for baseline gyro reading calibration (%d seconds)", GyroCalibDelaySeconds); ledManager.on(); eatSamplesForSeconds(GyroCalibDelaySeconds); ledManager.off(); @@ -396,14 +345,14 @@ class SoftFusionSensor : public Sensor { const auto targetCalib = millis() + 1000 * GyroCalibSeconds; uint32_t sampleCount = 0; - while (millis() < targetCalib) { + while (millis() < targetCalib) + { #ifdef ESP8266 - ESP.wdtFeed(); - #endif + ESP.wdtFeed(); + #endif m_sensor.bulkRead( - [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, - [&sumXYZ, - &sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [&sumXYZ, &sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) { sumXYZ[0] += xyz[0]; sumXYZ[1] += xyz[1]; sumXYZ[2] += xyz[2]; @@ -417,20 +366,13 @@ class SoftFusionSensor : public Sensor { m_calibration.G_off[1] = ((double)sumXYZ[1]) / sampleCount; m_calibration.G_off[2] = ((double)sumXYZ[2]) / sampleCount; - m_Logger.info( - "Gyro offset after %d samples: %f %f %f", - sampleCount, - UNPACK_VECTOR_ARRAY(m_calibration.G_off) - ); + m_Logger.info("Gyro offset after %d samples: %f %f %f", sampleCount, UNPACK_VECTOR_ARRAY(m_calibration.G_off)); } - void calibrateAccel() { + void calibrateAccel() + { auto magneto = std::make_unique(); - m_Logger.info( - "Put the device into 6 unique orientations (all sides), leave it still and " - "do not hold/touch for %d seconds each", - AccelCalibRestSeconds - ); + m_Logger.info("Put the device into 6 unique orientations (all sides), leave it still and do not hold/touch for %d seconds each", AccelCalibRestSeconds); ledManager.on(); eatSamplesForSeconds(AccelCalibDelaySeconds); ledManager.off(); @@ -452,32 +394,22 @@ class SoftFusionSensor : public Sensor { uint16_t numCurrentPositionSamples = 0; bool waitForMotion = true; - auto accelCalibrationChunk - = std::make_unique(numSamplesPerPosition * 3); + auto accelCalibrationChunk = std::make_unique(numSamplesPerPosition * 3); ledManager.pattern(100, 100, 6); ledManager.on(); m_Logger.info("Gathering accelerometer data..."); - m_Logger.info( - "Waiting for position %i, you can leave the device as is...", - numPositionsRecorded + 1 - ); + m_Logger.info("Waiting for position %i, you can leave the device as is...", numPositionsRecorded + 1); bool samplesGathered = false; while (!samplesGathered) { #ifdef ESP8266 - ESP.wdtFeed(); - #endif + ESP.wdtFeed(); + #endif m_sensor.bulkRead( - [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { - const sensor_real_t scaledData[] - = {static_cast( - AScale * static_cast(xyz[0]) - ), - static_cast( - AScale * static_cast(xyz[1]) - ), - static_cast( - AScale * static_cast(xyz[2]) - )}; + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + const sensor_real_t scaledData[] = { + static_cast(AScale * static_cast(xyz[0])), + static_cast(AScale * static_cast(xyz[1])), + static_cast(AScale * static_cast(xyz[2]))}; calibrationRestDetection.updateAcc(imu::AccTs, scaledData); if (waitForMotion) { @@ -507,10 +439,7 @@ class SoftFusionSensor : public Sensor { if (numPositionsRecorded < expectedPositions) { ledManager.pattern(50, 50, 2); ledManager.on(); - m_Logger.info( - "Recorded, waiting for position %i...", - numPositionsRecorded + 1 - ); + m_Logger.info("Recorded, waiting for position %i...", numPositionsRecorded + 1); waitForMotion = true; } } @@ -518,11 +447,12 @@ class SoftFusionSensor : public Sensor { numCurrentPositionSamples = 0; } - if (numPositionsRecorded >= expectedPositions) { + if (numPositionsRecorded >= expectedPositions) + { samplesGathered = true; } }, - [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {} + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {} ); } ledManager.off(); @@ -540,22 +470,14 @@ class SoftFusionSensor : public Sensor { m_calibration.A_Ainv[0][i] = A_BAinv[1][i]; m_calibration.A_Ainv[1][i] = A_BAinv[2][i]; m_calibration.A_Ainv[2][i] = A_BAinv[3][i]; - m_Logger.debug( - " %f, %f, %f, %f", - A_BAinv[0][i], - A_BAinv[1][i], - A_BAinv[2][i], - A_BAinv[3][i] - ); + m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); } m_Logger.debug("}"); } - void calibrateSampleRate() { - m_Logger.debug( - "Calibrating IMU sample rate in %d second(s)...", - SampleRateCalibDelaySeconds - ); + void calibrateSampleRate() + { + m_Logger.debug("Calibrating IMU sample rate in %d second(s)...", SampleRateCalibDelaySeconds); ledManager.on(); eatSamplesForSeconds(SampleRateCalibDelaySeconds); @@ -565,46 +487,35 @@ class SoftFusionSensor : public Sensor { const auto calibTarget = millis() + 1000 * SampleRateCalibSeconds; m_Logger.debug("Counting samples now..."); uint32_t currentTime; - while ((currentTime = millis()) < calibTarget) { + while ((currentTime = millis()) < calibTarget) + { m_sensor.bulkRead( - [&accelSamples]( - const RawSensorT xyz[3], - const sensor_real_t timeDelta - ) { accelSamples++; }, - [&gyroSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) { - gyroSamples++; - } + [&accelSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) { accelSamples++; }, + [&gyroSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) { gyroSamples++; } ); } - const auto millisFromStart - = currentTime - (calibTarget - 1000 * SampleRateCalibSeconds); - m_Logger.debug( - "Collected %d gyro, %d acc samples during %d ms", - gyroSamples, - accelSamples, - millisFromStart - ); + const auto millisFromStart = currentTime - (calibTarget - 1000 * SampleRateCalibSeconds); + m_Logger.debug("Collected %d gyro, %d acc samples during %d ms", gyroSamples, accelSamples, millisFromStart); m_calibration.A_Ts = millisFromStart / (accelSamples * 1000.0); m_calibration.G_Ts = millisFromStart / (gyroSamples * 1000.0) ; - m_Logger.debug( - "Gyro frequency %fHz, accel frequency: %fHz", - 1.0 / m_calibration.G_Ts, - 1.0 / m_calibration.A_Ts - ); + m_Logger.debug("Gyro frequency %fHz, accel frequency: %fHz", 1.0/m_calibration.G_Ts, 1.0/m_calibration.A_Ts); ledManager.off(); //fusion needs to be recalculated recalcFusion(); } - SensorStatus getSensorState() override final { return m_status; } + SensorStatus getSensorState() override final + { + return m_status; + } SensorFusionRestDetect m_fusion; T m_sensor; - SlimeVR::Configuration::SoftFusionCalibrationConfig m_calibration - = {// let's create here transparent calibration that doesn't affect input data + SlimeVR::Configuration::SoftFusionCalibrationConfig m_calibration = { + // let's create here transparent calibration that doesn't affect input data .ImuType = {imu::Type}, .MotionlessDataLen = {MotionlessCalibDataSize()}, .A_B = {0.0, 0.0, 0.0}, @@ -617,7 +528,8 @@ class SoftFusionSensor : public Sensor { .G_Ts = imu::GyrTs, .M_Ts = imu::MagTs, .G_Sens = {1.0, 1.0, 1.0}, - .MotionlessData = {}}; + .MotionlessData = {} + }; SensorStatus m_status = SensorStatus::SENSOR_OFFLINE; uint32_t m_lastPollTime = micros(); @@ -625,4 +537,4 @@ class SoftFusionSensor : public Sensor { uint32_t m_lastTemperaturePacketSent = 0; }; -} // namespace SlimeVR::Sensors +} // namespace From 3769b4f30a91d3651ed390b8f8131d7656ee952c Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sat, 5 Oct 2024 05:07:48 +0200 Subject: [PATCH 4/7] 42688 hires maybe? --- src/sensors/softfusion/drivers/icm42688.h | 38 ++++++++++++++++------- 1 file changed, 27 insertions(+), 11 deletions(-) diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index e071db55a..769e34c89 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -50,6 +50,8 @@ struct ICM42688 static constexpr float GyroSensitivity = 32.8f; static constexpr float AccelSensitivity = 4096.0f; + static constexpr bool Uses32BitSensorData = true; + I2CImpl i2c; SlimeVR::Logging::Logger &logger; ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger &logger) @@ -76,7 +78,7 @@ struct ICM42688 }; struct FifoConfig1 { static constexpr uint8_t reg = 0x5f; - static constexpr uint8_t value = 0b1 | (0b1 << 1) | (0b0 << 2); //fifo accel en=1, gyro=1, temp=0 todo: fsync, hires + static constexpr uint8_t value = 0b1 | (0b1 << 1) | (0b0 << 2) | (0b0 << 4); //fifo accel en=1, gyro=1, temp=0, hires=1 }; struct GyroConfig { static constexpr uint8_t reg = 0x4f; @@ -106,15 +108,18 @@ struct ICM42688 struct { int16_t accel[3]; int16_t gyro[3]; - uint8_t temp; - uint8_t timestamp[2]; // cannot do uint16_t because it's unaligned + uint16_t temp; + uint16_t timestamp; // cannot do uint16_t because it's unaligned + uint8_t xlsb; + uint8_t ylsb; + uint8_t zlsb; } part; - uint8_t raw[15]; + uint8_t raw[19]; }; }; #pragma pack(pop) - static constexpr size_t FullFifoEntrySize = 16; + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; bool initialize() { @@ -143,7 +148,7 @@ struct ICM42688 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; @@ -151,14 +156,25 @@ struct ICM42688 for (auto i=0u; i(entry.part.gyro[0]) << 4 | (entry.part.xlsb & 0xf), + static_cast(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf), + static_cast(entry.part.gyro[2]) << 4 | (entry.part.zlsb & 0xf), + }; + processGyroSample(gyroData, GyrTs); if (entry.part.accel[0] != -32768) { - processAccelSample(entry.part.accel, AccTs); - } - } + const int32_t accelData[3]{ + static_cast(entry.part.accel[0]) << 4 | (static_cast(entry.part.xlsb) & 0xf0 >> 4), + static_cast(entry.part.accel[1]) << 4 | (static_cast(entry.part.ylsb) & 0xf0 >> 4), + static_cast(entry.part.accel[2]) << 4 | (static_cast(entry.part.zlsb) & 0xf0 >> 4), + }; + processAccelSample(accelData, AccTs); + } + } } }; -} // namespace \ No newline at end of file +} // namespace From bfd86f10c26559539059c634c1d0b993e5841018 Mon Sep 17 00:00:00 2001 From: kounocom Date: Wed, 2 Oct 2024 08:00:31 +0300 Subject: [PATCH 5/7] Make sensor reset work (lol) --- src/sensors/softfusion/drivers/icm45686.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h index 73e9344dd..51a95501a 100644 --- a/src/sensors/softfusion/drivers/icm45686.h +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -67,7 +67,7 @@ struct ICM45686 { struct DeviceConfig { static constexpr uint8_t reg = 0x7f; - static constexpr uint8_t valueSwReset = 1; + static constexpr uint8_t valueSwReset = 0b11; }; struct Pin9Config { From da18c7b1cf7092a6db87c937dd4344042d1758c0 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 7 Nov 2024 18:54:53 +0100 Subject: [PATCH 6/7] ICM45605 support --- src/consts.h | 2 + src/sensors/SensorManager.cpp | 4 +- src/sensors/softfusion/drivers/icm45605.h | 193 ++++++++++++++++++++++ 3 files changed, 198 insertions(+), 1 deletion(-) create mode 100644 src/sensors/softfusion/drivers/icm45605.h diff --git a/src/consts.h b/src/consts.h index 989e0a6d9..b3d9a89ff 100644 --- a/src/consts.h +++ b/src/consts.h @@ -43,6 +43,7 @@ enum class ImuID { LSM6DSO, LSM6DSR, ICM45686, + ICM45605, Empty = 255 }; @@ -64,6 +65,7 @@ enum class ImuID { #define IMU_LSM6DSR SoftFusionLSM6DSR #define IMU_MPU6050_SF SoftFusionMPU6050 #define IMU_ICM45686 SoftFusionICM45686 +#define IMU_ICM45605 SoftFusionICM45605 #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index b437d07ee..f875671af 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -39,6 +39,7 @@ #include "softfusion/drivers/lsm6dsr.h" #include "softfusion/drivers/mpu6050.h" #include "softfusion/drivers/icm45686.h" +#include "softfusion/drivers/icm45605.h" #include "softfusion/i2cimpl.h" @@ -58,6 +59,7 @@ namespace SlimeVR using SoftFusionLSM6DSR = SoftFusionSensor; using SoftFusionMPU6050 = SoftFusionSensor; using SoftFusionICM45686 = SoftFusionSensor; + using SoftFusionICM45605 = SoftFusionSensor; // TODO Make it more generic in the future and move another place (abstract sensor interface) void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin) @@ -174,7 +176,7 @@ namespace SlimeVR m_LastBundleSentAtMicros = now; #endif - + #if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED networkConnection.beginBundle(); #endif diff --git a/src/sensors/softfusion/drivers/icm45605.h b/src/sensors/softfusion/drivers/icm45605.h new file mode 100644 index 000000000..241200806 --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -0,0 +1,193 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include +#include +#include + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45605 { + static constexpr uint8_t Address = 0x68; + static constexpr auto Name = "ICM-45605"; + static constexpr auto Type = ImuID::ICM45605; + + static constexpr float GyrTs = 1.0 / 409.6; + static constexpr float AccTs = 1.0 / 102.4; + + static constexpr float MagTs = 1.0 / 100; + + static constexpr float GyroSensitivity = 131.072f; + static constexpr float AccelSensitivity = 16384.0f; + + static constexpr bool Uses32BitSensorData = true; + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x72; + static constexpr uint8_t value = 0xe5; + }; + static constexpr uint8_t TempData = 0x0c; + + struct DeviceConfig { + static constexpr uint8_t reg = 0x7f; + static constexpr uint8_t valueSwReset = 0b11; + }; + + struct GyroConfig { + static constexpr uint8_t reg = 0x1c; + static constexpr uint8_t value + = (0b0000 << 4) | 0b0111; // 4000dps, odr=409.6Hz + }; + + struct AccelConfig { + static constexpr uint8_t reg = 0x1b; + static constexpr uint8_t value + = (0b000 << 4) | 0b1001; // 32g, odr = 102.4Hz + }; + + struct FifoConfig0 { + static constexpr uint8_t reg = 0x1d; + static constexpr uint8_t value + = (0b01 << 6) | (0b011111); // stream to FIFO mode, FIFO depth + // 8k bytes <-- this disables all APEX + // features, but we don't need them + }; + + struct FifoConfig3 { + static constexpr uint8_t reg = 0x21; + static constexpr uint8_t value = (0b1 << 0) | (0b1 << 1) | (0b1 << 2) + | (0b1 << 3); // enable FIFO, + // enable accel, + // enable gyro, + // enable hires mode + }; + + struct PwrMgmt0 { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value + = 0b11 | (0b11 << 2); // accel in low noise mode, gyro in low noise + }; + + static constexpr uint8_t FifoCount = 0x12; + static constexpr uint8_t FifoData = 0x14; + }; + +#pragma pack(push, 1) + struct FifoEntryAligned { + union { + struct { + int16_t accel[3]; + int16_t gyro[3]; + uint16_t temp; + uint16_t timestamp; + uint8_t lsb[3]; + } part; + uint8_t raw[19]; + }; + }; +#pragma pack(pop) + + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; + + bool initialize() { + // perform initialization step + i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); + delay(35); + 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::FifoConfig3::reg, Regs::FifoConfig3::value); + i2c.writeReg(Regs::PwrMgmt0::reg, Regs::PwrMgmt0::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_packets = i2c.readReg16(Regs::FifoCount); + const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); + + std::array read_buffer; // max 8 readings + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(fifo_bytes) + ) + / FullFifoEntrySize * FullFifoEntrySize; + i2c.readBytes(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 + const int32_t gyroData[3]{ + static_cast(entry.part.gyro[0]) << 4 + | (entry.part.lsb[0] & 0xf), + static_cast(entry.part.gyro[1]) << 4 + | (entry.part.lsb[1] & 0xf), + static_cast(entry.part.gyro[2]) << 4 + | (entry.part.lsb[2] & 0xf), + }; + processGyroSample(gyroData, GyrTs); + + if (entry.part.accel[0] != -32768) { + const int32_t accelData[3]{ + static_cast(entry.part.accel[0]) << 4 + | (static_cast(entry.part.lsb[0]) & 0xf0 >> 4), + static_cast(entry.part.accel[1]) << 4 + | (static_cast(entry.part.lsb[1]) & 0xf0 >> 4), + static_cast(entry.part.accel[2]) << 4 + | (static_cast(entry.part.lsb[2]) & 0xf0 >> 4), + }; + processAccelSample(accelData, AccTs); + } + } + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers From 50b402917bd5f113d86bbad569b76f8b06329d86 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 7 Nov 2024 19:08:04 +0100 Subject: [PATCH 7/7] Removed a fair amount of code duplication --- src/sensors/softfusion/drivers/icm45605.h | 152 +--------------- src/sensors/softfusion/drivers/icm45686.h | 156 +---------------- src/sensors/softfusion/drivers/icm45base.h | 194 +++++++++++++++++++++ 3 files changed, 209 insertions(+), 293 deletions(-) create mode 100644 src/sensors/softfusion/drivers/icm45base.h diff --git a/src/sensors/softfusion/drivers/icm45605.h b/src/sensors/softfusion/drivers/icm45605.h index 241200806..c27813f8f 100644 --- a/src/sensors/softfusion/drivers/icm45605.h +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -23,9 +23,7 @@ #pragma once -#include -#include -#include +#include "icm45base.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -37,156 +35,18 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // Timestamps reading not used, as they're useless (constant predefined increment) template -struct ICM45605 { - static constexpr uint8_t Address = 0x68; +struct ICM45605 : ICM45Base { static constexpr auto Name = "ICM-45605"; static constexpr auto Type = ImuID::ICM45605; - static constexpr float GyrTs = 1.0 / 409.6; - static constexpr float AccTs = 1.0 / 102.4; - - static constexpr float MagTs = 1.0 / 100; - - static constexpr float GyroSensitivity = 131.072f; - static constexpr float AccelSensitivity = 16384.0f; - - static constexpr bool Uses32BitSensorData = true; - - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x72; - static constexpr uint8_t value = 0xe5; - }; - static constexpr uint8_t TempData = 0x0c; - - struct DeviceConfig { - static constexpr uint8_t reg = 0x7f; - static constexpr uint8_t valueSwReset = 0b11; - }; - - struct GyroConfig { - static constexpr uint8_t reg = 0x1c; - static constexpr uint8_t value - = (0b0000 << 4) | 0b0111; // 4000dps, odr=409.6Hz - }; - - struct AccelConfig { - static constexpr uint8_t reg = 0x1b; - static constexpr uint8_t value - = (0b000 << 4) | 0b1001; // 32g, odr = 102.4Hz - }; - - struct FifoConfig0 { - static constexpr uint8_t reg = 0x1d; - static constexpr uint8_t value - = (0b01 << 6) | (0b011111); // stream to FIFO mode, FIFO depth - // 8k bytes <-- this disables all APEX - // features, but we don't need them - }; - - struct FifoConfig3 { - static constexpr uint8_t reg = 0x21; - static constexpr uint8_t value = (0b1 << 0) | (0b1 << 1) | (0b1 << 2) - | (0b1 << 3); // enable FIFO, - // enable accel, - // enable gyro, - // enable hires mode - }; + : ICM45Base(i2c, logger) {} - struct PwrMgmt0 { - static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value - = 0b11 | (0b11 << 2); // accel in low noise mode, gyro in low noise - }; - - static constexpr uint8_t FifoCount = 0x12; - static constexpr uint8_t FifoData = 0x14; - }; - -#pragma pack(push, 1) - struct FifoEntryAligned { - union { - struct { - int16_t accel[3]; - int16_t gyro[3]; - uint16_t temp; - uint16_t timestamp; - uint8_t lsb[3]; - } part; - uint8_t raw[19]; - }; - }; -#pragma pack(pop) - - static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; + using ICM45Base::i2c; bool initialize() { - // perform initialization step - i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); - delay(35); - 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::FifoConfig3::reg, Regs::FifoConfig3::value); - i2c.writeReg(Regs::PwrMgmt0::reg, Regs::PwrMgmt0::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_packets = i2c.readReg16(Regs::FifoCount); - const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); - - std::array read_buffer; // max 8 readings - const auto bytes_to_read = std::min( - static_cast(read_buffer.size()), - static_cast(fifo_bytes) - ) - / FullFifoEntrySize * FullFifoEntrySize; - i2c.readBytes(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 - const int32_t gyroData[3]{ - static_cast(entry.part.gyro[0]) << 4 - | (entry.part.lsb[0] & 0xf), - static_cast(entry.part.gyro[1]) << 4 - | (entry.part.lsb[1] & 0xf), - static_cast(entry.part.gyro[2]) << 4 - | (entry.part.lsb[2] & 0xf), - }; - processGyroSample(gyroData, GyrTs); - - if (entry.part.accel[0] != -32768) { - const int32_t accelData[3]{ - static_cast(entry.part.accel[0]) << 4 - | (static_cast(entry.part.lsb[0]) & 0xf0 >> 4), - static_cast(entry.part.accel[1]) << 4 - | (static_cast(entry.part.lsb[1]) & 0xf0 >> 4), - static_cast(entry.part.accel[2]) << 4 - | (static_cast(entry.part.lsb[2]) & 0xf0 >> 4), - }; - processAccelSample(accelData, AccTs); - } - } + ICM45Base::softResetIMU(); + return ICM45Base::initializeBase(); } }; diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h index 51a95501a..e3aff45bb 100644 --- a/src/sensors/softfusion/drivers/icm45686.h +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -23,9 +23,7 @@ #pragma once -#include -#include -#include +#include "icm45base.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -37,39 +35,14 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // Timestamps reading not used, as they're useless (constant predefined increment) template -struct ICM45686 { - static constexpr uint8_t Address = 0x68; +struct ICM45686 : ICM45Base { static constexpr auto Name = "ICM-45688"; static constexpr auto Type = ImuID::ICM45686; - static constexpr float GyrTs = 1.0 / 409.6; - static constexpr float AccTs = 1.0 / 102.4; - - static constexpr float MagTs = 1.0 / 100; - - static constexpr float GyroSensitivity = 131.072f; - static constexpr float AccelSensitivity = 16384.0f; - - static constexpr bool Uses32BitSensorData = true; - - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} - - struct Regs { - struct WhoAmI { - static constexpr uint8_t reg = 0x72; - static constexpr uint8_t value = 0xe9; - }; - static constexpr uint8_t TempData = 0x0c; - - struct DeviceConfig { - static constexpr uint8_t reg = 0x7f; - static constexpr uint8_t valueSwReset = 0b11; - }; + : ICM45Base(i2c, logger) {} + struct AdditionalRegs { struct Pin9Config { static constexpr uint8_t reg = 0x31; static constexpr uint8_t value = 0b00000110; // pin 9 to clkin @@ -79,126 +52,15 @@ struct ICM45686 { static constexpr uint8_t reg = 0x26; static constexpr uint8_t value = 0b00100011; // enable RTC }; - - struct GyroConfig { - static constexpr uint8_t reg = 0x1c; - static constexpr uint8_t value - = (0b0000 << 4) | 0b0111; // 4000dps, odr=409.6Hz - }; - - struct AccelConfig { - static constexpr uint8_t reg = 0x1b; - static constexpr uint8_t value - = (0b000 << 4) | 0b1001; // 32g, odr = 102.4Hz - }; - - struct FifoConfig0 { - static constexpr uint8_t reg = 0x1d; - static constexpr uint8_t value - = (0b01 << 6) | (0b011111); // stream to FIFO mode, FIFO depth - // 8k bytes <-- this disables all APEX - // features, but we don't need them - }; - - struct FifoConfig3 { - static constexpr uint8_t reg = 0x21; - static constexpr uint8_t value = (0b1 << 0) | (0b1 << 1) | (0b1 << 2) - | (0b1 << 3); // enable FIFO, - // enable accel, - // enable gyro, - // enable hires mode - }; - - struct PwrMgmt0 { - static constexpr uint8_t reg = 0x10; - static constexpr uint8_t value - = 0b11 | (0b11 << 2); // accel in low noise mode, gyro in low noise - }; - - static constexpr uint8_t FifoCount = 0x12; - static constexpr uint8_t FifoData = 0x14; - }; - -#pragma pack(push, 1) - struct FifoEntryAligned { - union { - struct { - int16_t accel[3]; - int16_t gyro[3]; - uint16_t temp; - uint16_t timestamp; - uint8_t lsb[3]; - } part; - uint8_t raw[19]; - }; }; -#pragma pack(pop) - static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; + using ICM45Base::i2c; bool initialize() { - // perform initialization step - i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); - delay(35); - i2c.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value); - i2c.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::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::FifoConfig3::reg, Regs::FifoConfig3::value); - i2c.writeReg(Regs::PwrMgmt0::reg, Regs::PwrMgmt0::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_packets = i2c.readReg16(Regs::FifoCount); - const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); - - std::array read_buffer; // max 8 readings - const auto bytes_to_read = std::min( - static_cast(read_buffer.size()), - static_cast(fifo_bytes) - ) - / FullFifoEntrySize * FullFifoEntrySize; - i2c.readBytes(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 - const int32_t gyroData[3]{ - static_cast(entry.part.gyro[0]) << 4 - | (entry.part.lsb[0] & 0xf), - static_cast(entry.part.gyro[1]) << 4 - | (entry.part.lsb[1] & 0xf), - static_cast(entry.part.gyro[2]) << 4 - | (entry.part.lsb[2] & 0xf), - }; - processGyroSample(gyroData, GyrTs); - - if (entry.part.accel[0] != -32768) { - const int32_t accelData[3]{ - static_cast(entry.part.accel[0]) << 4 - | (static_cast(entry.part.lsb[0]) & 0xf0 >> 4), - static_cast(entry.part.accel[1]) << 4 - | (static_cast(entry.part.lsb[1]) & 0xf0 >> 4), - static_cast(entry.part.accel[2]) << 4 - | (static_cast(entry.part.lsb[2]) & 0xf0 >> 4), - }; - processAccelSample(accelData, AccTs); - } - } + ICM45Base::softResetIMU(); + i2c.writeReg(AdditionalRegs::Pin9Config::reg, AdditionalRegs::Pin9Config::value); + i2c.writeReg(AdditionalRegs::RtcConfig::reg, AdditionalRegs::RtcConfig::value); + return ICM45Base::initializeBase(); } }; diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h new file mode 100644 index 000000000..f9ffcc84a --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -0,0 +1,194 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include +#include +#include + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45Base { + static constexpr uint8_t Address = 0x68; + + static constexpr float GyrTs = 1.0 / 409.6; + static constexpr float AccTs = 1.0 / 102.4; + + static constexpr float MagTs = 1.0 / 100; + + static constexpr float GyroSensitivity = 131.072f; + static constexpr float AccelSensitivity = 16384.0f; + + static constexpr bool Uses32BitSensorData = true; + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x72; + static constexpr uint8_t value = 0xe5; + }; + static constexpr uint8_t TempData = 0x0c; + + struct DeviceConfig { + static constexpr uint8_t reg = 0x7f; + static constexpr uint8_t valueSwReset = 0b11; + }; + + struct GyroConfig { + static constexpr uint8_t reg = 0x1c; + static constexpr uint8_t value + = (0b0000 << 4) | 0b0111; // 4000dps, odr=409.6Hz + }; + + struct AccelConfig { + static constexpr uint8_t reg = 0x1b; + static constexpr uint8_t value + = (0b000 << 4) | 0b1001; // 32g, odr = 102.4Hz + }; + + struct FifoConfig0 { + static constexpr uint8_t reg = 0x1d; + static constexpr uint8_t value + = (0b01 << 6) | (0b011111); // stream to FIFO mode, FIFO depth + // 8k bytes <-- this disables all APEX + // features, but we don't need them + }; + + struct FifoConfig3 { + static constexpr uint8_t reg = 0x21; + static constexpr uint8_t value = (0b1 << 0) | (0b1 << 1) | (0b1 << 2) + | (0b1 << 3); // enable FIFO, + // enable accel, + // enable gyro, + // enable hires mode + }; + + struct PwrMgmt0 { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value + = 0b11 | (0b11 << 2); // accel in low noise mode, gyro in low noise + }; + + static constexpr uint8_t FifoCount = 0x12; + static constexpr uint8_t FifoData = 0x14; + }; + +#pragma pack(push, 1) + struct FifoEntryAligned { + union { + struct { + int16_t accel[3]; + int16_t gyro[3]; + uint16_t temp; + uint16_t timestamp; + uint8_t lsb[3]; + } part; + uint8_t raw[19]; + }; + }; +#pragma pack(pop) + + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; + + void softResetIMU() { + i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); + delay(35); + } + + bool initializeBase() { + // perform initialization step + 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::FifoConfig3::reg, Regs::FifoConfig3::value); + i2c.writeReg(Regs::PwrMgmt0::reg, Regs::PwrMgmt0::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_packets = i2c.readReg16(Regs::FifoCount); + const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); + + std::array read_buffer; // max 8 readings + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(fifo_bytes) + ) + / FullFifoEntrySize * FullFifoEntrySize; + i2c.readBytes(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 + const int32_t gyroData[3]{ + static_cast(entry.part.gyro[0]) << 4 + | (entry.part.lsb[0] & 0xf), + static_cast(entry.part.gyro[1]) << 4 + | (entry.part.lsb[1] & 0xf), + static_cast(entry.part.gyro[2]) << 4 + | (entry.part.lsb[2] & 0xf), + }; + processGyroSample(gyroData, GyrTs); + + if (entry.part.accel[0] != -32768) { + const int32_t accelData[3]{ + static_cast(entry.part.accel[0]) << 4 + | (static_cast(entry.part.lsb[0]) & 0xf0 >> 4), + static_cast(entry.part.accel[1]) << 4 + | (static_cast(entry.part.lsb[1]) & 0xf0 >> 4), + static_cast(entry.part.accel[2]) << 4 + | (static_cast(entry.part.lsb[2]) & 0xf0 >> 4), + }; + processAccelSample(accelData, AccTs); + } + } + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers