Don't copy memory on ICM45 reads

SlimeVR · Jan 27, 2025 · 9675bd9 · 9675bd9
1 parent 56ee3a3
commit 9675bd9
Showing 1 changed file with 16 additions and 13 deletions.
diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h
@@ -53,6 +53,8 @@ struct ICM45Base {
 
 	static constexpr bool Uses32BitSensorData = true;
 
+	static constexpr int fifoReadSize = 8; // Can't be more than 12 or it will overflow i2c read size, default 8
+
 	I2CImpl i2c;
 	SlimeVR::Logging::Logger& logger;
 	ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
@@ -110,6 +112,7 @@ struct ICM45Base {
 	struct FifoEntryAligned {
 		union {
 			struct {
+				uint8_t header;
 				int16_t accel[3];
 				int16_t gyro[3];
 				uint16_t temp;
@@ -119,9 +122,13 @@ struct ICM45Base {
 			uint8_t raw[19];
 		};
 	};
+	struct FifoBuffer {
+		FifoEntryAligned entry[fifoReadSize];
+	};
 #pragma pack(pop)
 
-	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
+	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned);
+	static constexpr size_t FullFifoBufferSize = sizeof(FifoBuffer);
 
 	void softResetIMU() {
 		i2c.writeReg(BaseRegs::DeviceConfig::reg, BaseRegs::DeviceConfig::valueSwReset);
@@ -149,20 +156,16 @@ struct ICM45Base {
 		const auto fifo_packets = i2c.readReg16(BaseRegs::FifoCount);
 		const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize);
 
-		std::array<uint8_t, FullFifoEntrySize * 8> read_buffer;  // max 8 readings
+		FifoBuffer read_buffer;
 		const auto bytes_to_read = std::min(
-									   static_cast<size_t>(read_buffer.size()),
-									   static_cast<size_t>(fifo_bytes)
-								   )
-								 / FullFifoEntrySize * FullFifoEntrySize;
-		i2c.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data());
+									static_cast<size_t>(FullFifoBufferSize),
+									static_cast<size_t>(fifo_bytes)
+								)
+								/ FullFifoEntrySize * FullFifoEntrySize;
+		i2c.readBytes(BaseRegs::FifoData, bytes_to_read, (uint8_t*) &read_buffer);
+		uint8_t index = 0;
 		for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
-			FifoEntryAligned entry;
-			memcpy(
-				entry.raw,
-				&read_buffer[i + 0x1],
-				sizeof(FifoEntryAligned)
-			);  // skip fifo header
+			FifoEntryAligned entry = read_buffer.entry[index++];
 			const int32_t gyroData[3]{
 				static_cast<int32_t>(entry.part.gyro[0]) << 4
 					| (entry.part.lsb[0] & 0xf),