前言在上一篇《RS6130 点云采集与超低功耗实测》中我们实现了动态帧率控制。但还有一个核心功能没有展开——模组自带的9轴IMU。为什么雷达模组要集成IMU安装补偿用户安装时不可能保证模组绝对水平IMU可以估计安装姿态对点云做坐标旋转补偿振动抑制陀螺仪检测环境振动辅助雷达滤除地面震动、大功率音响等干扰航向参考磁力计提供绝对方向与陀螺仪融合后得到稳定的偏航角本文详细介绍QMI8658A6轴IMU MMC5603NJ3轴磁力计​ 的驱动实现以及基于互补滤波的9轴姿态融合算法。所有代码均在模组上实测通过。一、硬件连接器件I2C地址功能中断引脚QMI8658A0x6B6轴IMU加速度计陀螺仪INT2 → PA8上升沿触发MMC5603NJ0x303轴磁力计无中断轮询读取关键设计QMI8658A 的 DRDY数据就绪信号固定在 INT2 引脚输出本板 INT2 已接 PA8两个传感器共用同一路 I2C 总线通过切换从机地址分时访问磁力计采样期间禁用 PA8 中断避免 I2C 操作与中断竞争导致死锁二、软件架构主循环imu9_task │ ├─ 等待 QMI8658A DRDY 中断信号量 │ ├─ 读取加速度计 陀螺仪 温度连续12字节 │ ├─ 陀螺仪零偏校准上电强制平均 运行时慢速跟踪 │ ├─ 每100帧采样一次磁力计单次测量模式 │ └─ 倾斜补偿后用于修正 yaw 漂移 │ └─ 互补滤波融合陀螺仪积分 加速度计修正 roll/pitch 磁力计修正 yaw三、核心代码3.1 宏定义与全局变量/* 磁力计 MMC5603NJ 寄存器 */ #define MAG_I2C_ADDR 0x30U #define MAG_REG_XOUT0 0x00U /* X 轴数据 [19:12] */ #define MAG_REG_XOUT1 0x01U /* X 轴数据 [11:4] */ #define MAG_REG_YOUT0 0x02U #define MAG_REG_YOUT1 0x03U #define MAG_REG_ZOUT0 0x04U #define MAG_REG_ZOUT1 0x05U #define MAG_REG_XOUT2 0x06U /* X 轴数据 [3:0] */ #define MAG_REG_YOUT2 0x07U #define MAG_REG_ZOUT2 0x08U #define MAG_REG_CTRL0 0x1BU /* bit0 TMM: 写1触发单次测量 */ #define MAG_REG_PRODUCT_ID 0x39U /* WHO_AM_I, 期望 0x10 */ #define MAG_PRODUCT_ID_EXPECT 0x10U /* IMU QMI8658A 寄存器 */ #define QMI8658A_I2C_ADDR 0x6BU #define QMI8658A_REG_WHO_AM_I 0x00U /* 期望 0x05 */ #define QMI8658A_REG_CTRL1 0x02U /* bit6:地址自增, bit4:INT2使能 */ #define QMI8658A_REG_CTRL2 0x03U /* 加速度计量程ODR */ #define QMI8658A_REG_CTRL3 0x04U /* 陀螺仪量程ODR */ #define QMI8658A_REG_CTRL7 0x08U /* 传感器使能DRDY控制 */ #define QMI8658A_REG_CTRL8 0x09U /* 活动中断控制 */ #define QMI8658A_REG_CTRL9 0x0AU /* 命令寄存器 */ #define QMI8658A_REG_STATUS0 0x2EU /* bit0ACC就绪, bit1GYR就绪 */ #define QMI8658A_REG_AX_L 0x35U /* 连续读12字节(acc6gyro6) */ #define QMI8658A_REG_TEMP_L 0x33U /* 温度低字节(16bit有符号小端) */ #define QMI8658A_REG_RESET 0x60U /* 写0xB0软复位 */ #define QMI8658A_WHO_AM_I_EXPECT 0x05U /* QMI8658A 配置参数 */ #define QMI8658A_CTRL1_AUTO_INC_INT2 (0x40U | 0x10U) /* 地址自增 INT2使能 */ #define QMI8658A_CTRL2_8G_500HZ 0x21U /* ±8g 500Hz */ #define QMI8658A_CTRL3_512DPS_448HZ 0x54U /* ±512dps 448Hz */ #define QMI8658A_CTRL7_ACC_GYRO_EN 0x03U /* ACC_EN|GYR_EN, DRDY使能 */ #define QMI8658A_CTRL8_DISABLE 0x00U /* 活动中断全部关闭 */ #define QMI8658A_CTRL9_ACK 0x00U /* 不触发任何命令 */ /* 物理量换算 */ #define ACC_LSB_PER_G 4096.0f /* ±8g: 32768/8 */ #define GYRO_DPS_PER_LSB (512.0f / 32768.0f) /* ±512dps */ #define MAG_UT_PER_LSB 0.00625f /* 1LSB 1/16384 G 0.00625 μT */ #define MAG_NULL_FIELD 524288 /* 20bit unsigned 零场中点 */ /* 滤波参数 */ #define FUSION_ALPHA 0.98f /* 陀螺仪姿态权重(roll/pitch) */ #define FUSION_ALPHA_YAW 0.96f /* 陀螺仪偏航权重 */ #define GYRO_STATIC_THRESH_LSB 500 /* 静止判定: 去偏角速度~7.8dps */ #define GYRO_BIAS_FORCE_FRAMES 50 /* 上电强制校准帧数 */ #define GYRO_BIAS_TRACK_ALPHA 0.01f /* 运行时零偏跟踪速率 */ #define MAG_SAMPLE_PERIOD 100 /* 每100帧采样一次磁力计 */ #define PRINT_PERIOD 50 /* 每50帧打印一次融合结果 */ #define DEFAULT_DT_SEC (1.0f / 448.0f) /* 默认帧间隔 */ /* 磁力计坐标轴对齐PCB正反面贴装需要翻转 */ #define MAG_FLIP_X /* 全局姿态状态 */ static float g_roll 0.0f; /* 横滚角(度) */ static float g_pitch 0.0f; /* 俯仰角(度) */ static float g_yaw 0.0f; /* 偏航角(度) */ static float g_gyroBias[3] {0.0f, 0.0f, 0.0f}; /* 陀螺仪零点(LSB) */ static float g_magVec[3] {0.0f, 0.0f, 0.0f}; /* 归一化磁场方向 */ static int g_magValid 0; /* 磁力计数据有效标志 */ static int g_magFrameCnt 0; /* 磁力计采样帧计数 */ static int g_calibCnt 0; /* 零偏校准累计帧数 */ static int g_printCnt 0; /* 打印帧计数 */ static int16_t g_tempRaw 0; /* 最近一次温度原始值 */ static uint32_t g_lastTick 0; /* 上一帧 tick */3.2 陀螺仪零偏校准/* * 陀螺仪零偏校准含温度漂移跟踪 * * 阶段1前 GYRO_BIAS_FORCE_FRAMES 帧无条件递增平均。 * MEMS 陀螺仪零偏可达 1~2dps约 64~128LSB如果用静止判定 * 阈值 40LSB会让校准永不启动。故上电后直接强制平均 * 前提是上电时设备保持静止。 * * 阶段2之后静止时慢速跟踪温度漂移。 * 静止判定用去偏后的角速度避免大零偏导致判定永远不满足。 */ static void gyro_bias_update(int16_t gx, int16_t gy, int16_t gz) { float g[3] {(float)gx, (float)gy, (float)gz}; if (g_calibCnt GYRO_BIAS_FORCE_FRAMES) { /* 上电强制校准无条件递增平均 */ g_calibCnt; g_gyroBias[0] (g[0] - g_gyroBias[0]) / (float)g_calibCnt; g_gyroBias[1] (g[1] - g_gyroBias[1]) / (float)g_calibCnt; g_gyroBias[2] (g[2] - g_gyroBias[2]) / (float)g_calibCnt; } else { /* 运行中静止判定用去偏后的角速度 */ float gc0 g[0] - g_gyroBias[0]; float gc1 g[1] - g_gyroBias[1]; float gc2 g[2] - g_gyroBias[2]; if (fabsf(gc0) GYRO_STATIC_THRESH_LSB fabsf(gc1) GYRO_STATIC_THRESH_LSB fabsf(gc2) GYRO_STATIC_THRESH_LSB) { /* 静止慢速跟踪温度漂移 */ g_gyroBias[0] gc0 * GYRO_BIAS_TRACK_ALPHA; g_gyroBias[1] gc1 * GYRO_BIAS_TRACK_ALPHA; g_gyroBias[2] gc2 * GYRO_BIAS_TRACK_ALPHA; } } }3.3 互补滤波融合/* * 互补滤波陀螺仪积分 加速度计修正 roll/pitch 磁力计修正 yaw * * 原理 * - 陀螺仪短时精度高但有积分漂移 * - 加速度计可确定重力方向roll/pitch但受运动加速度干扰 * - 磁力计可确定绝对航向yaw但受环境磁场干扰 * - 三者通过互补权重融合各取所长 */ static void fusion_update(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz, float dt) { float axg (float)ax / ACC_LSB_PER_G; float ayg (float)ay / ACC_LSB_PER_G; float azg (float)az / ACC_LSB_PER_G; /* 陀螺仪去除零偏后转 dps */ float gxr ((float)gx - g_gyroBias[0]) * GYRO_DPS_PER_LSB; float gyr ((float)gy - g_gyroBias[1]) * GYRO_DPS_PER_LSB; float gzr ((float)gz - g_gyroBias[2]) * GYRO_DPS_PER_LSB; /* 加速度计参考姿态度 */ float roll_ref atan2f(ayg, azg) * 180.0f / (float)M_PI; float pitch_ref atan2f(-axg, sqrtf(ayg*ayg azg*azg)) * 180.0f / (float)M_PI; /* 加速度计健康检查模长偏离1g过大时降低修正权重 */ float acc_norm sqrtf(axg*axg ayg*ayg azg*azg); float acc_weight (fabsf(acc_norm - 1.0f) 0.15f) ? 0.1f : 1.0f; /* 互补融合 roll/pitch */ g_roll FUSION_ALPHA * (g_roll gxr * dt) (1.0f - FUSION_ALPHA) * acc_weight * roll_ref; g_pitch FUSION_ALPHA * (g_pitch gyr * dt) (1.0f - FUSION_ALPHA) * acc_weight * pitch_ref; /* 磁力计参考 yaw倾斜补偿后求航向 */ if (g_magValid) { float rr g_roll * (float)M_PI / 180.0f; float pr g_pitch * (float)M_PI / 180.0f; float bx g_magVec[0] * cosf(pr) g_magVec[2] * sinf(pr); float by g_magVec[0] * sinf(rr)*sinf(pr) g_magVec[1] * cosf(rr) - g_magVec[2] * sinf(rr)*cosf(pr); float yaw_ref atan2f(-by, bx) * 180.0f / (float)M_PI; /* 角度差修正避免360度环绕跳变 */ float d yaw_ref - g_yaw; while (d 180.0f) d - 360.0f; while (d -180.0f) d 360.0f; g_yaw (1.0f - FUSION_ALPHA_YAW) * d; } /* 陀螺仪积分 yaw */ g_yaw gzr * dt; /* 归一化到 [-180, 180) */ while (g_yaw 180.0f) g_yaw - 360.0f; while (g_yaw -180.0f) g_yaw 360.0f; }3.4 磁力计读取/* * 触发 MMC5603NJ 单次测量并读取 20bit 磁场数据 * * 注意磁力计与 IMU 在 PCB 正反面贴装坐标系存在镜像。 * 通过 MAG_FLIP_X/Y/Z 宏选择翻转轴使磁力计轴系与 IMU 一致。 */ static void mag_read(int32_t *mx, int32_t *my, int32_t *mz, uint8_t *ov) { uint8_t mag[9]; int32_t v[3]; /* 触发单次测量CTRL0 bit0 TMM 1 */ i2c_write_reg(pi2cDev, MAG_REG_CTRL0, 0x01); /* 等待测量完成典型 ~2ms */ OSI_MSleep(5); /* 读取 0x00~0x08 共 9 字节原始数据 */ i2c_read_regs(pi2cDev, MAG_REG_XOUT0, mag, sizeof(mag)); /* 拼装 20-bit 无符号数据减去零场偏移转为有符号 */ v[0] (mag[0] 12) | (mag[1] 4) | (mag[6] 4); v[1] (mag[2] 12) | (mag[3] 4) | (mag[7] 4); v[2] (mag[4] 12) | (mag[5] 4) | (mag[8] 4); v[0] - MAG_NULL_FIELD; v[1] - MAG_NULL_FIELD; v[2] - MAG_NULL_FIELD; /* 饱和检测 */ if (ov ! NULL) { *ov (v[0] 400000 || v[0] -400000 || v[1] 400000 || v[1] -400000 || v[2] 400000 || v[2] -400000) ? 1 : 0; } /* 坐标轴变换磁力计坐标系对齐到 IMU */ #if defined(MAG_FLIP_X) v[1] -v[1]; v[2] -v[2]; /* 绕X翻Y→-Y, Z→-Z */ #elif defined(MAG_FLIP_Y) v[0] -v[0]; v[2] -v[2]; /* 绕Y翻X→-X, Z→-Z */ #elif defined(MAG_FLIP_Z) v[0] -v[0]; v[1] -v[1]; /* 绕Z翻X→-X, Y→-Y */ #endif *mx v[0]; *my v[1]; *mz v[2]; }3.5 主循环static void imu9_task(void *arg) { (void)arg; uint8_t imu[12], tbuf[2]; int16_t ax, ay, az, gx, gy, gz; uint32_t now; float dt; while (1) { /* 1. 阻塞等待数据就绪中断448Hz约每2.2ms一次 */ if (qmi8658a_int_wait(500) 0) { /* 超时诊断 */ LOG_PRINT([WARN] int timeout #%d\n, g_intTimeoutCnt); continue; } /* 2. 读取 IMU0x35 起连续12字节acc 6 gyro 6 */ i2c_read_regs(pi2cDev, QMI8658A_REG_AX_L, imu, sizeof(imu)); ax (int16_t)((uint16_t)imu[0] | ((uint16_t)imu[1] 8)); ay (int16_t)((uint16_t)imu[2] | ((uint16_t)imu[3] 8)); az (int16_t)((uint16_t)imu[4] | ((uint16_t)imu[5] 8)); gx (int16_t)((uint16_t)imu[6] | ((uint16_t)imu[7] 8)); gy (int16_t)((uint16_t)imu[8] | ((uint16_t)imu[9] 8)); gz (int16_t)((uint16_t)imu[10] | ((uint16_t)imu[11] 8)); /* 3. 读取温度 */ i2c_read_regs(pi2cDev, QMI8658A_REG_TEMP_L, tbuf, 2); g_tempRaw (int16_t)((uint16_t)tbuf[0] | ((uint16_t)tbuf[1] 8)); /* 4. 计算帧间隔 */ now OSI_GetTicks(); dt (float)OSI_TicksToMSecs(now - g_lastTick) / 1000.0f; if (dt 0.0f || dt 0.1f) dt DEFAULT_DT_SEC; g_lastTick now; /* 5. 陀螺仪零偏校准 */ gyro_bias_update(gx, gy, gz); /* 6. 周期性采样磁力计 */ if (g_magFrameCnt MAG_SAMPLE_PERIOD) { int32_t mx, my, mz; uint8_t ov; g_magFrameCnt 0; /* 磁力计采样期间禁用 PA8 中断避免竞争 */ if (s_qmiGpioDev ! NULL) { HAL_GPIO_DisableIRQ(s_qmiGpioDev, QMI8658A_INT_PIN, 1); } /* 切换到磁力计 */ HAL_I2C_Open(pi2cDev, I2C_ADDR_WIDTH_7BIT, MAG_I2C_ADDR); mag_read(mx, my, mz, ov); float norm sqrtf((float)mx*mx (float)my*my (float)mz*mz); LOG_PRINT([MAG] X%7d Y%7d Z%7d | %.1f uT | 模长%.0f%s\n, mx, my, mz, norm * MAG_UT_PER_LSB, norm, ov ? [饱和] : ); if (ov 0 norm 1.0f norm 200000.0f) { g_magVec[0] (float)mx / norm; g_magVec[1] (float)my / norm; g_magVec[2] (float)mz / norm; g_magValid 1; } else { g_magValid 0; } /* 切回 IMU重新使能中断 */ HAL_I2C_Open(pi2cDev, I2C_ADDR_WIDTH_7BIT, QMI8658A_I2C_ADDR); if (s_qmiGpioDev ! NULL) { HAL_GPIO_EnableIRQ(s_qmiGpioDev, QMI8658A_INT_PIN); } } /* 7. 互补滤波融合 */ fusion_update(ax, ay, az, gx, gy, gz, dt); /* 8. 周期打印 */ if (g_printCnt PRINT_PERIOD) { g_printCnt 0; float temp_c (float)g_tempRaw / 256.0f; float wx ((float)gx - g_gyroBias[0]) * GYRO_DPS_PER_LSB; float wy ((float)gy - g_gyroBias[1]) * GYRO_DPS_PER_LSB; float wz ((float)gz - g_gyroBias[2]) * GYRO_DPS_PER_LSB; float bx g_gyroBias[0] * GYRO_DPS_PER_LSB; float by g_gyroBias[1] * GYRO_DPS_PER_LSB; float bz g_gyroBias[2] * GYRO_DPS_PER_LSB; LOG_PRINT([FUS] 温度%.2f°C 横滚%7.2f 俯仰%7.2f 偏航%7.2f | 角速度%7.2f %7.2f %7.2f dps | 零偏%6.2f %6.2f %6.2f dps%s\n, temp_c, g_roll, g_pitch, g_yaw, wx, wy, wz, bx, by, bz, (g_calibCnt GYRO_BIAS_FORCE_FRAMES) ? [校准中] : ); } } }四、实测输出与分析4.1 上电初始化阶段Product ID (reg 0x39) 0x10 (expect 0x10) [OK] MMC5603NJ communication works! QMI8658A WHO_AM_I (reg 0x00) 0x05 (expect 0x05) [OK] QMI8658A communication works! [OK] QMI8658A INT2(PA8) DRDY interrupt enabled两个传感器的 I2C 通讯验证通过数据就绪中断已启用。4.2 零偏校准过程[FUS] 温度29.69°C 横滚 3.27 俯仰 -12.90 偏航 -0.17 | 角速度 -4.68 -1.65 2.24 dps | 零偏 1.09 11.43 -4.60 dps [校准中] [FUS] 温度29.70°C 横滚 3.90 俯仰 -17.84 偏航 5.25 | 角速度 -3.82 -1.60 3.41 dps | 零偏 0.90 10.81 -4.10 dps [校准中]可以看到陀螺仪 Y 轴零偏高达11.43 dps这就是 MEMS 器件的典型表现随着强制校准进行零偏逐渐收敛角速度值从原始的约 -16 dps未显示变为去偏后的 -4.68 dps4.3 稳定后的融合结果复制[FUS] 温度29.73°C 横滚 4.89 俯仰 -20.37 偏航 121.26 | 角速度 0.14 -1.95 0.70 dps | 零偏 -2.04 6.73 -0.47 dps [FUS] 温度29.72°C 横滚 4.77 俯仰 -20.36 偏航 121.26 | 角速度 -1.85 -0.59 -0.42 dps | 零偏 -1.91 6.89 -0.58 dps [FUS] 温度29.71°C 横滚 4.80 俯仰 -20.33 偏航 121.31 | 角速度 0.47 0.90 0.37 dps | 零偏 -1.87 6.81 -0.16 dps稳定后横滚角 ≈ 4.8°模组安装略有倾斜俯仰角 ≈ -20.3°模组明显前倾可能固定在倾斜的墙面上偏航角 ≈ 121°模组朝向东南方向角速度接近 0设备静止陀螺仪输出正确归零温度稳定在 29.7°C温漂已基本消除4.4 磁力计采样日志复制[MAG] 磁场X -1885 Y -5411 Z 5145 | 48.1 uT | 模长7701 [MAG] 磁场X -1938 Y -5377 Z 4821 | 46.7 uT | 模长7477 [MAG] 磁场X -1900 Y -5400 Z 4900 | 47.0 uT | 模长7528磁场强度约47 μT符合地磁场典型范围25~65 μT模长稳定在 7500 左右说明环境磁场干扰较小磁力计数据有效可用于修正 yaw 漂移五、关键技术要点5.1 中断与 I2C 竞争处理磁力计采样时需要切换到另一个 I2C 从机地址。如果在此期间 448Hz 的 DRDY 中断持续触发中断服务程序中的信号量释放可能与 I2C 操作竞争导致死锁。解决方案磁力计采样前禁用 PA8 中断采样完成后重新使能。最多丢失 1 个 DRDY 脉冲下一帧自动补上。/* 采样前禁用中断 */ HAL_GPIO_DisableIRQ(gpioDev, PIN, 1); /* 切换从机、读取磁力计 */ HAL_I2C_Open(dev, ADDR, MAG_ADDR); mag_read(...); HAL_I2C_Open(dev, ADDR, IMU_ADDR); /* 采样后重新使能中断 */ HAL_GPIO_EnableIRQ(gpioDev, PIN);5.2 陀螺仪零偏校准策略MEMS 陀螺仪存在两大问题上电零偏不确定每次上电零偏不同可达 1~2dps温度漂移工作时温度变化会引起零偏缓慢漂移本方案的处理上电后前 50 帧强制平均假设设备静止之后用去偏角速度做静止判定静止时慢速跟踪跟踪速率 α0.01兼顾响应速度和稳定性5.3 加速度计健康检查当设备剧烈运动时加速度计测量的不仅是重力还包括运动加速度。直接用此时的加速度计数据修正姿态会引入错误。解决方案检查加速度计模长是否接近 1g。偏离超过 15% 时将修正权重降至 0.1让陀螺仪积分主导。float acc_norm sqrtf(axg*axg ayg*ayg azg*azg); float acc_weight (fabsf(acc_norm - 1.0f) 0.15f) ? 0.1f : 1.0f;六、下篇预告《9轴IMU修正雷达点云坐标让安装不再需要调水平》​ — 利用本文实现的姿态角对雷达点云做坐标旋转补偿由于手贱手出汗时摸了一下芯片厂的开发底板坏了近300元呀所以只好等下周新的模组焊好及自己的底板回来再继续了