零知IDE——STM32驱动ICM20948九轴姿态角解算（上位机演示+数据导出）

目录

一、系统接线部分

[1.1 硬件清单](#1.1 硬件清单)

[1.2 接线方案表](#1.2 接线方案表)

[1.3 具体接线图](#1.3 具体接线图)

[1.4 接线实物图](#1.4 接线实物图)

二、安装与使用部分

三、核心代码讲解

[3.1 初始化与寄存器组切换](#3.1 初始化与寄存器组切换)

[3.2 姿态解算与磁场融合](#3.2 姿态解算与磁场融合)

[3.3 滤波器收敛检测](#3.3 滤波器收敛检测)

[3.4 双协议并行输出](#3.4 双协议并行输出)

[3.5 项目完整代码](#3.5 项目完整代码)

四、项目结果演示

[4.1 操作流程](#4.1 操作流程)

[4.2 上位机搭建](#4.2 上位机搭建)

[4.3 视频演示](#4.3 视频演示)

五、ICM20948技术知识讲解

[5.1 ICM20948内部架构](#5.1 ICM20948内部架构)

[5.2 I2C通信与寄存器Bank机制](#5.2 I2C通信与寄存器Bank机制)

六、常见问题解答（FAQ）

Q1：Yaw角上电后就开始漂移，永远不收敛

Q2：Pitch接近±90°时Yaw突然跳变到任意值

---

项目概述

本项目以零知增强板 为核心控制平台，主控芯片为STM32F407VET6，采集三轴加速度、三轴陀螺仪和三轴磁力计的原始数据。结合优化的Mahony互补滤波算法，实时解算出绝对姿态角。为了实现高效的数据采集与分析，实现了双协议并行输出，将姿态数据同时传输给VOFA+上位机进行实时波形显示，并自动记录到Excel中，为后续的数据分析与算法优化提供了极大便利

项目难点及解决方案

问题描述：磁力计初始化模式错误导致Yaw持续漂移

**解决方案：**先向寄存器写入0x00的Power-down模式等待复位，再写入0x06进行100Hz连续测量模式，Yaw角得以正常收敛

一、系统接线部分

1.1 硬件清单

序号	模块名称	规格 / 型号	数量	备注
1	零知增强板	STM32F407VET6，主频168MHz	1	项目主控
2	ICM20948 模块	九轴IMU，I²C/SPI接口	1	姿态传感器
3	USB 数据线	Micro-USB	1	烧录 & 串口通信
4	杜邦线	2.0端子转公头，20cm	1	I²C连接
5	PC	Windows 10/11	1	上位机运行环境

1.2 接线方案表

本项目使用硬件I2C进行通信。ADO引脚接高电平，设备地址为0x69。请严格按照以下引脚定义进行连接

ICM20948 引脚	信号功能	零知增强板引脚	说明
VCC	电源正	3.3V	必须接3.3V，禁止接5V
SDA	I²C 数据	SDA（20）	硬件I²C1-SDA，对应Arduino Pin A4
SCL	I²C 时钟	SCL（21）	硬件I²C1-SCL，对应Arduino Pin A5
GND	电源地	GND	与主板共地
ADO	地址选择	3.3V	接高电平 → 设备地址 = 0x69
INT	数据就绪中断	悬空	本项目使用软件轮询，不接

1.3 具体接线图

VCC接零知增强板3.3V逻辑电平，避免损坏芯片

1.4 接线实物图

二、安装与使用部分

2.1 开源平台-输入"ICM20948九轴姿态角解算"并搜索-代码下载自动打开

2.2 连接-验证-上传

2.3 调试-串口监视器

三、核心代码讲解

本项目代码由三个核心文件由传感器驱动库、姿态解算库、主程序初始化和数据输出组成。以下重点讲解四个核心部分

3.1 初始化与寄存器组切换

ICM20948内部寄存器分为4个User Bank（Bank0～3）。每次访问不同Bank的寄存器前，必须通过地址0x7F（REG_BANK_SEL）切换当前Bank

void ICM20948::initICM20948()
{
    // Get stable time source
  // Auto select clock source to be PLL gyroscope reference if ready else
  writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
  delay(200);
  
  
  
  // Switch to user bank 2
  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
	
  // Configure Gyro and Thermometer
  // Disable FSYNC and set gyro bandwidth to 51.2 Hz,
  // respectively;
  // minimum delay time for this setting is 5.9 ms, which means sensor fusion
  // update rates cannot be higher than 1 / 0.0059 = 170 Hz
  // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
  // With the ICM20948, it is possible to get gyro sample rates of 32 kHz (!),
  // 8 kHz, or 1 kHz
  // Set gyroscope full scale range to 250 dps
  writeByte(ICM20948_ADDRESS, ACCEL_CONFIG_2, 0x01); // 启用加速度计DLPF
  writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x01);
  writeByte(ICM20948_ADDRESS, TEMP_CONFIG, 0x03);

  // Set sample rate = gyroscope output rate/(1 + GYRO_SMPLRT_DIV)
  // Use a 220 Hz rate; a rate consistent with the filter update rate
  // determined inset in CONFIG above.
  
  writeByte(ICM20948_ADDRESS, GYRO_SMPLRT_DIV, 0x07);

  // Set gyroscope full scale range
  // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are
  // left-shifted into positions 4:3

  // Set accelerometer full-scale range configuration
  // Get current ACCEL_CONFIG register value
  uint8_t c = readByte(ICM20948_ADDRESS, ACCEL_CONFIG);
  // c = c & ~0xE0; // Clear self-test bits [7:5]
  c = c & ~0x06;  // Clear AFS bits [4:3]
  c = c | Ascale << 1; // Set full scale range for the accelerometer
  c = c | 0x01; // Set enable accel DLPF for the accelerometer
  c = c | 0x18; // and set DLFPFCFG to 50.4 hz
  // Write new ACCEL_CONFIG register value
  writeByte(ICM20948_ADDRESS, ACCEL_CONFIG, c);

  // Set accelerometer sample rate configuration
  // It is possible to get a 4 kHz sample rate from the accelerometer by
  // choosing 1 for accel_fchoice_b bit [3]; in this case the bandwidth is
  // 1.13 kHz
  writeByte(ICM20948_ADDRESS, ACCEL_SMPLRT_DIV_2, 0x07);
  // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
  // but all these rates are further reduced by a factor of 5 to 200 Hz because
  // of the GYRO_SMPLRT_DIV setting

  // Switch to user bank 0
  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);

  // Configure Interrupts and Bypass Enable
  // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH
  // until interrupt cleared, clear on read of INT_STATUS, and enable
  // I2C_BYPASS_EN so additional chips can join the I2C bus and all can be
  // controlled by the Arduino as master.
  writeByte(ICM20948_ADDRESS, INT_PIN_CFG, 0x22);
  // Enable data ready (bit 0) interrupt
  writeByte(ICM20948_ADDRESS, INT_ENABLE_1, 0x01);
}

参数说明：

寄存器	写入值	含义
REG_BANK_SEL (0x7F)	0x00/0x10/0x20/0x30	切换到Bank0/1/2/3
GYRO_SMPLRT_DIV	0x07	陀螺仪采样率分频 = 8，实际输出速率 = 1kHz/(1+7) = 125Hz
GYRO_CONFIG_1	0x01	Bit0=1使能DLPF，进行低通滤波
ACCEL_CONFIG	`0x01	Ascale<<1

3.2 姿态解算与磁场融合

Mahony算法是一种高效的互补滤波器，它使用陀螺仪积分跟踪快速姿态变化，同时用加速度计和磁力计对长期漂移进行修正

void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
{
  // short name local variable for readability
  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
  float norm;
  float hx, hy, bx, bz;
  float vx, vy, vz, wx, wy, wz;
  float ex, ey, ez;
  float pa, pb, pc;

  // Auxiliary variables to avoid repeated arithmetic
  float q1q1 = q1 * q1;
  float q1q2 = q1 * q2;
  float q1q3 = q1 * q3;
  float q1q4 = q1 * q4;
  float q2q2 = q2 * q2;
  float q2q3 = q2 * q3;
  float q2q4 = q2 * q4;
  float q3q3 = q3 * q3;
  float q3q4 = q3 * q4;
  float q4q4 = q4 * q4;

  // Normalise accelerometer measurement
  norm = sqrt(ax * ax + ay * ay + az * az);
  if (norm == 0.0f) return; // Handle NaN
  norm = 1.0f / norm;       // Use reciprocal for division
  ax *= norm;
  ay *= norm;
  az *= norm;

  // Normalise magnetometer measurement
  norm = sqrt(mx * mx + my * my + mz * mz);
  if (norm == 0.0f) return; // Handle NaN
  norm = 1.0f / norm;       // Use reciprocal for division
  mx *= norm;
  my *= norm;
  mz *= norm;

  // Reference direction of Earth's magnetic field
  hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
  hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
  bx = sqrt((hx * hx) + (hy * hy));
  bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);

  // Estimated direction of gravity and magnetic field
  vx = 2.0f * (q2q4 - q1q3);
  vy = 2.0f * (q1q2 + q3q4);
  vz = q1q1 - q2q2 - q3q3 + q4q4;
  wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
  wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
  wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);

  // Error is cross product between estimated direction and measured direction of gravity
  ex = (ay * vz - az * vy) + (my * wz - mz * wy);
  ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
  ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
  if (Ki > 0.0f)
  {
    eInt[0] += ex;      // accumulate integral error
    eInt[1] += ey;
    eInt[2] += ez;
  }
  else
  {
    eInt[0] = 0.0f;     // prevent integral wind up
    eInt[1] = 0.0f;
    eInt[2] = 0.0f;
  }

  // Apply feedback terms
  gx = gx + Kp * ex + Ki * eInt[0];
  gy = gy + Kp * ey + Ki * eInt[1];
  gz = gz + Kp * ez + Ki * eInt[2];
 
  // Integrate rate of change of quaternion
  pa = q2;
  pb = q3;
  pc = q4;
  q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
  q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
  q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
  q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);

  // Normalise quaternion
  norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
  norm = 1.0f / norm;
  q[0] = q1 * norm;
  q[1] = q2 * norm;
  q[2] = q3 * norm;
  q[3] = q4 * norm;
}

参数说明与调优指南：

参数	默认值	作用	调大效果	调小效果
Kp (比例增益)	3.0f	加速度/磁场纠正力度	响应快，收敛快，但抖动大	响应慢，收敛慢，但更平滑
Ki (积分增益)	0.1f	消除长期陀螺仪零偏	静态误差消除快，可能积分饱和	静态误差消除慢，更稳定

3.3 滤波器收敛检测

// CONVERGE_FRAMES：收敛判定所需的连续静止帧数
// CONVERGE_THRESHOLD：加速度模长与 1g 的允许偏差
#define CONVERGE_FRAMES    80
#define CONVERGE_THRESHOLD 0.05f

// ── 滤波器收敛检测 ────────────────────────────────────────────
// 每帧在 loop() 中调用，传入当前加速度模长（单位 g）
// 连续 CONVERGE_FRAMES 帧模长偏差 < CONVERGE_THRESHOLD 即标记收敛
// 一旦收敛后不再重置（除非手动调用 resetConvergence()）
void ICM20948::updateConvergence(float accelMag)
{
  if (_converged) return; // 已收敛，直接返回，不消耗 CPU

  if (fabs(accelMag - 1.0f) < CONVERGE_THRESHOLD)
  {
    _convergeCount++;
    if (_convergeCount >= CONVERGE_FRAMES)
    {
      _converged = true;
    }
  }
  else
  {
    // 检测到加速度异常（运动或冲击），重置计数
    _convergeCount = 0;
  }
}

主程序中的调用方式

// 在loop()中，计算加速度模长并调用库函数
float accelMag = sqrt(myIMU.ax*myIMU.ax + myIMU.ay*myIMU.ay + myIMU.az*myIMU.az);
myIMU.updateConvergence(accelMag);

// 如果未收敛，只输出VOFA+数据，跳过PLX-DAQ
if (!myIMU.isFilterConverged()) {
  Serial.print(myIMU.yaw, 2); Serial.print(',');
  Serial.print(myIMU.pitch, 2); Serial.print(',');
  Serial.println(myIMU.roll, 2);
  return; // 跳过PLX-DAQ数据帧
}

// 收敛后，正常输出VOFA+和PLX-DAQ双协议帧

3.4 双协议并行输出

利用两套上位机的容错机制，实现了单串口双协议输出

if (myIMU.delt_t > 60)
    {
      // ── 四元数转欧拉角 ──────────────────────────────────
      myIMU.yaw = atan2(
        2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() * *(getQ()+3)),
        *getQ() * *getQ() + *(getQ()+1) * *(getQ()+1)
        - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) * *(getQ()+3)
      );
      myIMU.pitch = -asin(
        2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() * *(getQ()+2))
      );
      myIMU.roll = atan2(
        2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) * *(getQ()+3)),
        *getQ() * *getQ() - *(getQ()+1) * *(getQ()+1)
        - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) * *(getQ()+3)
      );

      myIMU.pitch *= RAD_TO_DEG;
      myIMU.yaw   *= RAD_TO_DEG;
      myIMU.roll  *= RAD_TO_DEG;

      // 修正磁偏角（深圳约 -2.5°，查询: ngdc.noaa.gov/geomag-web）
      myIMU.yaw -= 2.5;

      // ══════════════════════════════════════════════════════
      //  滤波器收敛检测（基于库函数，不在 ino 中手动计数）
      //
      //  updateConvergence() 每帧检测加速度模长是否稳定在 1g 附近，
      //  连续 CONVERGE_FRAMES 帧满足条件后 isFilterConverged() 返回 true。
      //  参数在 ICM20948.h 中配置：
      //    CONVERGE_FRAMES    = 80  （约 5 秒）
      //    CONVERGE_THRESHOLD = 0.05f
      // ══════════════════════════════════════════════════════
      float accelMag = sqrt(myIMU.ax * myIMU.ax
                          + myIMU.ay * myIMU.ay
                          + myIMU.az * myIMU.az);
      myIMU.updateConvergence(accelMag);

      if (!myIMU.isFilterConverged())
      {
        // ── 收敛期：VOFA+ 发送真实数据，但幅度受限于收敛过程 ──
        // 直接输出当前解算结果，VOFA+ 会看到姿态角从初始漂移
        // 逐渐稳定的过程（比强制零值更真实，也不会卡在 0,0,0）
        // PLX-DAQ 在此期间不写入 Excel，等待收敛后才正式记录
        Serial.print(myIMU.yaw,   2); Serial.print(",");
        Serial.print(myIMU.pitch, 2); Serial.print(",");
        Serial.print(myIMU.roll,  2); Serial.println(" ");

        myIMU.count    = millis();
        myIMU.sumCount = 0;
        myIMU.sum      = 0;
        return; // 跳过 PLX-DAQ 数据帧，等收敛后再记录
      }

      // ══════════════════════════════════════════════════════
      //  ① VOFA+ Firewater 格式（与原代码完全一致）
      //  格式: yaw,pitch,roll \n
      //  PLX-DAQ 无法识别此行，自动忽略
      // ══════════════════════════════════════════════════════
      Serial.print(myIMU.yaw,   2); Serial.print(",");
      Serial.print(myIMU.pitch, 2); Serial.print(",");
      Serial.print(myIMU.roll,  2); Serial.println(" ");

      // ══════════════════════════════════════════════════════
      //  ② PLX-DAQ v2 DATA 帧（紧跟 VOFA+ 帧之后）
      //  格式: DATA,TIME,TIMER,val1,...,lastVal\n
      //  VOFA+ 无法识别 DATA 开头的行，自动忽略
      //  TIME  ：替换为 PC 系统时间
      //  TIMER ：替换为本次连接运行秒数
      //  最后一字段用 println，换行符触发 PLX-DAQ 写入一行
      // ══════════════════════════════════════════════════════
      Serial.print("DATA,TIME,TIMER,");

      // 姿态角（°，2 位小数）
      Serial.print(myIMU.yaw,   2); Serial.print(",");
      Serial.print(myIMU.pitch, 2); Serial.print(",");
      Serial.print(myIMU.roll,  2); Serial.print(",");

      // 加速度（g，4 位小数）
      Serial.print(myIMU.ax, 4); Serial.print(",");
      Serial.print(myIMU.ay, 4); Serial.print(",");
      Serial.print(myIMU.az, 4); Serial.print(",");

      // 陀螺仪（°/s，3 位小数）
      Serial.print(myIMU.gx, 3); Serial.print(",");
      Serial.print(myIMU.gy, 3); Serial.print(",");
      Serial.print(myIMU.gz, 3); Serial.print(",");

      // 磁力计（mG，2 位小数）最后一字段用 println
      Serial.print(myIMU.mx,  2); Serial.print(",");
      Serial.print(myIMU.my,  2); Serial.print(",");
      Serial.println(myIMU.mz, 2);

      myIMU.count    = millis();
      myIMU.sumCount = 0;
      myIMU.sum      = 0;
      digitalWrite(myLed, !digitalRead(myLed));
    }

3.5 项目完整代码

/**************************************************************************************
 * 文件: /ICM20948_PLXDAQ_VOFA/ICM20948_PLXDAQ_VOFA.ino
 * 作者：零知实验室（深圳市在芯间科技有限公司）
 * -^^- 零知实验室，让电子制作变得更简单！ -^^-
 * 时间: 2026-3-24
 * 功能介绍: ICM20948 九轴传感器姿态解算，同时兼容 VOFA+ Firewater
 *           上位机实时显示与 PLX-DAQ v2 Excel 数据记录，含初始化
 *           静态漂移抑制功能。
 **************************************************************************************/

#include "AHRSAlgorithms.h"
#include "ICM20948.h"

// true  = 输出 Mahony 解算后的姿态角（Yaw / Pitch / Roll）
// false = 输出原始加速度、陀螺仪、磁力计读数（调试用）
#define AHRS true

int myLed = LED_BUILTIN;

ICM20948 myIMU;

void setup()
{
  Wire.begin();
  Serial.begin(115200);

  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, HIGH);

  if (myIMU.isInI2cMode())
    Serial.println("传感器使用 I2C 接口");
  else
    Serial.println("传感器使用 SPI 接口");

  // ── 复位 ICM20948 ──────────────────────────────────────────
  myIMU.writeByte(ICM20948_ADDRESS, PWR_MGMT_1, READ_FLAG);
  delay(100);
  myIMU.writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
  delay(100);

  // ── 读取 WHO_AM_I，验证通信是否正常 ──────────────────────
  byte c = myIMU.readByte(ICM20948_ADDRESS, WHO_AM_I_ICM20948);
  Serial.print(F("ICM20948 设备ID: 0x")); Serial.println(c, HEX);
  Serial.print(F("期望值:          0x")); Serial.println(0xEA, HEX);

  if (c == 0xEA)
  {
    Serial.println(F("ICM20948 在线，开始初始化..."));

    // ── 自检 ──────────────────────────────────────────────────
    myIMU.ICM20948SelfTest(myIMU.selfTest);
    Serial.print(F("加速度 X 轴自检偏差: ")); Serial.print(myIMU.selfTest[0], 1); Serial.println("%");
    Serial.print(F("加速度 Y 轴自检偏差: ")); Serial.print(myIMU.selfTest[1], 1); Serial.println("%");
    Serial.print(F("加速度 Z 轴自检偏差: ")); Serial.print(myIMU.selfTest[2], 1); Serial.println("%");
    Serial.print(F("陀螺仪 X 轴自检偏差: ")); Serial.print(myIMU.selfTest[3], 1); Serial.println("%");
    Serial.print(F("陀螺仪 Y 轴自检偏差: ")); Serial.print(myIMU.selfTest[4], 1); Serial.println("%");
    Serial.print(F("陀螺仪 Z 轴自检偏差: ")); Serial.print(myIMU.selfTest[5], 1); Serial.println("%");

    // ── 陀螺仪和加速度计校准，偏置写入硬件寄存器 ──────────
    myIMU.calibrateICM20948(myIMU.gyroBias, myIMU.accelBias);
    myIMU.initICM20948();
    Serial.println("ICM20948 初始化完成");

    // ── 验证磁力计通信 ─────────────────────────────────────
    byte d = myIMU.readByte(AK09916_ADDRESS, WHO_AM_I_AK09916);
    Serial.print("AK09916 设备ID: 0x"); Serial.println(d, HEX);
    Serial.print("期望值:         0x"); Serial.println(0x09, HEX);

    if (d != 0x09)
    {
      Serial.println(F("磁力计通信失败，程序终止！"));
      Serial.flush();
      abort();
    }

    // ── 磁力计初始化（库已修正为 100Hz 连续模式）──────────
    myIMU.initAK09916();
    Serial.println("AK09916 初始化完成");

    // ── 获取各传感器量程分辨率（只需执行一次）─────────────
    myIMU.getAres();
    myIMU.getGres();
    myIMU.getMres();

    // ── 磁力计偏置与比例校准 ───────────────────────────────
    // MAG_CAL_FAST=true  约 6 秒（在 ICM20948.h 中配置）
    // MAG_CAL_FAST=false 约 22 秒
    // 校准期间库函数每 50 帧打印一次进度，VOFA+ / PLX-DAQ 连接不会超时
    myIMU.magCalICM20948(myIMU.magBias, myIMU.magScale);
    Serial.println("磁偏置校准值 (mG):");
    Serial.println(myIMU.magBias[0]);
    Serial.println(myIMU.magBias[1]);
    Serial.println(myIMU.magBias[2]);

    delay(500);

    // ══════════════════════════════════════════════════════════
    //  PLX-DAQ 初始化指令
    //  CLEARDATA  ：清空工作表原有数据
    //  LABEL      ：设置列标题（粗体）
    //  RESETTIMER ：内部计时器归零
    // ══════════════════════════════════════════════════════════
    Serial.println("CLEARDATA");
    Serial.println("LABEL,PC time, running time(s),Yaw(deg),Pitch(deg),Roll(deg),"
                   "Ax(g),Ay(g),Az(g),Gx(dps),Gy(dps),Gz(dps),Mx(mG),My(mG),Mz(mG)");
    Serial.println("RESETTIMER");
  }
  else
  {
    Serial.print("无法连接到 ICM20948，设备ID: 0x");
    Serial.println(c, HEX);
    Serial.println(F("通信失败，程序终止！"));
    Serial.flush();
    abort();
  }
}

void loop()
{
  // ── 检查数据就绪中断标志位 ────────────────────────────────
  if (myIMU.readByte(ICM20948_ADDRESS, INT_STATUS_1) & 0x01)
  {
    // 读取加速度计，转换为 g
    myIMU.readAccelData(myIMU.accelCount);
    myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes;
    myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes;
    myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes;

    // 读取陀螺仪，转换为 °/s
    myIMU.readGyroData(myIMU.gyroCount);
    myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
    myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
    myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;

    // 读取磁力计，转换为 mG 并减去偏置
    myIMU.readMagData(myIMU.magCount);
    myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes - myIMU.magBias[0];
    myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes - myIMU.magBias[1];
    myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes - myIMU.magBias[2];
  }

  // 更新积分时间间隔（必须在四元数更新前调用）
  myIMU.updateTime();

  // ── Mahony 姿态解算 ───────────────────────────────────────
  // 注意：ICM20948 加速度计与磁力计轴向存在对准偏差，
  // 按数据手册约定交换 mx/my 轴，使传感器 X 轴朝向正前方
  MahonyQuaternionUpdate(
    myIMU.ax, myIMU.ay, myIMU.az,
    myIMU.gx * DEG_TO_RAD, myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD,
    myIMU.my, myIMU.mx, myIMU.mz,
    myIMU.deltat
  );

  // ── 原始数据调试模式（AHRS = false，500ms 一次）──────────
  if (!AHRS)
  {
    myIMU.delt_t = millis() - myIMU.count;
    if (myIMU.delt_t > 500)
    {
      Serial.print("加速度 X: "); Serial.print(1000 * myIMU.ax); Serial.print(" mg  ");
      Serial.print("Y: ");        Serial.print(1000 * myIMU.ay); Serial.print(" mg  ");
      Serial.print("Z: ");        Serial.print(1000 * myIMU.az); Serial.println(" mg");
      Serial.print("陀螺仪 X: "); Serial.print(myIMU.gx, 3);    Serial.print(" °/s  ");
      Serial.print("Y: ");        Serial.print(myIMU.gy, 3);    Serial.print(" °/s  ");
      Serial.print("Z: ");        Serial.print(myIMU.gz, 3);    Serial.println(" °/s");
      Serial.print("磁力计 X: "); Serial.print(myIMU.mx);        Serial.print(" mG  ");
      Serial.print("Y: ");        Serial.print(myIMU.my);        Serial.print(" mG  ");
      Serial.print("Z: ");        Serial.print(myIMU.mz);        Serial.println(" mG");

      myIMU.tempCount   = myIMU.readTempData();
      myIMU.temperature = ((float)myIMU.tempCount) / 333.87 + 21.0;
      Serial.print("温度: "); Serial.print(myIMU.temperature, 1); Serial.println(" °C");

      myIMU.count = millis();
      digitalWrite(myLed, !digitalRead(myLed));
    }
  }

  // ── AHRS 姿态角输出模式（AHRS = true，60ms 一次）─────────
  else
  {
    myIMU.delt_t = millis() - myIMU.count;

    if (myIMU.delt_t > 60)
    {
      // ── 四元数转欧拉角 ──────────────────────────────────
      myIMU.yaw = atan2(
        2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() * *(getQ()+3)),
        *getQ() * *getQ() + *(getQ()+1) * *(getQ()+1)
        - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) * *(getQ()+3)
      );
      myIMU.pitch = -asin(
        2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() * *(getQ()+2))
      );
      myIMU.roll = atan2(
        2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) * *(getQ()+3)),
        *getQ() * *getQ() - *(getQ()+1) * *(getQ()+1)
        - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) * *(getQ()+3)
      );

      myIMU.pitch *= RAD_TO_DEG;
      myIMU.yaw   *= RAD_TO_DEG;
      myIMU.roll  *= RAD_TO_DEG;

      // 修正磁偏角（深圳约 -2.5°，查询: ngdc.noaa.gov/geomag-web）
      myIMU.yaw -= 2.5;

      // ══════════════════════════════════════════════════════
      //  滤波器收敛检测（基于库函数，不在 ino 中手动计数）
      //
      //  updateConvergence() 每帧检测加速度模长是否稳定在 1g 附近，
      //  连续 CONVERGE_FRAMES 帧满足条件后 isFilterConverged() 返回 true。
      //  参数在 ICM20948.h 中配置：
      //    CONVERGE_FRAMES    = 80  （约 5 秒）
      //    CONVERGE_THRESHOLD = 0.05f
      // ══════════════════════════════════════════════════════
      float accelMag = sqrt(myIMU.ax * myIMU.ax
                          + myIMU.ay * myIMU.ay
                          + myIMU.az * myIMU.az);
      myIMU.updateConvergence(accelMag);

      if (!myIMU.isFilterConverged())
      {
        // ── 收敛期：VOFA+ 发送真实数据，但幅度受限于收敛过程 ──
        // 直接输出当前解算结果，VOFA+ 会看到姿态角从初始漂移
        // 逐渐稳定的过程（比强制零值更真实，也不会卡在 0,0,0）
        // PLX-DAQ 在此期间不写入 Excel，等待收敛后才正式记录
        Serial.print(myIMU.yaw,   2); Serial.print(",");
        Serial.print(myIMU.pitch, 2); Serial.print(",");
        Serial.print(myIMU.roll,  2); Serial.println(" ");

        myIMU.count    = millis();
        myIMU.sumCount = 0;
        myIMU.sum      = 0;
        return; // 跳过 PLX-DAQ 数据帧，等收敛后再记录
      }

      // ══════════════════════════════════════════════════════
      //  ① VOFA+ Firewater 格式（与原代码完全一致）
      //  格式: yaw,pitch,roll \n
      //  PLX-DAQ 无法识别此行，自动忽略
      // ══════════════════════════════════════════════════════
      Serial.print(myIMU.yaw,   2); Serial.print(",");
      Serial.print(myIMU.pitch, 2); Serial.print(",");
      Serial.print(myIMU.roll,  2); Serial.println(" ");

      // ══════════════════════════════════════════════════════
      //  ② PLX-DAQ v2 DATA 帧（紧跟 VOFA+ 帧之后）
      //  格式: DATA,TIME,TIMER,val1,...,lastVal\n
      //  VOFA+ 无法识别 DATA 开头的行，自动忽略
      //  TIME  ：替换为 PC 系统时间
      //  TIMER ：替换为本次连接运行秒数
      //  最后一字段用 println，换行符触发 PLX-DAQ 写入一行
      // ══════════════════════════════════════════════════════
      Serial.print("DATA,TIME,TIMER,");

      // 姿态角（°，2 位小数）
      Serial.print(myIMU.yaw,   2); Serial.print(",");
      Serial.print(myIMU.pitch, 2); Serial.print(",");
      Serial.print(myIMU.roll,  2); Serial.print(",");

      // 加速度（g，4 位小数）
      Serial.print(myIMU.ax, 4); Serial.print(",");
      Serial.print(myIMU.ay, 4); Serial.print(",");
      Serial.print(myIMU.az, 4); Serial.print(",");

      // 陀螺仪（°/s，3 位小数）
      Serial.print(myIMU.gx, 3); Serial.print(",");
      Serial.print(myIMU.gy, 3); Serial.print(",");
      Serial.print(myIMU.gz, 3); Serial.print(",");

      // 磁力计（mG，2 位小数）最后一字段用 println
      Serial.print(myIMU.mx,  2); Serial.print(",");
      Serial.print(myIMU.my,  2); Serial.print(",");
      Serial.println(myIMU.mz, 2);

      myIMU.count    = millis();
      myIMU.sumCount = 0;
      myIMU.sum      = 0;
      digitalWrite(myLed, !digitalRead(myLed));
    }
  }
}

/******************************************************************************
 * 深圳市在芯间科技有限公司
 * 淘宝店铺：在芯间科技零知板
 * 店铺网址：https://shop533070398.taobao.com
 * 版权说明：
 *  1.本代码的版权归【深圳市在芯间科技有限公司】所有，仅限个人非商业性学习使用。
 *  2.严禁将本代码或其衍生版本用于任何商业用途（包括但不限于产品开发、付费服务、企业内部使用等）。
 *  3.任何商业用途均需事先获得【深圳市在芯间科技有限公司】的书面授权，未经授权的商业使用行为将被视为侵权。
******************************************************************************/

系统流程图

四、项目结果演示

4.1 操作流程

编译上传：在零知IDE中选择「零知增强板」，编译并上传代码。等待初始化完成，串口输出"磁力计校准完成！"

磁力计校准

收到"磁力计快速校准：请以8字形缓慢转动设备"提示后，将设备以8字形转动约6秒，让磁力计采集各方向数据

V OFA+实时显示

打开VOFA+，选择正确COM口，波特率115200，协议选Firewater。点击连接，即可看到Yaw/Pitch/Roll三通道实时波形

PLX-DAQ Excel记录

关闭串口监视器，打开PLX-DAQ v2.11.xlsm，填写COM口和波特率115200，点击Connect。Excel会自动填充数据（注意：数据会在滤波器收敛后才开始写入）

数据导出：采集完成后点击Disconnect，在Excel中另存为.xlsx文件即完成导出。

4.2 上位机搭建

（1）VOFA+ 配置

新建数据源 → 串口 → 选择对应COM口 → 波特率115200 → 协议选择FireWater

添加3D立方体控件

绑定四元数数据通道 → 设置模型缩放比例

添加波形通道

添加三个通道 → 对应Yaw / Pitch / Roll，可设置颜色

（2）PLX-DAQ v2配置

下载PLX-DAQ-v2.11.xlsm，双击打开并点击「启用内容」、Port填写COM编号，Baud填写115200、关闭零知IDE串口监视器后再点击Connect

4.3 视频演示

姿态角解算 + PLX-DAQ Excel数据导出 演示

完整演示基于零知增强板驱动ICM20948九轴传感器的项目全流程， VOFA+ Firewater协议实时三通道姿态角波形展示。PLX-DAQ v2 Excel自动记录演示（含收敛后才写入的效果对比）。数据导出为.xlsx文件并在Excel中绘制姿态角时序图

五、ICM20948技术知识讲解

ICM-20948 支持用于连接外部传感器的辅助 I²C 接口、片上 16 位模数转换器（ADC）、可编程数字滤波器、内置温度传感器以及可编程中断

5.1 ICM20948内部架构

加速度计和陀螺仪共用STM32的主I²C总线（地址0x69）

磁力计AK09916通过ICM20948内部的I²C Master桥接，或通过配置I²C Bypass模式直接访问（地址0x0C）。使用Bypass模式，使主控可以直接操作磁力计

5.2 I2C通信与寄存器Bank机制

I2C通信时序

STM32发出起始信号，发送设备地址0x69 + W，然后发送寄存器地址0x7F，再发送0x20（切换到Bank2），停止位结束。下一帧起始信号后访问Bank2中的GYRO_CONFIG_1（0x01）并写入配置值

Bank切换时序

User Bank机制里面全部寄存器分为4个Bank，通过REG_BANK_SEL（0x7F）切换

Bank	地址范围	主要内容
Bank 0 (0x00)	0x00 ~ 0x7E	传感器数据输出、中断状态、电源管理
Bank 1 (0x10)	0x00 ~ 0x53	自检寄存器、加速度计偏置
Bank 2 (0x20)	0x00 ~ 0x54	陀螺仪/加速度计配置、采样率分频、DLPF设置
Bank 3 (0x30)	0x00 ~ 0x17	I²C Master配置，AK09916桥接相关

六、常见问题解答（FAQ）

Q1：Yaw角上电后就开始漂移，永远不收敛

A：最可能的原因是磁力计未进行8字形校准 ，或校准时未充分转动设备。重新上电，在"磁力计快速校准"提示出现后认真做8字形运动约6秒。若仍然漂移，检查initAK09916()是否写入了正确的0x06模式

Q2：Pitch接近±90°时Yaw突然跳变到任意值

A：这是欧拉角表示法的万向节锁（Gimbal Lock）问题 。当Pitch=±90°时，Yaw和Roll失去区分度，atan2()输出不确定。解决方案：若应用需要大倾角场景，直接使用getQ()获取四元数进行计算，不要转换成欧拉角

项目资源整合

VOFA+ 上位机： VOFA+ 官网下载

ICM20948 数据手册： icm-20948-v1.5.pdf

PLX-DAQ v2.11.xlsm 宏数据采集插件： PLX-DAQ-v2.11.zip

✔访问零知实验室，获取更多实战项目和教程资源吧！

www.lingzhilab.com