驱动核心代码编写：I2C读写函数实现、数据解析与发布、参数配置与校准
好，咱们接着往下走。上一章我们把I2C设备注册进了PX4框架，设备能"活"起来了。但说实话，那只是个空壳子。真正的硬仗，在于怎么跟硬件打交道——读写寄存器、解析原始数据、把数据喂给上层，还得让用户能调参数。
这一章，我带你手撸核心代码。我会把我在几个量产项目里踩过的坑，都抖出来。
I2C读写函数实现：别小看这几十行代码
很多新手觉得I2C读写就是调个API，有啥好写的？嗯，我以前也这么想。直到有一次，一个传感器在低温下频繁丢数据，查了三天才发现是读写时序没处理好。
在PX4里，我们通常封装两个底层函数：_read_reg() 和 _write_reg()。我习惯把它们写成私有成员函数，放在类的private区域。
// 读一个寄存器
int MySensor::_read_reg(uint8_t reg, uint8_t &val)
{
uint8_t cmd = reg;
int ret = transfer(&cmd, 1, &val, 1);
if (ret != PX4_OK) {
DEVICE_DEBUG("read reg 0x%02x failed", reg);
return ret;
}
return PX4_OK;
}
// 写一个寄存器
int MySensor::_write_reg(uint8_t reg, uint8_t val)
{
uint8_t buf[2] = {reg, val};
int ret = transfer(buf, 2, nullptr, 0);
if (ret != PX4_OK) {
DEVICE_DEBUG("write reg 0x%02x failed", reg);
return ret;
}
return PX4_OK;
}
我的小习惯：每次读写失败都打一条DEBUG日志。量产时关掉DEBUG宏，但开发阶段这些日志能救命。我曾经靠一条"read reg failed"日志，定位到某批次传感器I2C地址被焊错了。
对于多字节读取，我建议封装一个批量读函数。很多传感器支持连续地址读取，一次拿回所有数据，效率高很多。
int MySensor::_read_multi(uint8_t reg, uint8_t *buf, uint8_t len)
{
int ret = transfer(®, 1, buf, len);
if (ret != PX4_OK) {
DEVICE_DEBUG("multi read reg 0x%02x len %d failed", reg, len);
}
return ret;
}
注意：有些传感器在连续读取时，地址会自动递增。但有些不会！你得看数据手册里的"Auto-increment"位是否支持。我遇到过一款气压计，连续读6个字节，结果读回来全是同一个寄存器的值——因为它的地址指针根本没动。
数据解析与发布：从原始字节到有用信息
数据读回来了，但都是原始字节。比如一个16位的温度值，可能是大端序，也可能是小端序。我见过最坑的是某款IMU，同一个寄存器在不同模式下字节序还不一样。
解析的第一步，是搞清楚数据格式。我一般会画个表格，贴在代码注释里：
| 寄存器地址 | 字节0 | 字节1 | 字节2 | 字节3 |
|---|---|---|---|---|
| 0x04 | 温度高8位 | 温度低8位 | 压力高8位 | 压力低8位 |
然后写解析函数。我习惯用联合体或者直接位运算：
void MySensor::_parse_sensor_data(const uint8_t *raw)
{
// 温度：16位有符号，大端序，单位0.01°C
int16_t temp_raw = (int16_t)((raw[0] << 8) | raw[1]);
float temperature = temp_raw * 0.01f;
// 压力：16位无符号，小端序，单位Pa
uint16_t press_raw = (uint16_t)(raw[2] | (raw[3] << 8));
float pressure = press_raw * 1.0f;
}
解析完数据，下一步就是发布。PX4里传感器数据通过uORB发布。我通常用 sensor_baro_s 或者 sensor_accel_s 这些现成的消息类型。
void MySensor::_publish_data()
{
sensor_baro_s baro{};
baro.timestamp = hrt_absolute_time();
baro.temperature = _temperature;
baro.pressure = _pressure;
_baro_pub.publish(baro);
}
关键点：timestamp一定要用硬件时间戳，别用软件延时算出来的。我见过有人用 hrt_absolute_time() 在读取前打一次、读取后打一次，然后取平均——其实没必要，读取完成后的时间戳就够用了。
参数配置与校准：让用户能调，让数据更准
参数配置，说白了就是让用户能在QGroundControl里改一些值，比如采样率、滤波器系数。PX4用参数系统来做这个，你只需要在代码里注册参数，然后监听变化。
我一般这样注册参数：
// 在构造函数里
_param_handle_rate = param_find("SENS_MY_RATE");
param_get(_param_handle_rate, &_param_rate);
然后在 _update_params() 里检查参数是否变了：
void MySensor::_update_params()
{
int32_t new_rate;
param_get(_param_handle_rate, &new_rate);
if (new_rate != _param_rate) {
_param_rate = new_rate;
_set_sampling_rate(_param_rate);
DEVICE_DEBUG("sampling rate updated to %d Hz", _param_rate);
}
}
避坑指南：参数更新函数不要放在中断里调用。我曾经把 param_get() 放在高优先级线程里，结果导致参数系统死锁。正确的做法是在主循环或者定时器回调里调用。
校准这块，不同传感器差别很大。对于气压计，我通常做零偏校准：
void MySensor::_calibrate_pressure()
{
// 采集100个样本取平均作为零偏
float sum = 0.0f;
for (int i = 0; i < 100; i++) {
sum += _pressure;
usleep(1000); // 1ms间隔
}
_pressure_offset = sum / 100.0f;
DEVICE_DEBUG("pressure offset: %.2f Pa", _pressure_offset);
}
校准完成后，把偏移量存到参数里，下次上电直接读取：
param_set(param_find("SENS_MY_OFFSET"), &_pressure_offset);
重要提醒：校准过程中不要移动传感器！我见过有人拿着飞控在手里晃来晃去地校准气压计，结果校准出来的偏移量比正常值大了10倍。校准环境要稳定，最好放在桌面上静置几秒钟。
本章小结
这一章我们干了三件事：
- 写了I2C读写函数，注意了错误处理和批量读取
- 实现了数据解析，用表格和位运算把原始字节变成物理量
- 配置了参数系统，让用户能调采样率；做了零偏校准，让数据更准
代码量不大，但每一行都关系到传感器能不能稳定工作。我见过太多"能跑就行"的代码，结果一到现场就出问题。驱动开发，说白了就是跟硬件较劲——你越细心，它越听话。