ICM-45 firmware support

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
 

src/sensors/SensorManager.cpp

@@ -38,6 +38,8 @@
 #include "softfusion/drivers/lsm6dso.h"
 #include "softfusion/drivers/lsm6dsr.h"
 #include "softfusion/drivers/mpu6050.h"
+#include "softfusion/drivers/icm45686.h"
+#include "softfusion/drivers/icm45605.h"
 
 #include "softfusion/i2cimpl.h"
 
@@ -56,6 +58,8 @@ namespace SlimeVR
         using SoftFusionLSM6DSO = SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl>;
         using SoftFusionLSM6DSR = SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl>;
         using SoftFusionMPU6050 = SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl>;
+        using SoftFusionICM45686 = SoftFusionSensor<SoftFusion::Drivers::ICM45686, SoftFusion::I2CImpl>;
+        using SoftFusionICM45605 = SoftFusionSensor<SoftFusion::Drivers::ICM45605, SoftFusion::I2CImpl>;
 
         // TODO Make it more generic in the future and move another place (abstract sensor interface)
         void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin)
@@ -172,7 +176,7 @@ namespace SlimeVR
 
                 m_LastBundleSentAtMicros = now;
             #endif
-            
+
             #if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
                 networkConnection.beginBundle();
             #endif

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";

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,22 +148,33 @@ struct ICM42688
     template <typename AccelCall, typename GyroCall>
     void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
         const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
-        
+
         std::array<uint8_t, FullFifoEntrySize * 8> read_buffer; // max 8 readings
         const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()),
             static_cast<size_t>(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);
+
+			const int32_t gyroData[3]{
+				static_cast<int32_t>(entry.part.gyro[0]) << 4 | (entry.part.xlsb & 0xf),
+				static_cast<int32_t>(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf),
+				static_cast<int32_t>(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<int32_t>(entry.part.accel[0]) << 4 | (static_cast<int32_t>(entry.part.xlsb) & 0xf0 >> 4),
+					static_cast<int32_t>(entry.part.accel[1]) << 4 | (static_cast<int32_t>(entry.part.ylsb) & 0xf0 >> 4),
+					static_cast<int32_t>(entry.part.accel[2]) << 4 | (static_cast<int32_t>(entry.part.zlsb) & 0xf0 >> 4),
+				};
+				processAccelSample(accelData, AccTs);
+            }
+        }
     }
 
 };
 
-} // namespace
+} // namespace

src/sensors/softfusion/drivers/icm45605.h

@@ -0,0 +1,53 @@
+/*
+	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 = 102.4Hz
+// Timestamps reading not used, as they're useless (constant predefined increment)
+
+template <typename I2CImpl>
+struct ICM45605 : ICM45Base<I2CImpl> {
+	static constexpr auto Name = "ICM-45605";
+	static constexpr auto Type = ImuID::ICM45605;
+
+	ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
+		: ICM45Base<I2CImpl>(i2c, logger) {}
+
+	using ICM45Base<I2CImpl>::i2c;
+
+	bool initialize() {
+		ICM45Base<I2CImpl>::softResetIMU();
+		return ICM45Base<I2CImpl>::initializeBase();
+	}
+};
+
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/drivers/icm45686.h

@@ -0,0 +1,67 @@
+/*
+	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 = 102.4Hz
+// Timestamps reading not used, as they're useless (constant predefined increment)
+
+template <typename I2CImpl>
+struct ICM45686 : ICM45Base<I2CImpl> {
+	static constexpr auto Name = "ICM-45688";
+	static constexpr auto Type = ImuID::ICM45686;
+
+	ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
+		: ICM45Base<I2CImpl>(i2c, logger) {}
+
+	struct AdditionalRegs {
+		struct Pin9Config {
+			static constexpr uint8_t reg = 0x31;
+			static constexpr uint8_t value = 0b00000110;  // pin 9 to clkin
+		};
+
+		struct RtcConfig {
+			static constexpr uint8_t reg = 0x26;
+			static constexpr uint8_t value = 0b00100011;  // enable RTC
+		};
+	};
+
+	using ICM45Base<I2CImpl>::i2c;
+
+	bool initialize() {
+		ICM45Base<I2CImpl>::softResetIMU();
+		i2c.writeReg(AdditionalRegs::Pin9Config::reg, AdditionalRegs::Pin9Config::value);
+		i2c.writeReg(AdditionalRegs::RtcConfig::reg, AdditionalRegs::RtcConfig::value);
+		return ICM45Base<I2CImpl>::initializeBase();
+	}
+};
+
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers