seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

fix: IMU calibration (Issue #520) #530

Merged
sl-jetson merged 2 commits from sl-firmware/issue-520-imu-calibration into main 2026-03-06 23:34:17 -05:00
Conversation 0 Commits 2 Files Changed 14 +1278 -603
sl-jetson commented 2026-03-06 23:15:12 -05:00
Collaborator
Copy Link

Summary

  • Root cause 1: icm42688_init() used a single WHO_AM_I read — one SPI glitch returning 0x00 caused ret=-128, skipping mpu6000_calibrate() entirely → s_calibrated never set to true. Fix: retry up to 3× with 10ms gaps.
  • Root cause 2: rx[15] in icm42688_read() was uninitialized. If HAL_SPI_TransmitReceive() timed out, garbage stack data was accumulated as gyro bias. Fix: zero-init.
  • Root cause 3: raw in calibration loop was uninitialized (UB if icm42688_read() no-ops). Fix: zero-init each iteration.
  • DCache: Added SCB_InvalidateDCache_by_Addr() on SPI rx buffers in rreg() and icm42688_read(). Currently a no-op (DCache disabled), but required if SCB_EnableDCache() is ever called — stack buffers in SRAM2 are in the cacheable region on STM32F7.

Test plan

  • Flash firmware, observe LED1+LED2 solid during ~1s calibration
  • Confirm mpu6000_is_calibrated() returns true after boot
  • Arm via USB A command or RC CH5 — should succeed within 3s hold
  • Verify balance mode engages
  • Verify trace buffer (via icm42688_get_trace()) shows trace[0]=0x68 (WHO_AM_I) and correct imu_type

Build: Flash 59904B (+512B) / RAM 17100B — SUCCESS ✓

🤖 Generated with Claude Code

## Summary - **Root cause 1**: `icm42688_init()` used a single WHO_AM_I read — one SPI glitch returning `0x00` caused `ret=-128`, skipping `mpu6000_calibrate()` entirely → `s_calibrated` never set to `true`. Fix: retry up to 3× with 10ms gaps. - **Root cause 2**: `rx[15]` in `icm42688_read()` was uninitialized. If `HAL_SPI_TransmitReceive()` timed out, garbage stack data was accumulated as gyro bias. Fix: zero-init. - **Root cause 3**: `raw` in calibration loop was uninitialized (UB if `icm42688_read()` no-ops). Fix: zero-init each iteration. - **DCache**: Added `SCB_InvalidateDCache_by_Addr()` on SPI rx buffers in `rreg()` and `icm42688_read()`. Currently a no-op (DCache disabled), but required if `SCB_EnableDCache()` is ever called — stack buffers in SRAM2 are in the cacheable region on STM32F7. ## Test plan - [ ] Flash firmware, observe LED1+LED2 solid during ~1s calibration - [ ] Confirm `mpu6000_is_calibrated()` returns true after boot - [ ] Arm via USB `A` command or RC CH5 — should succeed within 3s hold - [ ] Verify balance mode engages - [ ] Verify trace buffer (via `icm42688_get_trace()`) shows `trace[0]=0x68` (WHO_AM_I) and correct imu_type Build: Flash 59904B (+512B) / RAM 17100B — SUCCESS ✓ 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-jetson added 2 commits 2026-03-06 23:15:12 -05:00 
Three bugs prevented mpu6000_is_calibrated() from returning true,
blocking arming and balance mode:

1. WHO_AM_I single-attempt: one SPI glitch returning 0x00 caused
   icm42688_init() to return -128, skipping mpu6000_calibrate()
   entirely. Fix: retry WHO_AM_I up to 3 times with 10ms gaps.

2. icm42688_read() rx[15] uninitialized: if HAL_SPI_TransmitReceive()
   failed, garbage stack data was accumulated as gyro bias. Fix: zero-
   init rx[15] so failed transfers produce zero data.

3. mpu6000_calibrate() raw uninitialized: UB if icm42688_read() is
   a no-op (imu_type mismatch). Fix: zero-init raw each iteration.

Also add SCB_InvalidateDCache_by_Addr() on SPI rx buffers in rreg()
and icm42688_read() for DCache coherency. Currently a no-op (DCache
is not enabled), but required if SCB_EnableDCache() is added — stack
buffers in SRAM2 are in the cacheable memory region on STM32F7.

Fix misleading DCache comment in icm42688.c (claimed DCache was
disabled by main.c; actually SCB_EnableDCache() is never called).

Build: 59904 bytes Flash (+512), 17100 bytes RAM — SUCCESS

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 54bc2509c1 into main 2026-03-06 23:34:17 -05:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#530
No description provided.
Delete Branch "sl-firmware/issue-520-imu-calibration"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?