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

Source: https://forum.modalai.com/topic/5147/multiple-critical-bugs-in-voxl-imu-server-incorrect-fifo-latching-and-hires-indexing-icm-42688
Category: VOXL SDK and Software (https://forum.modalai.com/category/47/voxl-sdk-and-software)
Tags: voxl-sdk
Posted: 2026-04-01 19:45:08 UTC by Igor
Replies: 3 · Views: 2243

## Igor · 2026-04-01 19:45:08 UTC

While auditing the IMU driver logic for the QRB5165 ([icm42688.cpp](https://gitlab.com/voxl-public/voxl-sdk/services/qrb5165-imu-server/-/blob/dev/server/src/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 
![Screenshot 2026-04-01 211119.png](https://forum.modalai.com/assets/uploads/files/1775070851675-screenshot-2026-04-01-211119.png) 

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.
![Screenshot 2026-04-01 213534.png](https://forum.modalai.com/assets/uploads/files/1775072484134-screenshot-2026-04-01-213534.png)

## Reply by Alex Kushleyev (ModalAI staff) · 2026-04-02 13:28:03 UTC

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

## Reply by Alex Kushleyev (ModalAI staff) · 2026-04-15 15:21:04 UTC (in reply to 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

## Reply by Igor · 2026-04-15 16:39:43 UTC (in reply to Alex Kushleyev)

@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