4、陀螺仪传感器驱动开发：L3GD20/ICM-20602驱动实现、角速度数据校准、零偏稳定性分析

陀螺仪这东西，说白了就是测量角速度的。你拿着手机转一下，屏幕跟着转，背后就是陀螺仪在干活。但在嵌入式系统里，事情没那么简单——你得先把它驱动起来，再把数据弄准。

这一章，我带你搞定两个常用型号：L3GD20（ST 家的）和 ICM-20602（InvenSense 家的）。顺便聊聊角速度校准和零偏稳定性分析。嗯，都是实战中绕不开的硬骨头。

4.1 硬件接口与寄存器概览

这两个芯片都支持 SPI 和 I²C。我个人习惯用 SPI，速度快，时序可控。但如果你板子上 I²C 总线已经拉好了，用 I²C 也行，就是注意速率别超过 400kHz。

先看 L3GD20 的关键寄存器：

寄存器名 地址 说明
WHO_AM_I 0x0F 读出来应该是 0xD4，用来验证通信
CTRL_REG1 0x20 使能各轴、设置输出数据速率
CTRL_REG4 0x23 量程选择：250/500/2000 dps
OUT_X_L / OUT_X_H 0x28 / 0x29 X 轴角速度原始值，16 位有符号

ICM-20602 的寄存器布局类似，但地址不同：

寄存器名 地址 说明
WHO_AM_I 0x75 固定值 0x12
GYRO_CONFIG 0x1B 量程选择 + 低通滤波配置
GYRO_XOUT_H / GYRO_XOUT_L 0x43 / 0x44 X 轴角速度，高字节在前
小提示：L3GD20 的 OUT 寄存器是低字节在前，ICM-20602 是高字节在前。写驱动时千万别搞反，否则读出来的数据会「跳来跳去」。我曾经因为这个 bug 调了一整个下午。

4.2 L3GD20 驱动实现

驱动代码其实不复杂。核心就三步：初始化、读数据、换算成物理值。

先看初始化：

void L3GD20_Init(void) {
    // 1. 检查 WHO_AM_I
    uint8_t who = SPI_ReadReg(L3GD20_WHO_AM_I);
    if (who != 0xD4) {
        // 通信有问题，建议打印错误
        return;
    }

    // 2. 配置 CTRL_REG1：使能 X/Y/Z，ODR = 100Hz
    SPI_WriteReg(L3GD20_CTRL_REG1, 0x0F);

    // 3. 配置 CTRL_REG4：量程 ±500 dps
    // 500 dps 对应的灵敏度是 17.5 mdps/digit
    SPI_WriteReg(L3GD20_CTRL_REG4, 0x10);
}

读数据时要注意：L3GD20 支持自动递增地址，你可以一次读出 6 个字节：

void L3GD20_ReadGyro(int16_t *x, int16_t *y, int16_t *z) {
    uint8_t buf[6];
    SPI_ReadRegs(L3GD20_OUT_X_L | 0x80, buf, 6);  // 0x80 表示自动递增

    *x = (int16_t)(buf[1] << 8) | buf[0];
    *y = (int16_t)(buf[3] << 8) | buf[2];
    *z = (int16_t)(buf[5] << 8) | buf[4];
}

换算成角速度（单位：dps）：

float gyro_x_dps = (float)(*x) * 17.5f / 1000.0f;  // 17.5 mdps/digit
注意：L3GD20 的原始数据是补码格式。如果你用 int16_t 接收，直接左移再或操作就行。但如果你用 uint16_t，记得手动处理符号位。我见过有人用 uint16_t 然后直接强转 float，结果负数全变成了大正数。

4.3 ICM-20602 驱动实现

ICM-20602 的驱动思路一样，但细节有差异。它内置了数字低通滤波器（DLPF），这个很好用。

void ICM20602_Init(void) {
    // 复位设备
    SPI_WriteReg(ICM20602_PWR_MGMT_1, 0x80);
    delay_ms(100);

    // 配置陀螺仪：±500 dps，DLPF 带宽 41Hz
    SPI_WriteReg(ICM20602_GYRO_CONFIG, 0x08 | 0x03);
    // 0x08 表示 ±500 dps，灵敏度 65.5 LSB/dps
    // 0x03 表示 DLPF 带宽约 41Hz

    // 唤醒设备
    SPI_WriteReg(ICM20602_PWR_MGMT_1, 0x01);
}

读数据时，ICM-20602 也是高字节在前：

void ICM20602_ReadGyro(int16_t *x, int16_t *y, int16_t *z) {
    uint8_t buf[6];
    SPI_ReadRegs(ICM20602_GYRO_XOUT_H, buf, 6);

    *x = (int16_t)(buf[0] << 8) | buf[1];
    *y = (int16_t)(buf[2] << 8) | buf[3];
    *z = (int16_t)(buf[4] << 8) | buf[5];
}

换算：

float gyro_x_dps = (float)(*x) / 65.5f;  // 65.5 LSB/dps
核心区别总结：
  • L3GD20：低字节在前，灵敏度单位是 mdps/digit
  • ICM-20602：高字节在前，灵敏度单位是 LSB/dps
  • ICM-20602 有 DLPF，L3GD20 没有（需要外部滤波）

4.4 角速度数据校准

陀螺仪有个毛病：静止时输出不为零。这叫零偏（Zero-rate Offset）。你想想看，如果无人机悬停时陀螺仪报出 0.5 dps，飞控就会以为它在转，然后误动作。

校准方法很简单：采集静止时的数据，求平均，然后减去这个偏移。

#define CALIB_SAMPLES 200

void Gyro_Calibrate(float *offset_x, float *offset_y, float *offset_z) {
    float sum_x = 0, sum_y = 0, sum_z = 0;
    int16_t x, y, z;

    for (int i = 0; i < CALIB_SAMPLES; i++) {
        ReadGyro(&x, &y, &z);
        sum_x += (float)x;
        sum_y += (float)y;
        sum_z += (float)z;
        delay_ms(5);  // 等 5ms，总共采集 1 秒
    }

    *offset_x = sum_x / CALIB_SAMPLES;
    *offset_y = sum_y / CALIB_SAMPLES;
    *offset_z = sum_z / CALIB_SAMPLES;
}

使用时：

float raw_dps = (float)raw_value / sensitivity;
float calibrated_dps = raw_dps - offset_dps;
实战经验：采集时一定要确保传感器完全静止。我曾在桌面上校准，结果桌子本身在振动（空调外机引起的），校准出来的偏移全是错的。后来我改用大理石平台，数据才稳定下来。

4.5 零偏稳定性分析

零偏稳定性（Bias Stability）是衡量陀螺仪质量的核心指标。它描述的是：校准之后，零偏随时间漂移的程度。

分析方法：

  1. 校准后，让陀螺仪静止，连续采集 N 个样本（比如 10 分钟）
  2. 计算每个样本的零偏残差
  3. 用 Allan 方差法分析

简单点说，你可以算标准差：

float ComputeBiasStability(float *data, int len) {
    float mean = 0, std = 0;
    for (int i = 0; i < len; i++) mean += data[i];
    mean /= len;

    for (int i = 0; i < len; i++) std += (data[i] - mean) * (data[i] - mean);
    std = sqrt(std / (len - 1));

    return std;  // 单位 dps
}

典型值参考：

传感器 零偏稳定性（典型） 说明
L3GD20 ±0.5 dps 消费级，够用但别指望太高
ICM-20602 ±0.2 dps 工业级，性价比不错
高精度光纤陀螺 < 0.01 °/h 那是另一个世界了
注意：零偏稳定性受温度影响很大。如果你做的是户外产品，建议做温度补偿。我曾经在 -20°C 到 60°C 范围内测过 ICM-20602，零偏漂移了将近 1 dps。不做补偿的话，导航精度根本没法看。

4.6 实战建议

  • 先读 WHO_AM_I：这是验证通信最快的方法。读不到就查硬件，别急着调代码。
  • 量程选合适：做姿态检测用 ±250 dps 就够了，做旋转检测用 ±2000 dps。量程越小，分辨率越高。
  • 滤波别省：L3GD20 没有内置滤波器，建议加一个 20Hz 的 FIR 或 IIR 低通。ICM-20602 的 DLPF 开到 41Hz 带宽就挺好。
  • 校准要定期做：每次上电都做一次零偏校准。如果条件允许，做个温度查表补偿。

嗯，这一章就到这里。驱动写好了，数据也校准了，下一章我们聊聊怎么把这些数据融合成姿态角。到时候你会看到，陀螺仪和加速度计是怎么「互相帮忙」的。

← 上一章 📖 返回目录 下一章 →