嵌入式传感器驱动开发深度解析——从 I2C/SPI 驱动到数据融合算法

一、引言

传感器是嵌入式系统感知物理世界的"眼睛"和"耳朵"。温度、湿度、加速度、陀螺仪、气压、光照、距离......不管是消费电子、工业控制还是物联网设备，传感器始终处于数据链的最前端。

然而，把一个传感器"调通"和把它"用好"之间有很大的距离：

• 你能读到一个温度值，但它是准确的吗？

• 加速度计输出抖动得厉害，怎么滤除噪声？

• 陀螺仪积分后漂移严重，如何与加速度计融合？

• 多个传感器的数据时钟不同步，怎么处理？

本文从驱动层到算法层，系统讲解嵌入式传感器开发的完整链路：

• I2C/SPI 传感器的驱动框架

• 传感器的校准方法

• 数字滤波算法（滑动平均、中值滤波、卡尔曼滤波）

• 传感器数据融合（互补滤波、Mahony、Madgwick）

• 工程实践：从原始数据到可靠姿态

平台：STM32F103 + MPU6050（6 轴 IMU）+ SHT30（温湿度）+ BMP280（气压） 重点：不仅"能用"，更要"可靠"

---

二、传感器驱动框架

2.1 通用的传感器驱动结构

// sensor.h —— 传感器抽象接口
typedef struct {
    int (*init)(void);                 // 初始化
    int (*read)(void *data);          // 读取数据
    int (*calibrate)(void);           // 校准
    int (*set_config)(void *config);  // 配置参数
    int (*self_test)(void);           // 自检
} sensor_driver_t;
​
// 具体传感器实现：MPU6050
static sensor_driver_t mpu6050_driver = {
    .init      = mpu6050_init,
    .read      = mpu6050_read,
    .calibrate = mpu6050_calibrate,
    .set_config = mpu6050_set_config,
    .self_test  = mpu6050_self_test,
};

2.2 I2C 传感器的通用读写

// i2c_sensor.c —— 适用于所有 I2C 传感器
typedef struct {
    I2C_TypeDef *i2c;
    uint8_t      dev_addr;    // 7 位设备地址（左移 1 位后写入 DR）
    uint32_t     timeout_ms;
} i2c_sensor_t;
​
// 读寄存器（最常见操作）
int sensor_read_reg(i2c_sensor_t *s, uint8_t reg, uint8_t *buf, uint8_t len)
{
    // 1. 发送寄存器地址
    I2C_GenerateSTART(s->i2c, ENABLE);
    while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_MODE_SELECT));
    
    I2C_Send7bitAddress(s->i2c, s->dev_addr, I2C_Direction_Transmitter);
    while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
    
    I2C_SendData(s->i2c, reg);
    while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
    
    // 2. 重复起始 + 读
    I2C_GenerateSTART(s->i2c, ENABLE);
    while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_MODE_SELECT));
    
    I2C_Send7bitAddress(s->i2c, s->dev_addr, I2C_Direction_Receiver);
    while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
    
    for (int i = 0; i < len; i++) {
        if (i == len - 1) {
            I2C_AcknowledgeConfig(s->i2c, DISABLE);  // 最后一个字节发 NACK
        }
        while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_BYTE_RECEIVED));
        buf[i] = I2C_ReceiveData(s->i2c);
    }
    
    I2C_GenerateSTOP(s->i2c, ENABLE);
    return 0;
}
​
// 写寄存器
int sensor_write_reg(i2c_sensor_t *s, uint8_t reg, uint8_t val)
{
    I2C_GenerateSTART(s->i2c, ENABLE);
    while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_MODE_SELECT));
    
    I2C_Send7bitAddress(s->i2c, s->dev_addr, I2C_Direction_Transmitter);
    while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
    
    I2C_SendData(s->i2c, reg);
    while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
    
    I2C_SendData(s->i2c, val);
    while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
    
    I2C_GenerateSTOP(s->i2c, ENABLE);
    return 0;
}

---

三、MPU6050 驱动实战

3.1 芯片初始化

// MPU6050 初始化完整流程
#define MPU6050_ADDR     0xD0  // AD0=0 时的地址（含读写位）
#define MPU6050_WHO_AM_I 0x75
#define MPU6050_PWR_MGMT 0x6B
#define MPU6050_ACCEL_X  0x3B   // 加速度寄存器起始（连续 6 字节）
#define MPU6050_GYRO_X   0x43   // 陀螺仪寄存器起始（连续 6 字节）
​
typedef struct {
    int16_t accel_x, accel_y, accel_z;
    int16_t temp;
    int16_t gyro_x, gyro_y, gyro_z;
} mpu6050_raw_t;
​
int mpu6050_init(void)
{
    uint8_t whoami;
    i2c_sensor_t s = { .i2c = I2C1, .dev_addr = MPU6050_ADDR };
    
    // 1. 检查 WHO_AM_I 寄存器（应为 0x68）
    sensor_read_reg(&s, MPU6050_WHO_AM_I, &whoami, 1);
    if (whoami != 0x68) {
        printf("MPU6050 not found! WHO_AM_I=0x%02X\r\n", whoami);
        return -1;
    }
    
    // 2. 唤醒 MPU6050（退出睡眠模式）
    sensor_write_reg(&s, MPU6050_PWR_MGMT, 0x00);
    HAL_Delay(100);  // 等待起振稳定
    
    // 3. 配置陀螺仪量程：±2000°/s
    //    寄存器 0x1B, FS_SEL[4:3] = 11 = ±2000°/s
    sensor_write_reg(&s, 0x1B, 0x18);  // 0x18 = 0b00011000
    
    // 4. 配置加速度计量程：±16g
    //    寄存器 0x1C, AFS_SEL[4:3] = 11 = ±16g
    sensor_write_reg(&s, 0x1C, 0x18);
    
    // 5. 配置数字低通滤波器 (DLPF)：~43Hz 带宽
    //    寄存器 0x1A, DLPF_CFG[2:0] = 011 = 43Hz
    sensor_write_reg(&s, 0x1A, 0x03);
    
    // 6. 配置采样率分频：1kHz / (1+9) = 100Hz
    sensor_write_reg(&s, 0x19, 9);
    
    return 0;
}
​
// 读取原始数据
int mpu6050_read_raw(mpu6050_raw_t *raw)
{
    uint8_t buf[14];
    i2c_sensor_t s = { .i2c = I2C1, .dev_addr = MPU6050_ADDR };
    
    sensor_read_reg(&s, MPU6050_ACCEL_X, buf, 14);
    
    raw->accel_x = (int16_t)((buf[0] << 8) | buf[1]);
    raw->accel_y = (int16_t)((buf[2] << 8) | buf[3]);
    raw->accel_z = (int16_t)((buf[4] << 8) | buf[5]);
    raw->temp    = (int16_t)((buf[6] << 8) | buf[7]);
    raw->gyro_x  = (int16_t)((buf[8] << 8) | buf[9]);
    raw->gyro_y  = (int16_t)((buf[10] << 8) | buf[11]);
    raw->gyro_z  = (int16_t)((buf[12] << 8) | buf[13]);
    
    return 0;
}
​
// 转换为物理量
float accel_convert(int16_t raw, float scale)
{
    // scale: ±2g=16384, ±4g=8192, ±8g=4096, ±16g=2048
    return (float)raw / scale;  // 单位：g
}
​
float gyro_convert(int16_t raw, float scale)
{
    // scale: ±250=131, ±500=65.5, ±1000=32.8, ±2000=16.4
    return (float)raw / scale;  // 单位：°/s
}
​
float temp_convert(int16_t raw)
{
    return (float)raw / 340.0f + 36.53f;  // 单位：°C
}

---

四、传感器校准

4.1 加速度计六面校准法

// 加速度计校准原理：
// 理想情况下，重力加速度只沿一个轴作用
// 但存在零偏 (bias) 和比例误差 (scale)
​
// 真实值 = (原始值 - 零偏) × 比例系数
​
// 六面法：分别测量 +X, -X, +Y, -Y, +Z, -Z 六个面朝上时的输出
typedef struct {
    float offset[3];   // 零偏
    float scale[3];    // 比例系数
} accel_calib_t;
​
void accel_calibrate_six_face(float raw[6][3], accel_calib_t *calib)
{
    // raw[0] = +X 面朝上, raw[1] = -X 面朝上, ...
    for (int axis = 0; axis < 3; axis++) {
        // 零偏 = (正 + 负) / 2
        calib->offset[axis] = (raw[axis*2][axis] + raw[axis*2+1][axis]) / 2.0f;
        
        // 比例系数
        // 理想差值为 2g（+g 减去 -g = 2g）
        float diff = raw[axis*2][axis] - raw[axis*2+1][axis];
        calib->scale[axis] = 2.0f / diff;
    }
}

4.2 陀螺仪静止校准

// 陀螺仪在静止时输出应该为 0，但实际有零偏
void gyro_calibrate_static(float offset[3], int samples)
{
    float sum[3] = {0};
    
    for (int i = 0; i < samples; i++) {
        mpu6050_raw_t raw;
        mpu6050_read_raw(&raw);
        sum[0] += raw.gyro_x;
        sum[1] += raw.gyro_y;
        sum[2] += raw.gyro_z;
        HAL_Delay(10);
    }
    
    offset[0] = sum[0] / samples;
    offset[1] = sum[1] / samples;
    offset[2] = sum[2] / samples;
    
    printf("Gyro offsets: %.2f, %.2f, %.2f\r\n", 
           offset[0], offset[1], offset[2]);
}

---

五、数字滤波算法

5.1 滑动平均滤波（Moving Average）

// 最简单、最快速的平滑滤波
#define MA_WINDOW  16
​
typedef struct {
    float buffer[MA_WINDOW];
    int   index;
    float sum;
    int   count;
} moving_average_t;
​
void ma_init(moving_average_t *f)
{
    memset(f, 0, sizeof(*f));
}
​
float ma_update(moving_average_t *f, float new_val)
{
    // 减去最旧值，加入新值
    f->sum -= f->buffer[f->index];
    f->buffer[f->index] = new_val;
    f->sum += new_val;
    
    f->index = (f->index + 1) % MA_WINDOW;
    if (f->count < MA_WINDOW) f->count++;
    
    return f->sum / f->count;
}
​
// 优点：极快（只需 2 次加减法 + 1 次除法）
// 缺点：对脉冲噪声抑制能力弱

5.2 中值滤波（Median Filter）

// 最适合抑制脉冲噪声（尖刺）
// 但排序开销较大，适合低频数据
​
float median_filter(float *window, int size)
{
    // 复制并排序（或使用选择算法）
    float sorted[size];
    memcpy(sorted, window, size * sizeof(float));
    
    // 冒泡排序（数据量小时可用）
    for (int i = 0; i < size - 1; i++) {
        for (int j = 0; j < size - 1 - i; j++) {
            if (sorted[j] > sorted[j + 1]) {
                float tmp = sorted[j];
                sorted[j] = sorted[j + 1];
                sorted[j + 1] = tmp;
            }
        }
    }
    
    return sorted[size / 2];  // 中值
}

5.3 一阶低通/互补滤波器

// ★ 嵌入式最常用的滤波器
// 公式：output = α × input + (1-α) × prev_output
​
// α 取值范围：0.0 ~ 1.0
//   α ≈ 1.0 → 几乎无滤波（跟随输入）
//   α ≈ 0.0 → 强滤波（响应慢）
​
typedef struct {
    float alpha;
    float prev_output;
} lowpass_filter_t;
​
void lpf_init(lowpass_filter_t *f, float alpha)
{
    f->alpha = alpha;
    f->prev_output = 0;
}
​
float lpf_update(lowpass_filter_t *f, float input)
{
    f->prev_output = f->alpha * input + (1.0f - f->alpha) * f->prev_output;
    return f->prev_output;
}
​
// 时间常数与截止频率的换算：
// α = dt / (dt + RC)     —— 时间常数形式
// α = 2π × fc × dt       —— 截止频率形式（当 α << 1 时近似成立）
​
// 例：采样周期 dt=10ms，希望截止频率 fc=5Hz
// α = 2π × 5 × 0.01 = 0.314
// 在代码中：lpf_init(&f, 0.314f);

5.4 卡尔曼滤波器（单变量）

// 一维卡尔曼滤波器——适合单传感器滤波
// 公式清晰，计算量可控

typedef struct {
    float x;  // 状态估计值
    float p;  // 估计误差协方差
    float q;  // 过程噪声协方差（越大→越信任测量值）
    float r;  // 测量噪声协方差（越大→越不信任测量值）
    float k;  // 卡尔曼增益（内部计算）
} kalman1d_t;

void kalman1d_init(kalman1d_t *kf, float init_val, float q, float r)
{
    kf->x = init_val;
    kf->p = 1.0f;
    kf->q = q;
    kf->r = r;
}

float kalman1d_update(kalman1d_t *kf, float measurement)
{
    // 预测（Prediction）
    // 假设状态不变（最简单模型）
    kf->p = kf->p + kf->q;
    
    // 更新（Update）
    kf->k = kf->p / (kf->p + kf->r);
    kf->x = kf->x + kf->k * (measurement - kf->x);
    kf->p = (1.0f - kf->k) * kf->p;
    
    return kf->x;
}

// 调参经验：
//  q 大 → 滤波器更"信任"测量值，响应更快
//  r 大 → 滤波器更"不信任"测量值，输出更平滑
//  典型初始值：q=0.01, r=1.0

---

六、姿态解算------传感器融合

6.1 为什么需要融合？

加速度计：静态精度好，可以测重力方向
  但动态时受运动加速度干扰——无法区分重力和运动加速度

陀螺仪：动态响应快，不受运动加速度影响
  但积分累积漂移——静止 1 分钟后可能漂移好几度

→ 融合思路：
  陀螺仪提供短期姿态变化（高频响应好）
  加速度计提供长期姿态基准（低频稳定好）
  = 互补滤波器（取各自的长处）

6.2 Mahony 互补滤波器

// Mahony AHRS (Attitude and Heading Reference System) 互补滤波器
// 用四元数表示姿态，计算量小，适合 MCU

typedef struct {
    float q0, q1, q2, q3;  // 四元数
    float kp;               // 比例增益（典型 2.0）
    float ki;               // 积分增益（典型 0.0~0.1）
    float integral_fb[3];   // 积分项（补偿陀螺仪漂移）
    float dt;               // 采样周期
} mahony_t;

void mahony_init(mahony_t *m, float kp, float ki, float dt)
{
    m->q0 = 1.0f;  // 初始姿态：水平朝北
    m->q1 = 0.0f;
    m->q2 = 0.0f;
    m->q3 = 0.0f;
    m->kp = kp;
    m->ki = ki;
    m->dt = dt;
    m->integral_fb[0] = m->integral_fb[1] = m->integral_fb[2] = 0.0f;
}

// 核心更新函数
void mahony_update(mahony_t *m, float gx, float gy, float gz,  // 陀螺仪 rad/s
                                    float ax, float ay, float az)  // 加速度 g
{
    float q0 = m->q0, q1 = m->q1, q2 = m->q2, q3 = m->q3;
    float recip_norm;
    float vx, vy, vz;       // 重力的估计方向
    float ex, ey, ez;        // 误差（加速度测量 - 重力估计）
    
    // 1. 归一化加速度计读数
    recip_norm = 1.0f / sqrtf(ax*ax + ay*ay + az*az);
    ax *= recip_norm;
    ay *= recip_norm;
    az *= recip_norm;
    
    // 2. 从四元数估计重力方向（在机体坐标系下）
    vx = 2.0f * (q1*q3 - q0*q2);
    vy = 2.0f * (q0*q1 + q2*q3);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    
    // 3. 计算误差 = 测量值 - 估计值 的叉积
    ex = ay*vz - az*vy;
    ey = az*vx - ax*vz;
    ez = ax*vy - ay*vx;
    
    // 4. PI 控制器修正陀螺仪
    m->integral_fb[0] += m->ki * ex * m->dt;
    m->integral_fb[1] += m->ki * ey * m->dt;
    m->integral_fb[2] += m->ki * ez * m->dt;
    
    gx += m->kp * ex + m->integral_fb[0];
    gy += m->kp * ey + m->integral_fb[1];
    gz += m->kp * ez + m->integral_fb[2];
    
    // 5. 用修正后的角速度更新四元数（一阶龙格-库塔）
    q0 += (-q1*gx - q2*gy - q3*gz) * 0.5f * m->dt;
    q1 += ( q0*gx + q2*gz - q3*gy) * 0.5f * m->dt;
    q2 += ( q0*gy - q1*gz + q3*gx) * 0.5f * m->dt;
    q3 += ( q0*gz + q1*gy - q2*gx) * 0.5f * m->dt;
    
    // 6. 归一化四元数
    recip_norm = 1.0f / sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    m->q0 = q0 * recip_norm;
    m->q1 = q1 * recip_norm;
    m->q2 = q2 * recip_norm;
    m->q3 = q3 * recip_norm;
}

// 四元数→欧拉角（Roll/Pitch/Yaw）
void mahony_get_euler(mahony_t *m, float *roll, float *pitch, float *yaw)
{
    float q0 = m->q0, q1 = m->q1, q2 = m->q2, q3 = m->q3;
    
    *roll  = atan2f(2.0f*(q0*q1 + q2*q3), 1.0f - 2.0f*(q1*q1 + q2*q2));
    *pitch = asinf(2.0f*(q0*q2 - q3*q1));
    *yaw   = atan2f(2.0f*(q0*q3 + q1*q2), 1.0f - 2.0f*(q2*q2 + q3*q3));
    
    // 转换为度
    *roll  *= 57.29578f;
    *pitch *= 57.29578f;
    *yaw   *= 57.29578f;
}

---

七、多传感器数据处理模式

7.1 时间同步策略

多个传感器各自独立采样，时间标签不同：

  传感器 A (100Hz)  ──●──●──●──●──●──●──●──●──
  传感器 B (50Hz)   ────●──────●──────●──────●──
  传感器 C (10Hz)   ──────────●──────────────●──

融合处理时的时间对齐方法：
  1. 最近邻插值——取时间最接近的数据点
  2. 线性插值——根据两个采样点插值
  3. 统一采样时钟——用一个定时器同时触发所有传感器（最佳）

7.2 IMU 温度补偿

// MPU6050 的零偏随温度变化
// 开机后芯片温度上升 → 零偏漂移 → 需要补偿

typedef struct {
    float temp_offset[3];      // 温度-零偏关系的截距
    float temp_slope[3];       // 温度-零偏关系的斜率
} temp_compensation_t;

// 在恒温箱中对不同温度下的零偏进行标定
// 简单模型：零偏 = offset + slope × temperature
float gyro_compensate(float raw, float temp, temp_compensation_t *comp, int axis)
{
    float bias = comp->temp_offset[axis] + comp->temp_slope[axis] * temp;
    return raw - bias;
}

---

八、传感器选型速查

传感器类型	常用型号	接口	精度/特点
6 轴 IMU	MPU6050 / ICM-42688	I2C/SPI	消费级，~$1
9 轴 IMU	ICM-20948 / BNO055	I2C/SPI	+磁力计，BNO055 自带融合
温湿度	SHT30 / AHT20	I2C	±0.3°C / ±2%RH
气压	BMP280 / MS5611	I2C/SPI	±1m 高度分辨率
环境光	BH1750 / VEML7700	I2C	1~65535 lux
ToF 测距	VL53L0X / VL53L1X	I2C	2m/4m 量程
磁力计	QMC5883L / MMC5983MA	I2C	电子罗盘用

---

九、总结

层级	核心工作	关键点
驱动层	I2C/SPI 读写	WHO_AM_I 验证 + 错误处理
配置层	量程/采样率/滤波器	权衡精度和带宽
校准层	零偏/比例/温度补偿	校准比算法更重要
滤波层	低通/中值/卡尔曼	越简单越稳定
融合层	互补/卡尔曼/梯度下降	Mahony 在 MCU 上最实用

核心理念： 传感器开发的第一步不是上高级算法，而是做好校准和滤波。一个正确校准的简单滤波器，效果远好于一个未经校准的高级融合算法。

---