ICM-55686 Implementation

board-defaults.schema.json

@@ -34,7 +34,8 @@
 				"IMU_LSM6DSR",
 				"IMU_MPU6050_SF",
 				"IMU_ICM45686",
-				"IMU_ICM45605"
+				"IMU_ICM45605",
+				"IMU_ICM55686"
 			],
 			"description": "Imu Type"
 		},

src/consts.h

@@ -46,6 +46,7 @@ enum class SensorTypeID : uint8_t {
 	ICM45686,
 	ICM45605,
 	ADC_RESISTANCE,
+	ICM55686,
 	Empty = 255
 };
 
@@ -69,6 +70,7 @@ enum class SensorTypeID : uint8_t {
 #define IMU_MPU6050_SF SoftFusionMPU6050
 #define IMU_ICM45686 SoftFusionICM45686
 #define IMU_ICM45605 SoftFusionICM45605
+#define IMU_ICM55686 SoftFusionICM55686
 
 #define IMU_DEV_RESERVED 250  // Reserved, should not be used in any release firmware
 

src/sensors/SensorBuilder.cpp

@@ -113,6 +113,8 @@ SensorBuilder::buildSensorDynamically(SensorTypeID type, SensorDefinition sensor
 		//	return buildSensor<SoftFusionICM45605>(
 		//		sensorDef
 		//	);
+		case SensorTypeID::ICM55686:
+			return buildSensor<SoftFusionICM55686>(sensorDef);
 		default:
 			m_Manager->m_Logger.error(
 				"Unable to create sensor with type %s (%d)",

src/sensors/SensorBuilder.h

@@ -55,6 +55,7 @@
 #include "softfusion/drivers/icm42688.h"
 #include "softfusion/drivers/icm45605.h"
 #include "softfusion/drivers/icm45686.h"
+#include "softfusion/drivers/icm55686.h"
 #include "softfusion/drivers/lsm6ds3trc.h"
 #include "softfusion/drivers/lsm6dso.h"
 #include "softfusion/drivers/lsm6dsr.h"
@@ -96,6 +97,8 @@ using SoftFusionICM45686
 	= SoftFusionSensor<SoftFusion::Drivers::ICM45686, SFCALIBRATOR>;
 using SoftFusionICM45605
 	= SoftFusionSensor<SoftFusion::Drivers::ICM45605, SFCALIBRATOR>;
+using SoftFusionICM55686
+	= SoftFusionSensor<SoftFusion::Drivers::ICM55686, SFCALIBRATOR>;
 using SoftFusionBMI160 = SoftFusionSensor<SoftFusion::Drivers::BMI160, SFCALIBRATOR>;
 class SensorAuto {};
 
@@ -222,7 +225,8 @@ struct SensorBuilder {
 			SoftFusionLSM6DSR,
 			// SoftFusionMPU6050,
 			SoftFusionICM45686,
-			// SoftFusionICM45605
+			// SoftFusionICM45605,
+			SoftFusionICM55686,
 			BNO085Sensor>(sensorID, sensorInterface, accessInterface);
 	}
 

src/sensors/sensor.cpp

@@ -144,6 +144,8 @@ const char* getIMUNameByType(SensorTypeID imuType) {
 			return "ICM45605";
 		case SensorTypeID::ADC_RESISTANCE:
 			return "ADC Resistance";
+		case SensorTypeID::ICM55686:
+			return "ICM55686";
 		case SensorTypeID::Unknown:
 		case SensorTypeID::Empty:
 			return "UNKNOWN";

src/sensors/softfusion/drivers/icm55686.h

@@ -0,0 +1,244 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2026 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 <algorithm>
+#include <array>
+#include <cstdint>
+
+#include "../../../sensorinterface/RegisterInterface.h"
+#include "callbacks.h"
+#include "vqf.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 = 204.8Hz, accel ODR = 102.4Hz
+// Timestamps reading not used, as they're useless (constant predefined increment)
+
+struct ICM55686 {
+	static constexpr auto Name = "ICM-55686";
+	static constexpr auto Type = SensorTypeID::ICM55686;
+	static constexpr uint8_t Address = 0x68;
+
+	static constexpr float GyrTs = 1.0 / 204.8;
+	static constexpr float AccTs = 1.0 / 102.4;
+	static constexpr float TempTs = 1.0 / 409.6;
+
+	static constexpr float MagTs = 1.0 / 100;
+
+	static constexpr float GyroSensitivity = (1 << 19) / 4000.0f;
+	static constexpr float AccelSensitivity = (1 << 19) / 32.0f;
+
+	static constexpr float TemperatureBias = 25.0f;
+	static constexpr float TemperatureSensitivity = 128.0f;
+
+	static constexpr float TemperatureZROChange = 20.0f;
+
+	static constexpr VQFParams SensorVQFParams{};
+
+	RegisterInterface& m_RegisterInterface;
+	SlimeVR::Logging::Logger& m_Logger;
+	ICM55686(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
+		: m_RegisterInterface{registerInterface}
+		, m_Logger{logger} {}
+
+	struct Regs {
+		static constexpr uint8_t TempData = 0x0c;
+
+		struct WhoAmI {
+			static constexpr uint8_t reg = 0x72;
+			static constexpr uint8_t value = 0x08;
+		};
+
+		struct DeviceConfig {
+			static constexpr uint8_t reg = 0x7f;
+			static constexpr uint8_t valueSwReset = 0b11;
+		};
+
+		struct GyroConfig {
+			static constexpr uint8_t reg = 0x20;
+			static constexpr uint8_t value
+				= (0b0000 << 4) | 0b1000;  // 4000dps, odr=204.8Hz
+		};
+
+		struct AccelConfig {
+			static constexpr uint8_t reg = 0x1f;
+			static constexpr uint8_t value
+				= (0b000 << 4) | 0b1001;  // 32g, odr = 102.4Hz
+		};
+
+		struct FifoConfig0 {
+			static constexpr uint8_t reg = 0x21;
+			static constexpr uint8_t value
+				= (0b10 << 6) | (0b001111);  // stop on full mode, FIFO depth
+											 // 8k bytes <-- this disables all APEX
+											 // features, but we don't need them
+		};
+
+		struct FifoConfig3 {
+			static constexpr uint8_t reg = 0x25;
+			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 = 0x14;
+			static constexpr uint8_t value
+				= (0b11 << 0)
+				| (0b11 << 2);  // accel in low noise mode, gyro in low noise
+		};
+
+		struct IOCPADScenarioOvrd {
+			static constexpr uint8_t reg = 0x35;
+			static constexpr uint8_t value = 0b00000110;  // pin 9 to clkin
+		};
+
+		struct RtcConfig {
+			static constexpr uint8_t reg = 0x2a;
+			static constexpr uint8_t value = 0b00100011;  // enable RTC
+		};
+
+		static constexpr uint8_t FifoCount = 0x16;
+		static constexpr uint8_t FifoData = 0x18;
+	};
+
+#pragma pack(push, 1)
+	struct FifoEntryAligned {
+		uint16_t accel[3];
+		uint16_t gyro[3];
+		uint16_t temp;
+		uint16_t timestamp;
+		uint8_t lsb[3];
+	};
+#pragma pack(pop)
+
+	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
+	static_assert(FullFifoEntrySize == 20);
+
+	bool initialize() {
+		m_RegisterInterface.writeReg(
+			Regs::DeviceConfig::reg,
+			Regs::DeviceConfig::valueSwReset
+		);
+
+		delay(35);
+
+#if IMU_USE_EXTERNAL_CLOCK
+		m_RegisterInterface.writeReg(
+			Regs::IOCPADScenarioOvrd::reg,
+			Regs::IOCPADScenarioOvrd::value
+		);
+		m_RegisterInterface.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value);
+#endif
+
+		m_RegisterInterface.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
+		m_RegisterInterface.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
+		m_RegisterInterface.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
+		m_RegisterInterface.writeReg(Regs::FifoConfig3::reg, Regs::FifoConfig3::value);
+		m_RegisterInterface.writeReg(Regs::PwrMgmt0::reg, Regs::PwrMgmt0::value);
+
+		read_buffer.resize(FullFifoEntrySize * MaxReadings);
+
+		delay(1);
+
+		return true;
+	}
+
+	static constexpr size_t MaxReadings = 8;
+	// Allocate on heap so that it does not take up stack space, which can result in
+	// stack overflow and panic
+	std::vector<uint8_t> read_buffer;
+
+	bool bulkRead(DriverCallbacks<int32_t>&& callbacks) {
+		constexpr auto InvalidReading = static_cast<int16_t>(0x8000);
+
+		size_t fifo_packets
+			= __builtin_bswap16(m_RegisterInterface.readReg16(Regs::FifoCount));
+		auto packets_to_read = std::min(fifo_packets, MaxReadings);
+
+		size_t bytes_to_read = packets_to_read * FullFifoEntrySize;
+		m_RegisterInterface
+			.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
+
+		for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
+			uint8_t header = read_buffer[i];
+			bool has_gyro = header & (1 << 5);
+			bool has_accel = header & (1 << 6);
+
+			FifoEntryAligned entry;
+			memcpy(
+				&entry,
+				&read_buffer[i + 0x1],
+				sizeof(FifoEntryAligned)
+			);  // skip fifo header
+
+			int16_t gyro[3] = {
+				static_cast<int16_t>(__builtin_bswap16(entry.gyro[0])),
+				static_cast<int16_t>(__builtin_bswap16(entry.gyro[1])),
+				static_cast<int16_t>(__builtin_bswap16(entry.gyro[2])),
+			};
+			if (has_gyro && gyro[0] != InvalidReading) {
+				const int32_t gyroData[3]{
+					static_cast<int32_t>(gyro[0]) << 4 | (entry.lsb[0] & 0xf),
+					static_cast<int32_t>(gyro[1]) << 4 | (entry.lsb[1] & 0xf),
+					static_cast<int32_t>(gyro[2]) << 4 | (entry.lsb[2] & 0xf),
+				};
+				callbacks.processGyroSample(gyroData, GyrTs);
+			}
+
+			int16_t accel[3] = {
+				static_cast<int16_t>(__builtin_bswap16(entry.accel[0])),
+				static_cast<int16_t>(__builtin_bswap16(entry.accel[1])),
+				static_cast<int16_t>(__builtin_bswap16(entry.accel[2])),
+			};
+			if (has_accel && accel[0] != InvalidReading) {
+				const int32_t accelData[3]{
+					static_cast<int32_t>(accel[0]) << 4
+						| (static_cast<int32_t>((entry.lsb[0]) & 0xf0) >> 4),
+					static_cast<int32_t>(accel[1]) << 4
+						| (static_cast<int32_t>((entry.lsb[1]) & 0xf0) >> 4),
+					static_cast<int32_t>(accel[2]) << 4
+						| (static_cast<int32_t>((entry.lsb[2]) & 0xf0) >> 4),
+				};
+				callbacks.processAccelSample(accelData, AccTs);
+			}
+
+			auto temp = static_cast<int16_t>(__builtin_bswap16(entry.temp));
+			if (temp != InvalidReading) {
+				callbacks.processTempSample(temp, TempTs);
+			}
+		}
+
+		return fifo_packets > MaxReadings;
+	}
+};
+
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers