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.