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 +20 -6
2 changed files with 20 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit b0a5041261 - Show all commits

24
src/icm42688.c
View File

@ -45,6 +45,9 @@ static uint8_t rreg(uint8_t reg) {
cs_low();
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 100);
cs_high();
/* DCache coherency: invalidate rx so CPU reads SPI-written SRAM, not stale cache.
* No-op when DCache is disabled; required if DCache is on (e.g. SCB_EnableDCache). */
SCB_InvalidateDCache_by_Addr((uint32_t *)(uintptr_t)rx, (int32_t)sizeof(rx));
return rx[1];
}
@ -104,8 +107,10 @@ int icm42688_init(void) {
HAL_GPIO_Init(MPU_CS_PORT, &gpio);
cs_high();
/* DCache is disabled at startup in main.c before USB/peripherals init.
* No SCB_DisableDCache() here — calling it after USB starts corrupts USB state. */
/* DCache: main.c does NOT call SCB_EnableDCache(), so DCache is currently OFF.
* If DCache is ever enabled, all SPI rx buffers need SCB_InvalidateDCache_by_Addr()
* (already added in rreg() and icm42688_read()) and USB buffers must remain mapped
* non-cacheable via MPU Region 0 in usbd_conf.c. */
hspi1.Instance = SPI1;
hspi1.Init.Mode = SPI_MODE_MASTER;
@ -127,7 +132,13 @@ int icm42688_init(void) {
wreg(0x6B, 0x01); /* Wake, PLL */
HAL_Delay(50);
uint8_t who = rreg(MPU_REG_WHO_AM_I);
/* Retry WHO_AM_I up to 3 times: a single SPI glitch returning 0x00
* would otherwise abort init and prevent calibration from ever running. */
uint8_t who = 0;
for (int attempt = 0; attempt < 3 && who == 0; attempt++) {
if (attempt > 0) HAL_Delay(10);
who = rreg(MPU_REG_WHO_AM_I);
}
tr(who); /* trace[0] */
int ret;
@ -153,12 +164,15 @@ int icm42688_init(void) {
void icm42688_read(icm42688_data_t *d) {
if (imu_type == 1) {
/* MPU6000: ACCEL_XOUT_H (0x3B) → 14 bytes: accel(6)+temp(2)+gyro(6) */
uint8_t tx[15], rx[15];
uint8_t tx[15] = {0};
uint8_t rx[15] = {0}; /* zero-init: failed SPI transfers return 0, not garbage */
tx[0] = MPU_REG_ACCEL_XOUT_H | 0x80;
for (int i = 1; i < 15; i++) tx[i] = 0x00;
cs_low();
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 15, 100);
cs_high();
/* DCache coherency: force CPU to read SPI-written SRAM, not a stale cache line.
* No-op when DCache is disabled; critical if SCB_EnableDCache() is called. */
SCB_InvalidateDCache_by_Addr((uint32_t *)(uintptr_t)rx, (int32_t)sizeof(rx));
d->ax = (int16_t)((rx[1] << 8) | rx[2]);
d->ay = (int16_t)((rx[3] << 8) | rx[4]);

2
src/mpu6000.c
View File

@ -62,7 +62,7 @@ void mpu6000_calibrate(void) {
*/
int32_t sum_gx = 0, sum_gy = 0, sum_gz = 0;
for (int i = 0; i < GYRO_CAL_SAMPLES; i++) {
icm42688_data_t raw;
icm42688_data_t raw = {0}; /* zero-init: guard against icm42688_read no-op on imu_type mismatch */
icm42688_read(&raw);
sum_gx += raw.gx;
sum_gy += raw.gy;