Add ICM-56686 IMU support by g945643frankie · Pull Request #12028 · iNavFlight/inav

g945643frankie · 2026-09-24T22:43:09Z

Summary

This adds a driver for the TDK InvenSense ICM-56686 6-axis IMU (accel + gyro, SPI). It is ported from Betaflight's driver and rewritten to use INAV's busDevice_t / gyroDev_t / accDev_t code.

Upstream driver: src/main/drivers/accgyro/accgyro_spi_icm56686.c
Includes the init-order fix from fix(accgyro): program ICM-56686 filters and INT1 while the sensor is off betaflight/betaflight#15750 ("program ICM-56686 filters and INT1 while the sensor is off", merged 2026-09-24)

The ICM-56686 uses the same two-tier DREG/IREG layout as the ICM-456xx family, but its direct register map is different. PWR_MGMT0, INT1_CONFIG0/2, ACCEL_CONFIG0, GYRO_CONFIG0 and the IREG filter addresses all sit at other offsets, and WHO_AM_I = 0x08. Because of that, this is a separate driver (accgyro_icm56686.c) and not a WHO_AM_I variant of accgyro_icm45686.c, the same split Betaflight uses.

Init sequence (from betaflight#15750)

Soft reset through REG_MISC2, check WHO_AM_I, leave PWR_MGMT0 = off.
With both sensors still off, configure:
- SREG_CTRL for 16-bit little-endian output (the reset default is 20-bit big-endian, and FS_SEL is only honoured in 16-bit mode)
- gyro SRC + pre-filter, gyro UI LPF (from the INAV LPF table, ~400 Hz by default at 6.4 kHz ODR), gyro notch bypass
- accel SRC + pre-filter, accel UI LPF at ODR/8 (~200 Hz at 1.6 kHz ODR)
- ACCEL_CONFIG0 (±16 g, 1.6 kHz) and GYRO_CONFIG0 (±2000 dps, 6.4 kHz)
- INT1: clear all sources first (INT1_MODE/POLARITY can only be changed while every source is disabled), then set push-pull, pulsed, active-high
Switch both sensors to Low-Noise mode and wait for the 35 ms gyro startup.
Enable the DRDY source (INT1_CONFIG0) last, once the driver state is set up.

Why: according to the datasheet (DS-000563), SRC, UI LPF and notch fields may only be written while the sensor is off. Betaflight's earlier code wrote them after power-up. Betaflight also enabled DRDY before it had set the gyro state that its EXTI handler reads, so the handler could race against init. INAV's current gyro path does not use the EXTI data-ready handler (dataReady is never set by an ISR), so the race does not apply here today. The DRDY-last ordering is kept anyway so the behaviour matches upstream and stays safe if the INT pin is used later.

IREG writes go out as one busWriteBuf() burst (address MSB, address LSB, data) with CS held low. Betaflight found on hardware that separate register writes do not trigger the internal transfer on this part. Before IREG_DONE is polled, the driver waits the datasheet's minimum 4 µs gap.

Changes

drivers/accgyro/accgyro_icm56686.{c,h}: new driver (detect, init, gyro/accel/temperature read)
drivers/bus.h: DEVHW_ICM56686
drivers/accgyro/accgyro_mpu.h: ICM56686_WHO_AM_I_CONST
sensors/gyro.{c,h}, sensors/acceleration.{c,h}: GYRO_ICM56686 / ACC_ICM56686 and detection cases. The new enum values are appended after FAKE so the persisted value of FAKE (and existing saved configs) is unchanged; detection still tries ICM56686 before falling back to FAKE.
fc/settings.yaml, fc/cli.c: ICM56686 in the acc_hardware table and the gyro name list
target/common_hardware.c: BUSDEV_REGISTER_SPI behind USE_IMU_ICM56686, using ICM56686_SPI_BUS, ICM56686_CS_PIN and IMU_ICM56686_ALIGN
CMakeLists.txt: add the new source files
docs/Settings.md: regenerated with src/utils/update_cli_docs.py

The driver is not enabled on any target in this PR. Board targets can opt in with USE_IMU_ICM56686 and the bus, CS and align defines.

Testing

Build-tested only, with arm-none-eabi-gcc 13.2.rel1 (the version the cmake toolchain file downloads). Everything built with -Werror, with no warnings apart from the usual LTO serial-compilation notices:

MATEKF405 (F4), BETAFPVF722 (F7), IFLIGHT_2RAW_H743 (H7): built with USE_IMU_ICM56686 added locally (not committed) on the existing IMU SPI bus/CS, to compile and link the driver. nm shows the driver symbols and the busdev_icm56686 registry entry in each ELF.
MATEKF405 and TBS_LUCID_H7 (an ICM-45686 target) unmodified: build OK
SITL: build OK

Not tested on hardware. I have no ICM-56686 board, so detection, orientation, scaling and filter behaviour on real hardware still need checking. Hardware testing and feedback from anyone with an ICM-56686 board would be much appreciated.

qodo-code-review · 2026-09-24T22:43:14Z

ⓘ Qodo reviews are paused because the subscription is no longer active. Ask your workspace admin to reactivate the subscription to resume reviews. Manage billing

qodo-free-for-open-source-projects · 2026-09-24T22:44:39Z

PR Summary by Qodo

Add TDK ICM-56686 SPI IMU support

✨ Enhancement 📝 Documentation ⚙️ Configuration changes 🕐 40+ Minutes

AI Description

• Adds SPI acceleration, gyroscope, and temperature support for TDK ICM-56686 devices.
• Safely configures indirect filters and DRDY before starting sensors in low-noise mode.
• Wires opt-in detection, configuration, CLI naming, builds, and generated documentation.

Diagram

graph TD
  TARGET["Target Defines"] --> REGISTRY["SPI Registry"] --> DETECT{"WHO_AM_I valid?"}
  DETECT -->|Yes| INIT["Sensor-Off Setup"] --> REGS["DREG and IREG"] --> POWER["Low-Noise Mode"] --> API["Accel/Gyro Reads"]
  DETECT -->|No| FALLBACK["Next IMU"]

High-Level Assessment

The following are alternative approaches to this PR:

1. Shared ICM-45xxx/56xxx driver core

➕ Could centralize data reads, scaling, filter selection, and indirect-register polling.
➕ Would reduce duplication between closely related IMU drivers.
➖ Different direct and indirect register maps require extensive per-model tables or conditionals.
➖ Refactoring the existing ICM-45686 path would enlarge scope and hardware regression risk.
➖ The new burst-based IREG requirement differs from the existing driver's transaction behavior.

Recommendation: Keep the dedicated ICM-56686 driver for this PR. Its distinct register map and burst-only IREG behavior justify isolation and match the upstream implementation; shared helpers can be extracted later after both device paths receive hardware validation.

Files changed (13) +509 / -2

Enhancement (9) +501 / -1

accgyro_icm56686.cImplement the ICM-56686 SPI IMU driver +452/-0

Implement the ICM-56686 SPI IMU driver

• Implements reset and identity detection, burst-based indirect register access, sensor-off filter and interrupt configuration, and low-noise startup. Provides accelerometer, gyroscope, and temperature reads with INAV-compatible scaling and callbacks.

src/main/drivers/accgyro/accgyro_icm56686.c

accgyro_icm56686.hDeclare ICM-56686 detection interfaces +21/-0

Declare ICM-56686 detection interfaces
• Exposes accelerometer and gyroscope detection functions to the sensor subsystems.
src/main/drivers/accgyro/accgyro_icm56686.h

accgyro_mpu.hDefine the ICM-56686 device identity +1/-0

Define the ICM-56686 device identity
• Adds the ICM-56686 'WHO_AM_I' constant value of '0x08' for device validation.
src/main/drivers/accgyro/accgyro_mpu.h

bus.hAdd the ICM-56686 bus device type +1/-0

Add the ICM-56686 bus device type
• Extends the hardware device enumeration with 'DEVHW_ICM56686' for bus registration and lookup.
src/main/drivers/bus.h

cli.cExpose the ICM-56686 gyro name in CLI output +1/-1

Expose the ICM-56686 gyro name in CLI output
• Adds ICM56686 to the gyro name table while preserving synchronization with the gyro sensor enumeration.
src/main/fc/cli.c

acceleration.cIntegrate ICM-56686 accelerometer detection +13/-0

Integrate ICM-56686 accelerometer detection
• Includes the new driver and adds feature-gated explicit and automatic detection handling for its accelerometer.
src/main/sensors/acceleration.c

acceleration.hDefine the ICM-56686 accelerometer enum +1/-0

Define the ICM-56686 accelerometer enum
• Adds 'ACC_ICM56686' before 'ACC_FAKE' and retains the fake sensor as the enumeration maximum.
src/main/sensors/acceleration.h

gyro.cIntegrate ICM-56686 gyroscope detection +10/-0

Integrate ICM-56686 gyroscope detection
• Includes the new driver and adds feature-gated detection with fallback to subsequent IMU candidates.
src/main/sensors/gyro.c

gyro.hDefine the ICM-56686 gyroscope enum +1/-0

Define the ICM-56686 gyroscope enum
• Adds 'GYRO_ICM56686' before the fake gyroscope value.
src/main/sensors/gyro.h

Documentation (1) +1 / -0

Settings.mdDocument the ICM-56686 accelerometer option +1/-0

Document the ICM-56686 accelerometer option
• Adds ICM56686 to the generated list of supported 'acc_hardware' values.
docs/Settings.md

Other (3) +7 / -1

CMakeLists.txtCompile the ICM-56686 driver sources +2/-0

Compile the ICM-56686 driver sources
• Adds the new ICM-56686 implementation and header to the common firmware source list.
src/main/CMakeLists.txt

settings.yamlAdd ICM-56686 to accelerometer settings +1/-1

Add ICM-56686 to accelerometer settings
• Makes ICM56686 an accepted 'acc_hardware' enumeration value before the fake sensor option.
src/main/fc/settings.yaml

common_hardware.cRegister opt-in ICM-56686 SPI hardware +4/-0

Register opt-in ICM-56686 SPI hardware

• Registers the device when 'USE_IMU_ICM56686' is enabled, using target-provided SPI bus, chip-select, and alignment definitions. No board target is enabled by this change.

src/main/target/common_hardware.c

qodo-free-for-open-source-projects · 2026-09-24T22:46:09Z

Code Review by Qodo

🐞 Bugs (0) 📘 Rule violations (0) 📎 Requirement gaps (0) 🎨 UX issues (0) 🔗 Cross-repo conflicts (0) 📜 Skill insights (0)

1. ~~Failed setup corrupts sensor readings~~ ✓ Resolved 🐞 Bug ☼ Reliability

Description

icm56686AccAndGyroInit() ignores the Boolean results from the required SREG_CTRL write and every
subsequent indirect-register modification. When an indirect transfer times out, initialization still
powers the device and enables data-ready, while the read paths decode 16-bit little-endian samples
even if the sensor retained its 20-bit big-endian reset format.

Code

src/main/drivers/accgyro/accgyro_icm56686.c[326]
+    icm56686WriteIREG(dev, ICM56686_SREG_CTRL_IREG_ADDR, ICM56686_SREG_CTRL_16BIT_LE);

Evidence
The IREG helpers explicitly report timeout failures, but all callers in initialization discard those
results and proceed to enable the sensor. The driver's own register definitions and data readers
show that failure of the first write leaves an output format incompatible with the decoding logic.
src/main/drivers/accgyro/accgyro_icm56686.c[122-125]
src/main/drivers/accgyro/accgyro_icm56686.c[188-213]
src/main/drivers/accgyro/accgyro_icm56686.c[237-245]
src/main/drivers/accgyro/accgyro_icm56686.c[267-289]
src/main/drivers/accgyro/accgyro_icm56686.c[326-362]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
Required IREG operations can time out, but initialization ignores their results and enables a potentially misconfigured sensor.
## Fix Focus Areas
- src/main/drivers/accgyro/accgyro_icm56686.c[188-245]
- src/main/drivers/accgyro/accgyro_icm56686.c[311-364]
## Recommended Fix
Check every required IREG write and read-modify-write result. If any operation fails, stop before powering up or enabling DRDY and invoke the established `FAILURE_GYRO_INIT_FAILED` failure path; also propagate the final completion failure from IREG reads rather than reporting success.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

2. ~~Saved fake accelerometers stop loading~~ ✓ Resolved 🐞 Bug ≡ Correctness

Description

Adding ACC_ICM56686 before ACC_FAKE changes the persisted fake-sensor value from 13 to 14 while
the accelerometer parameter-group version remains unchanged. After an upgrade, a saved value of 13
is interpreted as the new hardware and therefore attempts the wrong detection path or fails
detection when that driver is not compiled.

Code

src/main/sensors/acceleration.h[49]
+    ACC_ICM56686,

Evidence
The enum previously ended with ACC_FAKE immediately after ACC_ICM40609D, while the added member
now occupies that old value. The hardware choice is stored directly as a byte in parameter-group
version 6 and serialized numerically, so no migration distinguishes an old fake-sensor value from
the new member.
src/main/sensors/acceleration.h[36-52]
src/main/sensors/acceleration.h[72-80]
src/main/sensors/acceleration.c[91-91]
src/main/sensors/acceleration.c[277-291]
src/main/fc/settings.yaml[4-6]
src/main/fc/fc_msp.c[1502-1502]
src/main/fc/fc_msp.c[2973-2973]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
The new enum member reuses the numeric value previously persisted for the fake accelerometer, so existing configurations are decoded differently after upgrading.
## Fix Focus Areas
- src/main/sensors/acceleration.h[36-52]
- src/main/fc/settings.yaml[4-6]
- docs/Settings.md[91-95]
## Recommended Fix
Preserve `ACC_FAKE` at its previous numeric value and assign the new sensor a distinct value, updating `ACC_MAX` and the settings table accordingly. Regenerate the settings documentation after changing the table order.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

Tip of the day

💡 Did you know, you can type 'qodo, fix this' on a finding and the fix lands right on your PR

More tips ↗ | Customize Qodo ↗ | Qodo docs ↗

github-actions · 2026-09-25T10:28:19Z

RAM / Flash usage vs. base commit 36220fb — commit eead383

Target	Flash Δ	RAM Δ
MATEKF405	+24 B (+0.00%)	CCM: ±0 B (±0.00%) RAM: ±0 B (±0.00%)
MATEKF722	+16 B (+0.00%)	ITCM_RAM: ±0 B (±0.00%) RAM: ±0 B (±0.00%) TCM: ±0 B (±0.00%)
MATEKF765	+16 B (+0.00%)	DTCM_RAM: ±0 B (±0.00%) SRAM1: ±0 B (±0.00%)
MATEKH743	+24 B (+0.00%)	D2_RAM: ±0 B (±0.00%) DTCM_RAM: ±0 B (±0.00%) ITCM_RAM: ±0 B (±0.00%) RAM: ±0 B (±0.00%)

See RAM/flash optimization guide for techniques to reduce usage.

github-actions · 2026-09-25T10:28:41Z

Test firmware build ready — commit eead383

Download firmware for PR #12028

251 targets built. Find your board's .hex file by name on that page (e.g. MATEKF405SE.hex). Files are individually downloadable — no GitHub login required.

Development build for testing only. Use Full Chip Erase when flashing.

Add a driver for the TDK InvenSense ICM-56686 6-axis accel/gyro, ported from Betaflight's accgyro_spi_icm56686.c and adapted to INAV's busDevice/gyroDev_t/accDev_t abstractions. The port includes the init-order fix from betaflight/betaflight#15750: SREG_CTRL (16-bit LE), gyro/accel SRC, UI LPF and notch bypass, ODR/FSR and the INT1 pin configuration are all programmed while both sensors are powered off, then the sensors are switched to Low-Noise mode, and the DRDY interrupt source is enabled last, after the driver state has been set up. - new DEVHW_ICM56686 / GYRO_ICM56686 / ACC_ICM56686 (inserted before FAKE, as done for previous IMUs), acc_hardware table entry and CLI name - BUSDEV_REGISTER_SPI entry in common_hardware.c behind USE_IMU_ICM56686 (ICM56686_SPI_BUS / ICM56686_CS_PIN / IMU_ICM56686_ALIGN); not enabled on any target yet - docs/Settings.md regenerated Upstream: https://github.com/betaflight/betaflight/blob/master/src/main/drivers/accgyro/accgyro_spi_icm56686.c Fix: betaflight/betaflight#15750

Address review findings on the ICM-56686 driver: - Check the result of every required IREG operation (SREG_CTRL write and the SRC / UI LPF / notch read-modify-writes). They are now done in icm56686ConfigureIREG() while both sensors are still off; on any failure init stops before ODR/FSR, powering the sensors up or enabling DRDY and takes the FAILURE_GYRO_INIT_FAILED path used by other INAV gyro drivers. The betaflight#15750 ordering is unchanged. - icm56686ReadIREG() now propagates bus errors and the timeout of the final post-read IREG_DONE wait; icm56686ModifyIREG() no longer falls back to a blind write when the read fails; icm56686WriteIREG() checks the SPI burst result. - Move ACC_ICM56686 / GYRO_ICM56686 after ACC_FAKE / GYRO_FAKE so the persisted acc_hardware value of FAKE (13) does not change; ACC_MAX, the acc_hardware table, the CLI gyro name list and docs/Settings.md are updated to match.

qodo-free-for-open-source-projects Bot reviewed Sep 24, 2026

View reviewed changes

Comment thread src/main/drivers/accgyro/accgyro_icm56686.c Outdated

Comment thread src/main/sensors/acceleration.h Outdated

g945643frankie added 2 commits September 26, 2026 09:57

g945643frankie force-pushed the feature/icm56686 branch from eead383 to b97d260 Compare September 26, 2026 01:59

Navigation Menu

Add ICM-56686 IMU support - #12028

Add ICM-56686 IMU support#12028
g945643frankie wants to merge 2 commits into
iNavFlight:maintenance-10.xfrom
g945643frankie:feature/icm56686

g945643frankie commented Sep 24, 2026 •

edited

Loading

Uh oh!

qodo-code-review Bot commented Sep 24, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Sep 24, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Sep 24, 2026 •

edited

Loading

Uh oh!

Uh oh!

Uh oh!

github-actions Bot commented Sep 25, 2026

Uh oh!

github-actions Bot commented Sep 25, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

Conversation

g945643frankie commented Sep 24, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Init sequence (from betaflight#15750)

Changes

Testing

Uh oh!

qodo-code-review Bot commented Sep 24, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Sep 24, 2026

PR Summary by Qodo

Uh oh!

qodo-free-for-open-source-projects Bot commented Sep 24, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Code Review by Qodo

Uh oh!

Uh oh!

Uh oh!

github-actions Bot commented Sep 25, 2026

Uh oh!

github-actions Bot commented Sep 25, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

g945643frankie commented Sep 24, 2026 •

edited

Loading

qodo-free-for-open-source-projects Bot commented Sep 24, 2026 •

edited

Loading