19. 姿态传感器（IMU）

📝本节您将学习天巧星板载 6 轴 IMU（LSM6DS3TRC）的驱动方法，掌握加速度和角速度数据的读取，以及基于 Madgwick 算法的姿态角解算。

🏆本章目标

1️⃣ 了解 6 轴 IMU 的工作原理（加速度计 + 陀螺仪）

2️⃣ 通过 I2C 读取 IMU 原始数据

3️⃣ 将原始数据转换为物理量（g / °/s）

4️⃣ 使用 AHRS 算法解算姿态角（Pitch / Roll / Yaw）

19.1 IMU 基础知识

IMU（Inertial Measurement Unit，惯性测量单元）是集成了加速度计和陀螺仪的传感器模块。天巧星板载 LSM6DS3TRC（ST 意法半导体），6 轴传感器。

传感器组成

传感器	测量内容	输出	单位
加速度计（Accelerometer）	线性加速度（含重力）	ax, ay, az	g（重力加速度）
陀螺仪（Gyroscope）	角速度	gx, gy, gz	°/s（度每秒）

坐标系

       Z (竖直向上)
       │
       │
       │______ Y (向右)
      /
     /
    X (向前)

静止水平放置时：

加速度计：ax ≈ 0, ay ≈ 0, az ≈ 1g（只有重力分量）
陀螺仪：gx ≈ 0, gy ≈ 0, gz ≈ 0（无旋转）

姿态角

角度	含义	旋转轴
Pitch（俯仰）	抬头/低头	绕 Y 轴
Roll（横滚）	左倾/右倾	绕 X 轴
Yaw（偏航）	左转/右转	绕 Z 轴

19.2 LSM6DS3TRC 参数

参数	规格
通信接口	I2C（共享 OLED 总线）
I2C 地址	0x6A（SA0 接 GND）
加速度量程	±2g / ±4g / ±8g / ±16g
陀螺仪量程	±125 / ±250 / ±500 / ±1000 / ±2000 °/s
输出数据率	12.5Hz ~ 6.66kHz
数据分辨率	16-bit（有符号）
引脚	PA0(SDA) / PA1(SCL)，与 OLED 共用

19.3 寄存器与初始化

19.3.1 关键寄存器

寄存器	地址	说明
WHO_AM_I	0x0F	设备 ID（读取应为 0x6A）
CTRL1_XL	0x10	加速度计控制（量程 + ODR）
CTRL2_G	0x11	陀螺仪控制（量程 + ODR）
CTRL3_C	0x12	通用控制（BDU、IF_INC 等）
OUTX_L_G	0x22	陀螺仪 X 轴低字节（6 字节连续读）
OUTX_L_XL	0x28	加速度计 X 轴低字节（6 字节连续读）

19.3.2 初始化代码

#include "ti_msp_dl_config.h"
#include "myiic.h"

#define LSM6DS3_ADDR    0x6A

// 寄存器地址
#define LSM6DS3_WHO_AM_I    0x0F
#define LSM6DS3_CTRL1_XL    0x10
#define LSM6DS3_CTRL2_G     0x11
#define LSM6DS3_CTRL3_C     0x12
#define LSM6DS3_OUTX_L_G    0x22
#define LSM6DS3_OUTX_L_XL   0x28

// 写单个寄存器
static void lsm6ds3_write_reg(uint8_t reg, uint8_t val)
{
    i2c_write_reg(LSM6DS3_ADDR, reg, &val, 1);
}

// 读单个寄存器
static uint8_t lsm6ds3_read_reg(uint8_t reg)
{
    uint8_t val = 0;
    i2c_read_reg(LSM6DS3_ADDR, reg, &val, 1);
    return val;
}

// 读多个连续寄存器
static void lsm6ds3_read_regs(uint8_t reg, uint8_t *buf, uint8_t len)
{
    i2c_read_reg(LSM6DS3_ADDR, reg, buf, len);
}

// 初始化 IMU
uint8_t lsm6ds3_init(void)
{
    // 验证设备 ID
    uint8_t who = lsm6ds3_read_reg(LSM6DS3_WHO_AM_I);
    if (who != 0x6A) {
        return 1;  // 设备未找到
    }

    // CTRL3_C: BDU=1（块数据更新）, IF_INC=1（地址自动递增）
    lsm6ds3_write_reg(LSM6DS3_CTRL3_C, 0x44);

    // CTRL1_XL: ODR=52Hz, FS=±2g
    lsm6ds3_write_reg(LSM6DS3_CTRL1_XL, 0x30);

    // CTRL2_G: ODR=52Hz, FS=±2000°/s
    lsm6ds3_write_reg(LSM6DS3_CTRL2_G, 0x3C);

    return 0;  // 初始化成功
}

19.4 读取原始数据

19.4.1 读取加速度和陀螺仪

typedef struct {
    int16_t ax, ay, az;    // 加速度原始值
    int16_t gx, gy, gz;    // 陀螺仪原始值
} IMU_RawData_t;

// 读取 6 轴原始数据
void lsm6ds3_read_raw(IMU_RawData_t *data)
{
    uint8_t buf[6];

    // 读取陀螺仪（6 字节，从 0x22 开始）
    lsm6ds3_read_regs(LSM6DS3_OUTX_L_G, buf, 6);
    data->gx = (int16_t)(buf[1] << 8 | buf[0]);
    data->gy = (int16_t)(buf[3] << 8 | buf[2]);
    data->gz = (int16_t)(buf[5] << 8 | buf[4]);

    // 读取加速度计（6 字节，从 0x28 开始）
    lsm6ds3_read_regs(LSM6DS3_OUTX_L_XL, buf, 6);
    data->ax = (int16_t)(buf[1] << 8 | buf[0]);
    data->ay = (int16_t)(buf[3] << 8 | buf[2]);
    data->az = (int16_t)(buf[5] << 8 | buf[4]);
}

19.4.2 原始值转物理量

typedef struct {
    float ax, ay, az;    // 单位：g
    float gx, gy, gz;    // 单位：°/s
} IMU_Data_t;

// 灵敏度常量（根据量程设置）
#define ACC_SENSITIVITY   0.000061f   // ±2g: 0.061 mg/LSB
#define GYRO_SENSITIVITY  0.070f      // ±2000°/s: 70 mdps/LSB

void lsm6ds3_read_data(IMU_Data_t *data)
{
    IMU_RawData_t raw;
    lsm6ds3_read_raw(&raw);

    data->ax = raw.ax * ACC_SENSITIVITY;
    data->ay = raw.ay * ACC_SENSITIVITY;
    data->az = raw.az * ACC_SENSITIVITY;

    data->gx = raw.gx * GYRO_SENSITIVITY;
    data->gy = raw.gy * GYRO_SENSITIVITY;
    data->gz = raw.gz * GYRO_SENSITIVITY;
}

19.5 姿态解算（Madgwick AHRS）

仅用加速度计可以计算静态姿态角（Pitch / Roll），但动态场景下噪声大。仅用陀螺仪积分可以获得平滑的角度变化，但会产生漂移。

Madgwick AHRS 算法融合加速度计和陀螺仪数据，兼顾动态响应和长期稳定性。出厂固件使用了开源的 Fusion 库（Madgwick 改进版）。

19.5.1 简化版姿态计算

对于入门学习，可以先用加速度计直接计算静态姿态角：

#include <math.h>

typedef struct {
    float pitch;   // 俯仰角（度）
    float roll;    // 横滚角（度）
} Attitude_t;

// 从加速度计数据计算姿态角（仅适用于静态/缓慢运动）
void calc_attitude_from_accel(IMU_Data_t *imu, Attitude_t *att)
{
    att->pitch = atan2f(-imu->ax, sqrtf(imu->ay * imu->ay + imu->az * imu->az))
                 * 180.0f / 3.14159f;
    att->roll  = atan2f(imu->ay, imu->az)
                 * 180.0f / 3.14159f;
}

局限性

纯加速度计方案：

✅ 无漂移，长期稳定
❌ 运动时噪声大，无法计算 Yaw 角
❌ 振动环境下误差大

实际应用建议使用 Madgwick 或互补滤波算法融合陀螺仪数据。

19.5.2 互补滤波（简易融合）

#define ALPHA  0.98f   // 互补滤波系数
#define DT     0.02f   // 采样周期（50Hz → 20ms）

static float pitch_filtered = 0.0f;
static float roll_filtered = 0.0f;

void attitude_update(IMU_Data_t *imu)
{
    // 加速度计计算角度
    float pitch_acc = atan2f(-imu->ax,
                     sqrtf(imu->ay * imu->ay + imu->az * imu->az))
                     * 180.0f / 3.14159f;
    float roll_acc  = atan2f(imu->ay, imu->az) * 180.0f / 3.14159f;

    // 陀螺仪积分
    pitch_filtered = ALPHA * (pitch_filtered + imu->gy * DT)
                   + (1.0f - ALPHA) * pitch_acc;
    roll_filtered  = ALPHA * (roll_filtered + imu->gx * DT)
                   + (1.0f - ALPHA) * roll_acc;
}

互补滤波将陀螺仪的短期精度与加速度计的长期稳定性结合：

高频部分（快速变化）信任陀螺仪（ALPHA=0.98）
低频部分（缓慢漂移）信任加速度计（1-ALPHA=0.02）

19.6 实验：串口输出姿态数据

#include "ti_msp_dl_config.h"
#include "myiic.h"
#include <stdio.h>
#include <math.h>

void uart_send_string(const char *str)
{
    while (*str) {
        while (DL_UART_Main_isBusy(UART_0_INST));
        DL_UART_Main_transmitData(UART_0_INST, *str++);
    }
}

int main(void)
{
    SYSCFG_DL_init();

    // 初始化 IMU
    if (lsm6ds3_init() != 0) {
        uart_send_string("IMU init failed!\r\n");
        while (1);
    }
    uart_send_string("IMU OK. Reading data...\r\n");

    char buf[128];
    IMU_Data_t imu;

    while (1) {
        lsm6ds3_read_data(&imu);
        attitude_update(&imu);

        sprintf(buf, "Pitch: %6.1f  Roll: %6.1f  |  ax:%.2f ay:%.2f az:%.2f\r\n",
                pitch_filtered, roll_filtered,
                imu.ax, imu.ay, imu.az);
        uart_send_string(buf);

        delay_cycles(CPUCLK_FREQ / 50);  // 50Hz 更新
    }
}

运行后，倾斜开发板可在串口终端中实时看到姿态角变化。

19.7 陀螺仪零偏校准

陀螺仪在静止时通常不会精确输出 0，存在零偏（Offset）。需要在启动时校准：

static float gyro_offset_x = 0, gyro_offset_y = 0, gyro_offset_z = 0;

// 校准：静止状态下采集 N 次取平均
void lsm6ds3_calibrate(uint16_t samples)
{
    float sum_x = 0, sum_y = 0, sum_z = 0;
    IMU_Data_t data;

    for (uint16_t i = 0; i < samples; i++) {
        lsm6ds3_read_data(&data);
        sum_x += data.gx;
        sum_y += data.gy;
        sum_z += data.gz;
        delay_cycles(CPUCLK_FREQ / 100);  // 10ms 间隔
    }

    gyro_offset_x = sum_x / samples;
    gyro_offset_y = sum_y / samples;
    gyro_offset_z = sum_z / samples;
}

// 读取数据时减去零偏
void lsm6ds3_read_calibrated(IMU_Data_t *data)
{
    lsm6ds3_read_data(data);
    data->gx -= gyro_offset_x;
    data->gy -= gyro_offset_y;
    data->gz -= gyro_offset_z;
}

校准注意事项

校准期间开发板必须完全静止
校准通常在上电后执行，采集 100~500 个样本
校准结果可保存到 SPI Flash，避免每次上电重新校准
温度变化会影响零偏，高精度场景需定期重新校准

19.8 知识总结

内容	说明
传感器型号	LSM6DS3TRC（6 轴）
I2C 地址	0x6A
输出数据	加速度（g）+ 角速度（°/s）
采样率	52Hz（可配置）
姿态角	Pitch / Roll / Yaw
融合算法	互补滤波 / Madgwick AHRS
校准	上电静止时采集零偏
引脚	PA0(SDA) / PA1(SCL)，与 OLED 共用

下一章将学习多功能按键库的实现，支持短按、长按、双击等丰富的按键事件检测。

电赛TI：简易PID入门项目

显示类

传感器类

无线通信类

控制类

19. 姿态传感器（IMU） ​

19.1 IMU 基础知识 ​

传感器组成 ​

坐标系 ​

姿态角 ​

19.2 LSM6DS3TRC 参数 ​

19.3 寄存器与初始化 ​

19.3.1 关键寄存器 ​

19.3.2 初始化代码 ​

19.4 读取原始数据 ​

19.4.1 读取加速度和陀螺仪 ​

19.4.2 原始值转物理量 ​

19.5 姿态解算（Madgwick AHRS） ​

19.5.1 简化版姿态计算 ​

19.5.2 互补滤波（简易融合） ​

19.6 实验：串口输出姿态数据 ​

19.7 陀螺仪零偏校准 ​

19.8 知识总结 ​