第四章：传感器数据采集与处理

好，咱们进入正题。传感器是飞控系统的眼睛和耳朵，没有它们，飞控就是个瞎子。这一章我带你搞定I2C/SPI总线驱动、传感器数据读取，还有滤波处理——低通滤波和卡尔曼滤波的入门。

4.1 总线选择：I2C vs SPI

飞控上常用的传感器，比如加速度计、陀螺仪、磁力计、气压计，通信接口无非两种：I2C和SPI。很多新手会纠结选哪个，我直接说结论：

• I2C：两根线（SDA、SCL），支持多设备挂载，速度一般（100kHz-400kHz）。适合对速率要求不高的传感器，比如温度、气压。
• SPI：四根线（MOSI、MISO、SCK、CS），全双工，速度可以跑到几十MHz。适合高数据率的传感器，比如IMU（惯性测量单元）。

我个人习惯，IMU用SPI，其他传感器用I2C。为什么？因为IMU数据更新率通常1kHz以上，SPI的吞吐量更稳。我在项目里遇到过I2C总线被其他设备拖慢，导致IMU数据丢帧的情况——嗯，从那以后IMU我坚决走SPI。

4.2 I2C总线驱动实现

I2C的驱动其实不复杂，核心就是起始条件、停止条件、数据收发。我直接给一个精简的代码框架，你拿去改改就能用。

// I2C 主模式发送
void i2c_start(void) {
    SDA_HIGH();
    SCL_HIGH();
    delay_us(5);
    SDA_LOW();   // SCL高时SDA拉低 → 起始条件
    delay_us(5);
    SCL_LOW();
}

void i2c_stop(void) {
    SDA_LOW();
    SCL_HIGH();
    delay_us(5);
    SDA_HIGH();  // SCL高时SDA拉高 → 停止条件
    delay_us(5);
}

uint8_t i2c_write_byte(uint8_t data) {
    for (int i = 0; i < 8; i++) {
        if (data & 0x80) SDA_HIGH();
        else SDA_LOW();
        SCL_HIGH();
        delay_us(2);
        SCL_LOW();
        data <<= 1;
    }
    // 接收ACK
    SDA_INPUT();
    SCL_HIGH();
    uint8_t ack = (SDA_READ() == 0) ? 0 : 1;
    SCL_LOW();
    SDA_OUTPUT();
    return ack;
}

你想想看，I2C的时序其实很固定。我刚开始写驱动时，总在起始条件和停止条件上栽跟头——延时不够，设备不响应。后来我统一用5微秒的延时，再没出过问题。

4.3 SPI总线驱动实现

SPI比I2C简单，因为它没有地址和ACK机制。你只需要控制时钟极性和相位（CPOL、CPHA），然后按位收发数据。

// SPI 单字节收发
uint8_t spi_transfer(uint8_t data) {
    for (int i = 0; i < 8; i++) {
        if (data & 0x80) MOSI_HIGH();
        else MOSI_LOW();
        SCK_HIGH();
        delay_us(1);
        data <<= 1;
        if (MISO_READ()) data |= 0x01;
        SCK_LOW();
        delay_us(1);
    }
    return data;
}

// 读取传感器寄存器
uint8_t spi_read_reg(uint8_t reg) {
    CS_LOW();
    spi_transfer(reg | 0x80);  // 读命令，最高位置1
    uint8_t val = spi_transfer(0x00);
    CS_HIGH();
    return val;
}

这里要注意：很多SPI传感器要求先发寄存器地址，再读数据。我踩过一个坑——MPU9250的SPI读时序，地址字节的最高位必须置1表示读，否则它不理你。当时查了两天手册才发现。

4.4 传感器数据读取实战

以MPU6050（六轴IMU）为例，读取加速度和角速度的流程：

1. 初始化I2C/SPI接口
2. 配置传感器（设置量程、采样率、低通滤波器）
3. 读取数据寄存器（连续读取6个字节）
4. 拼接成16位整数，转换为物理值

// 读取MPU6050加速度数据
void mpu6050_read_accel(float *ax, float *ay, float *az) {
    uint8_t buf[6];
    i2c_read_regs(MPU6050_ADDR, ACCEL_XOUT_H, buf, 6);
    int16_t raw_x = (buf[0] << 8) | buf[1];
    int16_t raw_y = (buf[2] << 8) | buf[3];
    int16_t raw_z = (buf[4] << 8) | buf[5];
    // 假设量程±2g，灵敏度16384 LSB/g
    *ax = raw_x / 16384.0f;
    *ay = raw_y / 16384.0f;
    *az = raw_z / 16384.0f;
}

为什么除以16384？因为MPU6050在±2g量程下，1g对应16384 LSB。这个值在数据手册里叫灵敏度（Sensitivity Scale Factor）。我建议你每次换传感器都先确认这个值，不然算出来的物理量全是错的。

4.5 低通滤波入门

传感器原始数据通常有高频噪声，比如电机振动、电磁干扰。低通滤波就是把这些高频成分滤掉。最简单的是一阶RC低通滤波，在数字域实现：

// 一阶低通滤波
float lowpass_filter(float input, float prev_output, float alpha) {
    return alpha * input + (1 - alpha) * prev_output;
}
// alpha = T / (T + RC)，T为采样周期，RC为时间常数

alpha值越小，滤波越平滑，但延迟越大。我一般取0.1~0.3之间。举个例子，采样率1kHz，alpha=0.1，截止频率大约16Hz——适合滤除电机振动，但不会影响飞行姿态的响应。

4.6 卡尔曼滤波入门

低通滤波只能处理单一信号，但卡尔曼滤波能融合多个传感器的数据。说白了，它是在「预测」和「观测」之间找一个最优估计。

卡尔曼滤波的核心公式就五个，我直接给你最简版本：

// 一维卡尔曼滤波（用于角度估计）
typedef struct {
    float Q;  // 过程噪声协方差
    float R;  // 测量噪声协方差
    float x;  // 状态估计
    float P;  // 估计误差协方差
} Kalman1D;

void kalman_init(Kalman1D *k, float Q, float R) {
    k->Q = Q;
    k->R = R;
    k->x = 0;
    k->P = 1;
}

float kalman_update(Kalman1D *k, float measurement) {
    // 预测
    float P_pred = k->P + k->Q;
    // 更新
    float K = P_pred / (P_pred + k->R);  // 卡尔曼增益
    k->x = k->x + K * (measurement - k->x);
    k->P = (1 - K) * P_pred;
    return k->x;
}

你看，代码其实很短。但调参才是关键——Q和R怎么设？Q代表你对模型的信任程度，R代表你对传感器的信任程度。我一般先设R为传感器噪声方差（可以从数据手册或实测得到），然后调Q让响应速度和噪声抑制达到平衡。

4.7 低通滤波 vs 卡尔曼滤波：怎么选？

对比项	低通滤波	卡尔曼滤波
计算量	极低（一次乘加）	中等（几次乘除）
延迟	固定，与alpha相关	动态，与噪声相关
多传感器融合	不支持	天然支持
适用场景	单一信号去噪	姿态估计、定位融合

我的建议是：如果只是滤掉高频噪声，低通滤波就够了。但如果你要做姿态解算（比如融合加速度计和陀螺仪），卡尔曼滤波是更好的选择。我在做四旋翼飞控时，角度估计用的就是卡尔曼滤波，效果比单纯低通好很多。

4.8 本章知识体系

下面这张图总结了传感器数据采集与处理的核心流程，你一看就明白：

从传感器原始数据到可用信号，中间经历了总线驱动、数据读取、物理量转换、滤波处理四个环节。每个环节都有坑，但只要你按部就班调试，都能搞定。