零知开源——STM32F103RBT6驱动 ICM20948 九轴传感器及 vofa + 上位机可视化教程

/* ICM20948完整优化代码 */
#include "AHRSAlgorithms.h"
#include "ICM20948.h"

#define AHRS true
#define SerialDebug true 

int myLed = LED_BUILTIN;  
ICM20948 myIMU;

void setup() {
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, HIGH);
  
  Serial.begin(115200);
  Wire.begin();
  
  // 初始化与自检
  if(myIMU.begin()) {
    Serial.println("ICM20948初始化成功");
    
    // 执行两级校准
    myIMU.calibrateICM20948(myIMU.gyroBias, myIMU.accelBias);
    float magBias[3], magScale[3];
    myIMU.magCalICM20948(magBias, magScale);
    
    // 设置优化分辨率
    myIMU.getAres(); 
    myIMU.getGres();
    myIMU.getMres();
  } else {
    Serial.println("传感器初始化失败!");
    while(1);
  }
}

void loop() {
  // 数据读取
  if (myIMU.readByte(ICM20948_ADDRESS, INT_STATUS_1) & 0x01) {
    myIMU.readAccelData(myIMU.accelCount);
    myIMU.readGyroData(myIMU.gyroCount);
    myIMU.readMagData(myIMU.magCount);
    
    // 单位转换
    myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes;
    myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes;
    myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes;
    myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
    myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
    myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;
    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();
  
  // 姿态解算（使用优化参数）
  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
  );
  
  // 转换为欧拉角
  const float* q = getQ();
  myIMU.yaw   = atan2(2.0f*(q[1]*q[2] + q[0]*q[3]), 
                   q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]) * RAD_TO_DEG;
  myIMU.pitch = -asin(2.0f*(q[1]*q[3] - q[0]*q[2])) * RAD_TO_DEG;
  myIMU.roll  = atan2(2.0f*(q[0]*q[1] + q[2]*q[3]),
                   q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]) * RAD_TO_DEG;
  
  // 发送到VOFA+
  Serial.print(myIMU.yaw, 1);   // yaw
  Serial.print(",");
  Serial.print(myIMU.pitch, 1); // pitch
  Serial.print(",");
  Serial.println(myIMU.roll, 1);// roll

  delay(10); // 100Hz输出
}

// 加速度计量程 (AFS_2G/AFS_4G/AFS_8G/AFS_16G)
void ICM20948::getAres()
{
  switch (Ascale)
  {
    // Possible accelerometer scales (and their register bit settings) are:
    // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11).
    // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
    // 2-bit value:
    case AFS_2G:
      aRes = 2.0f / 32768.0f;
      break;
    case AFS_4G:
      aRes = 4.0f / 32768.0f;
      break;
    case AFS_8G:
      aRes = 8.0f / 32768.0f;
      break;
    case AFS_16G:
      aRes = 16.0f / 32768.0f;
      break;
  }
}

// 陀螺仪量程 (GFS_250DPS/GFS_500DPS/GFS_1000DPS/GFS_2000DPS)
void ICM20948::getGres()
{
  switch (Gscale)
  {
    // Possible gyro scales (and their register bit settings) are:
    // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
    // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
    // 2-bit value:
    case GFS_250DPS:
      gRes = 250.0f / 32768.0f;
      break;
    case GFS_500DPS:
      gRes = 500.0f / 32768.0f;
      break;
    case GFS_1000DPS:
      gRes = 1000.0f / 32768.0f;
      break;
    case GFS_2000DPS:
      gRes = 2000.0f / 32768.0f;
      break;
  }
}

float gyroBias[3], accelBias[3];
IMU.calibrateICM20948(gyroBias, accelBias); // 上电时执行一次

// 原参数（漂移明显）
// #define Kp 2.0f * 5.0f
// #define Ki 0.0f

// 优化参数（大幅改善漂移）
#define Kp 3.0f    // 降低比例增益，减少高频噪声响应
#define Ki 0.1f   // 降低积分增益，抑制累积误差