Multiple Critical Bugs in voxl-imu-server: Incorrect FIFO Latching and HiRes Indexing (ICM-42688)

[Igor]

While auditing the IMU driver logic for the QRB5165 (icm42688.cpp), I identified two significant logical errors in how the ICM-42688-P FIFO data is retrieved and parsed. These bugs affect data integrity and 20-bit precision.
Bug 1: Incorrect FIFO_COUNT Latching Sequence (Race Condition)
Location: src/drivers/icm42688.c (inside read_imu_fifo)
The current implementation reads fifo_count starting from FIFO_COUNTH (0x2E).

    // first read how many bytes are available using the fifo_count register
    ret = voxl_spi_read_reg_word(bus, count_address, &fifo_count);

but acording to IMU document we must read FIFO_COUNTL to latch the data

Bug 2: Index Mismatch in 20-bit HiRes Packet Parsing
Location: fifo_read function under #ifdef HIRES_FIFO

Technical Detail:
When parsing Packet 4 (20-byte), the code maps the extension bits (lower 4 bits) incorrectly for the accelerometer.

		ax32 = ((int32_t)ax16 << 4) | ((base[11] & 0xF0) >> 4);
		ay32 = ((int32_t)ay16 << 4) | ((base[12] & 0xF0) >> 4);
		az32 = ((int32_t)az16 << 4) | ((base[13] & 0xF0) >> 4);

It uses byte indices 11, 12, and 13, but these correspond to Gyro Z and Temperature data. The correct indices are 17, 18, and 19.

[Alex Kushleyev]

Hello @Igor ,

After a quick look your concerns seem valid to me. We will investigate this further and get back to you soon.

Thank you for reporting the issue.

Alex

[Alex Kushleyev]

@Igor ,

You are absolutely correct about the incorrect parsing of the accel data. This is a typo and it was not detected because the effect of it is very small (4 LSB of the 20-bit number). The accelerometer noise in flight will dominate and these bytes will have no effect. We will fix this bug.

Regarding the order of the reading of FIFO_COUNT, we looked at many other sources, including the reference code from TDK and all the implementations read the FIFO_COUNTH first, so we suspect that the documentation has a typo (which makes sense, otherwise the implementation would have to have separate transactions for reading the H and L parts).

Thank you again for your bug report.

Alex

[Igor]

@Alex-Kushleyev Hi Alex,

Thank you for the clarification.

That makes sense regarding the accel parsing—agreed that a 4 LSB discrepancy on a 20-bit value is negligible compared to in-flight noise, but it’s good to see it will be corrected.

Appreciate you taking the time to investigate and confirm.
Igor