fix icm42688 (#493)

* fix_icm42688 test fullscale
Still issues with snapping

* fix temp scale, adjust buffer

* change to lowres

* fix semikolon

* Apply suggestions

Co-Authored-By: gorbit99 <5338164+gorbit99@users.noreply.github.com>

* formating

---------

Co-authored-by: gorbit99 <5338164+gorbit99@users.noreply.github.com>

Author: unlogisch04

src/sensors/softfusion/drivers/icm42688.h

@@ -30,6 +30,8 @@
 #include "callbacks.h"
 #include "vqf.h"
 
+constexpr static bool DEBUG_ICM42688_HIRES = false;
+
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
 // Driver uses acceleration range at 8g
@@ -44,16 +46,22 @@ struct ICM42688 {
 
 	static constexpr float GyrTs = 1.0 / 200.0;
 	static constexpr float AccTs = 1.0 / 100.0;
-	static constexpr float TempTs = 1.0 / 500.0;
+	static constexpr float TempTs = 1.0 / 200.0;
 
 	static constexpr float MagTs = 1.0 / 100;
 
-	static constexpr float GyroSensitivity = 32.8f;
-	static constexpr float AccelSensitivity = 4096.0f;
+	// When 20-bits data format is used, the only FSR settings that are
+	// operational are ±2000dps for gyroscope and ±16g for accelerometer, even if the
+	// FSR selection register settings are configured for other FSR values. The
+	// corresponding sensitivity scale factor values are 131 LSB/dps for gyroscope and
+	// 8192 LSB/g for accelerometer.
+	static constexpr float GyroSensitivity = (DEBUG_ICM42688_HIRES ? 131.0f : 32.8f);
+	static constexpr float AccelSensitivity
+		= (DEBUG_ICM42688_HIRES ? 8192.0f : 4096.0f);
 
 	static constexpr float TemperatureBias = 25.0f;
-	static constexpr float TemperatureSensitivity = 2.07f;
-
+	static constexpr float TemperatureSensitivity
+		= (DEBUG_ICM42688_HIRES ? 132.48f : 2.07f);
 	static constexpr float TemperatureZROChange = 20.0f;
 
 	static constexpr VQFParams SensorVQFParams{};
@@ -88,7 +96,9 @@ struct ICM42688 {
 			static constexpr uint8_t reg = 0x5f;
 			static constexpr uint8_t value
 				= 0b1 | (0b1 << 1) | (0b1 << 2)
-				| (0b0 << 4);  // fifo accel en=1, gyro=1, temp=1, hires=1
+				| (DEBUG_ICM42688_HIRES ? (0b1 << 4) : (0b0 << 4));
+			// fifo accel en=1, gyro=1, temp=1,
+			// hires=DEBUG_ICM42688_HIRES
 		};
 		struct GyroConfig {
 			static constexpr uint8_t reg = 0x4f;
@@ -115,7 +125,7 @@ struct ICM42688 {
 	};
 
 #pragma pack(push, 1)
-	struct FifoEntryAligned {
+	struct FifoEntryAlignedHires {
 		union {
 			struct {
 				int16_t accel[3];
@@ -128,10 +138,65 @@ struct ICM42688 {
 			} part;
 			uint8_t raw[19];
 		};
+
+		void getGyro(int32_t out[3]) {
+			// 6.1 Packet Structure for high resolution mode
+			// https://invensense.tdk.com/wp-content/uploads/2020/04/ds-000347_icm-42688-p-datasheet.pdf
+			// When 20-bits data format is used, gyroscope data consists of 19-bits
+			// of actual data and the LSB is always set to 0
+			out[0] = static_cast<int32_t>(part.gyro[0]) << 3 | ((part.xlsb & 0xe) >> 1);
+			out[1] = static_cast<int32_t>(part.gyro[1]) << 3 | ((part.ylsb & 0xe) >> 1);
+			out[2] = static_cast<int32_t>(part.gyro[2]) << 3 | ((part.zlsb & 0xe) >> 1);
+		}
+		void getAccel(int32_t out[3]) {
+			// accelerometer data consists of 18-bits of actual data and the two
+			// lowest order bits are always set to 0
+			out[0] = static_cast<int32_t>(part.accel[0]) << 2
+				   | (static_cast<int32_t>((part.xlsb) & 0xf0) >> 6);
+			out[1] = static_cast<int32_t>(part.accel[1]) << 2
+				   | (static_cast<int32_t>((part.ylsb) & 0xf0) >> 6);
+			out[2] = static_cast<int32_t>(part.accel[2]) << 2
+				   | (static_cast<int32_t>((part.zlsb) & 0xf0) >> 6);
+		}
+	};
+
+	struct FifoEntryAlignedDefault {
+		union {
+			struct {
+				int16_t accel[3];
+				int16_t gyro[3];
+				int8_t temp;
+				uint16_t timestamp;
+			} part;
+			uint8_t raw[15];
+		};
+
+		void getGyro(int32_t out[3]) {
+			out[0] = static_cast<int32_t>(part.gyro[0]);
+			out[1] = static_cast<int32_t>(part.gyro[1]);
+			out[2] = static_cast<int32_t>(part.gyro[2]);
+		}
+
+		void getAccel(int32_t out[3]) {
+			out[0] = static_cast<int32_t>(part.accel[0]);
+			out[1] = static_cast<int32_t>(part.accel[1]);
+			out[2] = static_cast<int32_t>(part.accel[2]);
+		}
 	};
 #pragma pack(pop)
 
+	static_assert(sizeof(FifoEntryAlignedHires) == 19);
+	static_assert(sizeof(FifoEntryAlignedDefault) == 15);
+
+	using FifoEntryAligned = std::conditional<
+		DEBUG_ICM42688_HIRES,
+		FifoEntryAlignedHires,
+		FifoEntryAlignedDefault>::type;
+
 	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
+	// max 4 readings in highres mode 8 readings delay too high 6 seems to be the
+	// edge to work reliably. Tested on ESP8266 with 2 IMU
+	static constexpr size_t MaxReadings = DEBUG_ICM42688_HIRES ? 4 : 8;
 
 	bool initialize() {
 		// perform initialization step
@@ -155,7 +220,8 @@ struct ICM42688 {
 	bool bulkRead(DriverCallbacks<int32_t>&& callbacks) {
 		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
 
-		std::array<uint8_t, FullFifoEntrySize * 8> read_buffer;  // max 8 readings
+		std::array<uint8_t, FullFifoEntrySize * MaxReadings> read_buffer;
+
 		const auto bytes_to_read = std::min(
 									   static_cast<size_t>(read_buffer.size()),
 									   static_cast<size_t>(fifo_bytes)
@@ -171,22 +237,13 @@ struct ICM42688 {
 				sizeof(FifoEntryAligned)
 			);  // skip fifo header
 
-			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),
-			};
+			int32_t gyroData[3];
+			entry.getGyro(gyroData);
 			callbacks.processGyroSample(gyroData, GyrTs);
 
 			if (entry.part.accel[0] != -32768) {
-				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),
-				};
+				int32_t accelData[3];
+				entry.getAccel(accelData);
 				callbacks.processAccelSample(accelData, AccTs);
 			}
 
@@ -197,7 +254,6 @@ struct ICM42688 {
 				);
 			}
 		}
-
 		return fifo_bytes > bytes_to_read;
 	}
 };