From a0802443c32dc3ba13a9d03d26d7e15dfbf3ec21 Mon Sep 17 00:00:00 2001
From: g945643frankie <106499958+g945643frankie@users.noreply.github.com>
Date: Sat, 12 Sep 2026 07:38:47 +0000
Subject: [PATCH] AP_InertialSensor: ICM56686 FIFO_CONFIG4 uses bit0 per
 DS-000563

Revert FIFO_TMST_FSYNC_EN to 0x01. ICM-56686 FIFO_CONFIG4 bit0 is TMST/FSYNC
and bit1 is compression (DS-000563 v1.1); the 45686 bit map does not apply.
---
 .../AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp    | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
index 6d791b601c2..3fa057e4e39 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
@@ -1056,8 +1056,10 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void)
     register_write_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x58, reg | 0x01);
 
     if (inv3_type == Invensensev3_Type::ICM56686) {
-        // FIFO_TMST_FSYNC_EN is bit1 (0x02); bit0 is fifo_es0_6b_9b. compression left disabled
-        register_write(reg456(INV3REG_456_FIFO_CONFIG4), 0x02, true);
+        // DS-000563 v1.1 §23.36 FIFO_CONFIG4 @ 0x26: bit0 FIFO_TMST_FSYNC_EN, bit1 FIFO_COMP_EN.
+        // (ICM-45686 map differs: bit0 fifo_es0_6b_9b, bit1 fifo_tmst_fsync_en.)
+        // Enable timestamp/FSYNC field; leave compression disabled.
+        register_write(reg456(INV3REG_456_FIFO_CONFIG4), 0x01, true);
     }
 
     uint8_t fifo_config = (1U<<2 | 1U<<1); // FIFO_ACCEL_EN | FIFO_GYRO_EN, FIFO_IF_EN disabled