diff --git a/src/consts.h b/src/consts.h index 4d529d03..0eeabba5 100644 --- a/src/consts.h +++ b/src/consts.h @@ -42,6 +42,8 @@ enum class ImuID { LSM6DSV, LSM6DSO, LSM6DSR, + ICM45686, + ICM45605, Empty = 255 }; @@ -62,6 +64,8 @@ enum class ImuID { #define IMU_LSM6DSO SoftFusionLSM6DSO #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 09262d03..f7345fd7 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -32,6 +32,8 @@ #include "sensoraddresses.h" #include "softfusion/drivers/bmi270.h" #include "softfusion/drivers/icm42688.h" +#include "softfusion/drivers/icm45605.h" +#include "softfusion/drivers/icm45686.h" #include "softfusion/drivers/lsm6ds3trc.h" #include "softfusion/drivers/lsm6dso.h" #include "softfusion/drivers/lsm6dsr.h" @@ -60,6 +62,10 @@ 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) diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index db9573da..9e79fce0 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -128,6 +128,10 @@ const char* getIMUNameByType(ImuID imuType) { return "LSM6DSO"; case ImuID::LSM6DSR: return "LSM6DSR"; + case ImuID::ICM45686: + return "ICM45686"; + case ImuID::ICM45605: + return "ICM45605"; case ImuID::Unknown: case ImuID::Empty: return "UNKNOWN"; diff --git a/src/sensors/softfusion/drivers/icm45605.h b/src/sensors/softfusion/drivers/icm45605.h new file mode 100644 index 00000000..de045ed7 --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -0,0 +1,60 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include "icm45base.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45605 : public ICM45Base { + static constexpr auto Name = "ICM-45605"; + static constexpr auto Type = ImuID::ICM45605; + + ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : ICM45Base{i2c, logger} {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x72; + static constexpr uint8_t value = 0xe5; + }; + }; + + float getDirectTemp() const { return ICM45Base::getDirectTemp(); } + + bool initialize() { + ICM45Base::softResetIMU(); + return ICM45Base::initializeBase(); + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h new file mode 100644 index 00000000..838ac462 --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -0,0 +1,74 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include "icm45base.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45686 : public ICM45Base { + static constexpr auto Name = "ICM-45686"; + static constexpr auto Type = ImuID::ICM45686; + + ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : ICM45Base{i2c, logger} {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x72; + static constexpr uint8_t value = 0xe9; + }; + + struct Pin9Config { + static constexpr uint8_t reg = 0x31; + static constexpr uint8_t value = 0b00000110; // pin 9 to clkin + }; + + struct RtcConfig { + static constexpr uint8_t reg = 0x26; + static constexpr uint8_t value = 0b00100011; // enable RTC + }; + }; + + float getDirectTemp() const { return ICM45Base::getDirectTemp(); } + + using ICM45Base::i2c; + + bool initialize() { + ICM45Base::softResetIMU(); + i2c.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value); + i2c.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value); + return ICM45Base::initializeBase(); + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h new file mode 100644 index 00000000..47dc5b1c --- /dev/null +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -0,0 +1,191 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include +#include +#include + +namespace SlimeVR::Sensors::SoftFusion::Drivers { + +// Driver uses acceleration range at 32g +// and gyroscope range at 4000dps +// using high resolution mode +// Uses 32.768kHz clock +// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM45Base { + static constexpr uint8_t Address = 0x68; + + static constexpr float GyrTs = 1.0 / 409.6; + static constexpr float AccTs = 1.0 / 204.8; + static constexpr float TempTs = 1.0 / 409.6; + + static constexpr float MagTs = 1.0 / 100; + + static constexpr float GyroSensitivity = 131.072f; + static constexpr float AccelSensitivity = 16384.0f; + + static constexpr float TemperatureBias = 25.0f; + static constexpr float TemperatureSensitivity = 128.0f; + + static constexpr bool Uses32BitSensorData = true; + + I2CImpl i2c; + SlimeVR::Logging::Logger& logger; + ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger) + : i2c(i2c) + , logger(logger) {} + + struct BaseRegs { + static constexpr uint8_t TempData = 0x0c; + + struct DeviceConfig { + static constexpr uint8_t reg = 0x7f; + static constexpr uint8_t valueSwReset = 0b11; + }; + + struct GyroConfig { + static constexpr uint8_t reg = 0x1c; + static constexpr uint8_t value + = (0b0000 << 4) | 0b0111; // 4000dps, odr=409.6Hz + }; + + struct AccelConfig { + static constexpr uint8_t reg = 0x1b; + static constexpr uint8_t value + = (0b000 << 4) | 0b1000; // 32g, odr = 204.8Hz + }; + + struct FifoConfig0 { + static constexpr uint8_t reg = 0x1d; + static constexpr uint8_t value + = (0b01 << 6) | (0b011111); // stream to FIFO mode, FIFO depth + // 8k bytes <-- this disables all APEX + // features, but we don't need them + }; + + struct FifoConfig3 { + static constexpr uint8_t reg = 0x21; + static constexpr uint8_t value = (0b1 << 0) | (0b1 << 1) | (0b1 << 2) + | (0b1 << 3); // enable FIFO, + // enable accel, + // enable gyro, + // enable hires mode + }; + + struct PwrMgmt0 { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value + = 0b11 | (0b11 << 2); // accel in low noise mode, gyro in low noise + }; + + static constexpr uint8_t FifoCount = 0x12; + static constexpr uint8_t FifoData = 0x14; + }; + +#pragma pack(push, 1) + struct FifoEntryAligned { + union { + struct { + int16_t accel[3]; + int16_t gyro[3]; + uint16_t temp; + uint16_t timestamp; + uint8_t lsb[3]; + } part; + uint8_t raw[19]; + }; + }; +#pragma pack(pop) + + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; + + void softResetIMU() { + i2c.writeReg(BaseRegs::DeviceConfig::reg, BaseRegs::DeviceConfig::valueSwReset); + delay(35); + } + + bool initializeBase() { + // perform initialization step + i2c.writeReg(BaseRegs::GyroConfig::reg, BaseRegs::GyroConfig::value); + i2c.writeReg(BaseRegs::AccelConfig::reg, BaseRegs::AccelConfig::value); + i2c.writeReg(BaseRegs::FifoConfig0::reg, BaseRegs::FifoConfig0::value); + i2c.writeReg(BaseRegs::FifoConfig3::reg, BaseRegs::FifoConfig3::value); + i2c.writeReg(BaseRegs::PwrMgmt0::reg, BaseRegs::PwrMgmt0::value); + delay(1); + + return true; + } + + float getDirectTemp() const { + const auto value = static_cast(i2c.readReg16(BaseRegs::TempData)); + float result = ((float)value / 132.48f) + 25.0f; + return result; + } + + template + void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + const auto fifo_packets = i2c.readReg16(BaseRegs::FifoCount); + const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); + + std::array read_buffer; // max 8 readings + const auto bytes_to_read = std::min( + static_cast(read_buffer.size()), + static_cast(fifo_bytes) + ) + / FullFifoEntrySize * FullFifoEntrySize; + i2c.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data()); + for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) { + FifoEntryAligned entry; + memcpy( + entry.raw, + &read_buffer[i + 0x1], + sizeof(FifoEntryAligned) + ); // skip fifo header + const int32_t gyroData[3]{ + static_cast(entry.part.gyro[0]) << 4 + | (entry.part.lsb[0] & 0xf), + static_cast(entry.part.gyro[1]) << 4 + | (entry.part.lsb[1] & 0xf), + static_cast(entry.part.gyro[2]) << 4 + | (entry.part.lsb[2] & 0xf), + }; + processGyroSample(gyroData, GyrTs); + + if (entry.part.accel[0] != -32768) { + const int32_t accelData[3]{ + static_cast(entry.part.accel[0]) << 4 + | (static_cast(entry.part.lsb[0]) & 0xf0 >> 4), + static_cast(entry.part.accel[1]) << 4 + | (static_cast(entry.part.lsb[1]) & 0xf0 >> 4), + static_cast(entry.part.accel[2]) << 4 + | (static_cast(entry.part.lsb[2]) & 0xf0 >> 4), + }; + processAccelSample(accelData, AccTs); + } + } + } +}; + +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 133bea48..7b518a4e 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -32,7 +32,14 @@ namespace SlimeVR::Sensors { template