仓库：https://github.com/sim336/lsm6dsv16x-sflp
许可：Apache-2.0
关键词：Rust / 嵌入式 / IMU / I²C / LSM6DSV16X / SFLP / 机器人

摘要

ST 的 LSM6DSV16X 片上带一块传感器融合单元（SFLP），能把加速度计和陀螺仪融合成一个四元数输出。常见做法是买一块带 MCU 的载板，让 MCU 去轮询芯片、再把结果转发给主机。

我这次走的是另一条路：把裸片直接焊到 SoC 自己的 I²C 控制器上，由 Linux 直读寄存器，然后组装出和载板逐字节相同的 12 字节块——这样下游代码完全不需要知道现在挂的是哪一种。

坑不在寄存器表，而在三个会静默失效的地方：总线上不报错、回读全部自洽、程序看起来一切正常，但结果就是错的。这篇文章把这三个坑、发现过程、以及怎么用测试把它们钉死，一次讲清楚。

一、为什么要做这个

先说清楚问题。LSM6DSV16X 的 SFLP 块会输出一个 game-rotation vector（游戏旋转矢量），本质上是个四元数，主机拿去就能当姿态用，不用自己写 madgwick 或互补滤波。

通常的拿法是：载板上放一颗小 MCU，MCU 轮询芯片，把融合结果发到另一条总线上,主机读一个寄存器就完事。这个设计本身没问题——MCU 管时序，主机读一个数。

但它有两个副作用：

1. 芯片和「需要姿态的那一方」之间多了一个处理器；
2. 寄存器级的行为，主机永远看不到。

我这条路的做法是：把裸片接到 SoC 自己的 I²C 上，直读寄存器，然后组装出和载板一模一样的 12 字节。硬件的选择在「接缝」处消失了，下游的摔倒检测、策略观测、日志，全都是一条代码路径。

难点从来不是寄存器表，而是下面这三处静默失效。

---

二、两条通路，一张图

上面那张图就是全部结构。左边是传统载板路线，右边是裸片直读，两者在中间那个黄色的「接缝」框汇合：

	路径 A（载板 MCU）	路径 B（本仓库）
传输	一条串行总线，和舵机共用	独占自己的 I²C 控制器
谁跟芯片说话	载板上的 MCU	SoC 自己，直读
谁组装数据块	MCU	assemble_block()
融合姿态	SFLP，在芯片片上	SFLP，同一颗芯片
线上是什么	载板 20 字节里的 12 字节	同样那 12 字节，逐字节一致

线上格式（这就是整个接口，没有别的）：

字节	内容
0..6	OUTX_L_G…OUTZ_H_G，i16 小端原始计数，±500 dps
6..12	SFLP 旋转矢量x/y/z，IEEE half；w = sqrt(1 - x² - y² - z²)

注意没有什么：没有时间戳、没有序号、没有有效位标志。这正是关键——消费者无法分辨数据来自哪条路，两条路才真正可互换。

---

三、硬件与接线

芯片引脚	接到	说明
VDD	3.3 V
VDD_IO	3.3 V
GND	GND
SCL	主机 I²C SCL	我用的板子上是 40-pin 第 28 脚
SDA	主机 I²C SDA	40-pin 第 27 脚
SA0	3.3 V	决定地址：拉高0x6B，拉低 0x6A
CS	3.3 V	选中 I²C

坑 0：SA0 不要悬空

这个不算「静默失效」，但很值得先讲，因为它浪费了我最多时间。

SA0 悬空不是第三种状态——它会拾取走线附近的电平。我那块板子上表现为器件在 0x6A / 0x6B 之间以大约 50 Hz 来回跳，内核侧表现为 ENXIO，数据里看不出任何规律。

当时为了搞清楚它到底在哪个地址，我写了两个诊断脚本（后来都进了仓库的 tools/）。SA0 接上电源轨之后这一切就消失了，地址固定 0x6B。

另外：不要扫描总线找它。 它的地址只在 0x6A 和 0x6B 之间可跳，扫描只会扫到错的东西。ADDRESS 在驱动里是个常量，不是扫描结果。

---

四、十分钟上手

第一步：Python 探针（不用编译）

这是最快拿到是/否的地方，而且除非你允许，它一个寄存器都不写：

python3 tools/body_imu_probe.py --bus /dev/i2c-4 --address 0x6B

它做三件事：i2cdetect 看 0x6B 有没有应答 → 读 WHO_AM_I 是不是 0x70 → 配置并等一个 SFLP 词。

退出码：0 全过；1 没应答或 WHO_AM_I 不对；2 没等到 SFLP 词；3 打不开总线。

我板子上的真实输出：

1) i2cdetect: looking for 0x6B
  60: -- -- -- -- -- -- -- -- -- -- -- 6b -- -- -- --
2) WHO_AM_I(0x0F)
  read 0x70, expected 0x70
3) configure (the driver's own sequence)
  CTRL3              0x12 <- 0x44  read back 0x44  OK
  CTRL1              0x10 <- 0x07  read back 0x07  OK
  CTRL2              0x11 <- 0x07  read back 0x07  OK
  CTRL6              0x15 <- 0x02  read back 0x02  OK
  CTRL8              0x17 <- 0x02  read back 0x02  OK
  FIFO_CTRL4         0x0A <- 0x00  read back 0x00  OK
  FIFO_CTRL3         0x09 <- 0x00  read back 0x00  OK
  SFLP_ODR           0x5E <- 0x18  read back 0x18  OK
  EMB_FUNC_EN_A      0x04 <- 0x02  read back 0x02  OK
  EMB_FUNC_FIFO_EN_A 0x44 <- 0x02  read back 0x02  OK
4) waiting for an SFLP game-rotation word (up to 5 s)
  poll 2: 3 word(s) queued
  raw   : 9f ea 31 73 3a 75 af
  tag   : 0x9F (bits[7:3] = 0x13, expected 0x13)
  x/y/z : 0x31EA 0x3A73 0xAF75  -> +0.18481 +0.80615 -0.11652
  w     : 0.16477  (|xyz| = 0.83523, must be <= 1)
verdict: 3/3 passed

不想写寄存器就先跑 --read-only，它只做 i2cdetect 和 WHO_AM_I。

第二步：Rust 台架程序

cargo build --release
./target/release/bench_imu --bus /dev/i2c-4 --hz 50 --seconds 10

它打印 WHO_AM_I、逐寄存器回读表、每秒一行统计、总计行，最后一行 attitude。

第三步：当依赖用

[dependencies]
lsm6dsv16x-sflp = { git = "https://github.com/sim336/lsm6dsv16x-sflp" }

核心代码短到可以贴全：

use lsm6dsv16x_sflp::{BoardBodyImu, SflpDecoder, MOUNT, ADDRESS};

let bus = linux_embedded_hal::I2cdev::new("/dev/i2c-imu")?;
let mut imu = BoardBodyImu::new(bus, ADDRESS);
imu.configure()?;

let mut decoder = SflpDecoder::new(MOUNT);  // MOUNT 逐板而定，务必自己测一次
loop {
    let block = imu.read_block()?;          // 12 字节，与载板一致
    let attitude = decoder.decode(&block);
    if decoder.ready() {                    // 约 0.25 s 的实时融合输出之后
        println!("gravity = {:?}", attitude.gravity);
        break;
    }
}

decoder.ready() 之前，那个姿态是默认值，不是测量值——摔倒检测不能在这之前跑。

---

五、三个静默陷阱（本文核心）

这三个的共同特征：总线上不报错，回读全部自洽，bring-up 全绿，但结果是错的。

陷阱 1：一次寄存器写必须是一条 i2c_msg

这是整个 bring-up 最要命的一条。

写法	线上字节	结果
合并（正确）	START addr reg data… STOP	寄存器拿到值
拆开（错误）	START addr reg RESTART addr data… STOP	芯片对每个字节都 ACK，然后丢掉载荷

拆开的写法会让寄存器停在旧值。回读自洽，因为什么都没变过。bring-up 报告成功。没有 NACK，没有错误标志，什么都没有——一颗被拆开写法配置过的芯片就是不融合，唯一的症状是姿态永远不来。

我是靠受控对照 6/6 次定位的：i2c_msg 的 len = 1 + n（寄存器和载荷在同一个 message 里）会落地；同样的写拆成两条 message 就不会。

Rust 这边是构造上正确的：embedded-hal 1.0 的默认 I2c::write 实现就是一个 Operation::Write，而 linux-embedded-hal 的 I2cdev 把一个 operation 变成一条 i2c_msg。所以 self.i2c.write(addr, &[reg, value]) 就是合并写法，整个 crate 里不存在第二种写法。

但「构造上正确」正是需要测试而不是注释的理由：

// 断言的是事务的「形状」，不是字节——两条 operation 就是两条 i2c_msg
#[test]
fn every_register_write_is_a_single_i2c_message() { /* ... */ }

顺带一提，我在前一个项目参考的 C 驱动里，这个 bug 是真实存在的。

陷阱 2：陀螺满量程必须 ±500 dps，这是与解码器的契约

SflpDecoder 用 GYRO_RAD_PER_LSB = 0.0175 × π/180 rad/s per count 来缩放陀螺计数——这就是 LSM6DSV16X 的 ±500 dps 灵敏度，没有别的解释。所以 configure() 写 CTRL6.FS_G = 0b0010。

总线上永远不会告诉你满量程是多少，设错了哪里都不报错——只是解出来的角速度不对。而附近的坑非常具体：

同类项目的参考 C 驱动跑的是 ±250 dps（灵敏度翻倍）。照抄它的序列，不会报错，只是把每一个角速度都算成一半。

所以这个测试不复述常量：

#[test]
fn gyro_full_scale_matches_the_decoder() {
    // 从真正写下去的那个字节里读出 FS_G 字段，
    // 经数据手册的 mdps/LSB 表换算，
    // 再和「把 1000 counts 过一个真 SflpDecoder」测出来的灵敏度比较。
    // → 设成 ±250 会让这个测试失败，而不是悄悄把每个角速度减半。
}

陷阱 3：安装旋转逐板而定，抄不得

最容易在自己接线时踩的一个。安装旋转不是芯片的属性，是芯片怎么被拧上去的属性。同一颗芯片，两块板可以差 180°，而失败的表现是：机器人静止站着，却报告自己倒过来了，同时每一个寄存器回读都自洽。

这个 crate 里：

• MOUNT = 绕 Y 轴 −90°（trunk = [−raw_z, +raw_y, +raw_x]），是我这块板子上实测的；
• SflpDecoder::V2_BOARD_MOUNT = 绕 Y 轴 +90°（trunk = [+raw_z, +raw_y, −raw_x]），是另一块板的。

两者差 180°，不可互换——这也正是它叫 V2_BOARD_MOUNT（按它描述的那块板命名）而不是 DEFAULT 的原因。

自己测，两次已知姿态就够。 装好的陀螺仪是唯一一种「不管怎么转、静止输出都一样」的传感器,所以只有重力能说话：

姿态	重力（躯干坐标系）	重力（芯片坐标系）	残差
直立	[-0.132, +0.102, +0.986]	[-0.986, +0.102, -0.132]	9.6°
前倾 90°（趴着）	[-0.995, -0.007, -0.103]	[+0.103, -0.007, -0.995]	5.9°

「芯片坐标系」那一列是把 V2_BOARD_MOUNT 撤销之后的同样两次读数——是芯片的纯粹属性，与被拟合的对象无关。把 24 种轴对齐旋转全打分（直立 → (0,0,-1)，前倾 → (1,0,0)），这个解总分 15.5°，第二名差 90.1°,所以解是唯一的，残差是鸭子站在台面上的自身倾斜，不是歧义。

如果你已经有 bench_imu，就这么做：

1. 把板子摆成已知的直立姿态，读 attitude 行；
2. 直立必须打印 gravity=[+0.000, +0.000, -1.000]，别的就是安装旋转；
3. 前倾 90°（趴着）再读一次，重力应该变成 [+1, 0, 0]；
4. 拟合出同时满足这两次的旋转，传给 SflpDecoder::new(your_mount)。

附赠：SFLP_ODR 的字段在 [5:3]，不在 [2:0]

这个错误长得特别像对的：

• SFLP_GAME_ODR = 011（120 Hz）位于 SFLP_ODR 的 bits [5:3]，不是 [2:0]；
• 这个寄存器的复位值回读是 0x03，作为一个枚举值看起来完全正确——但 0x03 是把 011 放进 [2:0]，也就是真字段里的 000，即 15 Hz；
• 同类项目的参考 C 驱动写的正是这个值，所以它注释里的「120 Hz」实际是 15 Hz。

我实测：字段在 011 时 118 words/s，在 000 时 14.8 words/s。一个「非空」的 FIFO 分辨不出这两者——所以 bring-up 工具报的是速率，不是有/没有。

---

六、FIFO 的一个反直觉设计

一次没有融合词的 tick，照样产出 12 字节，四元数槽置零。

这不是权宜之计：全零四元数字节本来就是 SflpDecoder 读作「保持上一个好姿态」的哨兵值，因为载板在 SFLP 写好表之前输出的就是这个。所以空 tick 在两端都是零成本的。

但它也意味着：数据块本身无法告诉你这次有没有词——活的 tick 和空的 tick 逐字节相同，这是故意的。这就是 FifoReport 存在的原因，也是 FIFO 那几条自己的规则存在的原因：

• 一个 FIFO 词是 7 字节：tag 字节 + 6 个数据字节；
• SFLP 旋转矢量的 tag 在 bits [7:3]，等于 0x13；tag 不对的词跳过，不猜；
• 全零载荷是 SFLP 在说「还没写好」，也跳过——当成单位四元数传出去，会把一个侧躺的机器人报成直立；
• MAX_FIFO_WORDS_PER_TICK = 4 把每个 tick 内的开销封顶。FIFO 是从最旧往新读的，所以丢掉的余量是最新的词，下个 tick 会收走。

还有一个「总线成功、芯片应答、数据块解码成功、姿态却冻住」的故障，只有 StaleImuTracker 抓得住：芯片的融合输出在静止时每位都在变，所以逐字节相同的块意味着数据流停了，不是机器人停了。连跑 25 次（50 Hz 下 0.5 s）才报警——低于这个数保持安静，是刻意的。

---

七、实测数据

不是对你这块板的承诺，是一块板（Radxa Zero 3，RK3566）在 50 Hz、/dev/i2c-4 0x6B 上真实做出来的：

项目	数值
WHO_AM_I	0x70
配置回读	10 / 10 个寄存器与写入一致	没有写丢
吞吐	30.0 s 内 1501 个 tick = 50.0 Hz	1501 个 SFLP 词，0 错误
读耗时	均值3.5–4.3 ms，p95 4.0–4.9 ms，最大 5.3 ms	装在 20 ms 的 tick 里，约占 20%
tick 抖动	p9540–170 µs，最差 907 µs
FIFO 空	0%
SFLP 词存在	100%	每个 tick 都有融合输出
|g|、|q|	1.000、1.000	重力与四元数都是单位向量

结论：读耗时均值 3.5–4.3 ms 对 20 ms 的 tick，意味着一次读可以放在 50 Hz 控制环内部完成。如果你的数字不是这样，退路是开一个线程发布最新的 ImuData——但别假设，去测。

把 attitude 那行老实读出来

attitude  gravity=[-0.367, -0.014, +0.930]  quat(wxyz)=[-0.180, +0.232, +0.955, +0.051]

它说明两件事：

• 它不是 [0, 0, -1]——因为当时机器人不在标定安装旋转时用的那个直立姿态上。这不是错误，恰恰是上面第 3 个陷阱里让你预期的结果，也是你必须为自己的板子重做的那次测量。
• 两次运行之间，四元数的 yaw 分量变了，重力没变。 game-rotation vector 没有磁力计，所以没有绝对航向——yaw 会漂，重力不受影响。任何需要绝对航向的东西，都需要这颗芯片没有的磁力计。

---

八、这三个坑是怎么被「钉住」的

一个 Recorder mock 实现 embedded_hal::i2c::I2c，记录每笔事务的形状（一个 operation 还是两个）——因为芯片静默丢掉的正是这个形状。22 个测试，全部不需要硬件。

测试	钉住什么
every_register_write_is_a_single_i2c_message	陷阱 1——断言形状，不是字节
gyro_full_scale_matches_the_decoder	陷阱 2——反查数据手册，不复述常量
mount_reports_the_upright_calibration_as_upright	陷阱 3——并断言V2_BOARD_MOUNT 给出镜像
assembled_block_decodes_identically_to_the_mcu_board	接缝：两个独立解码器对 4 个 tick 返回相等ImuData
no_sflp_word_holds_the_last_quaternion	空 tick ≠ 弹回直立
fifo_report_tells_a_live_word_from_an_empty_fifo	数据块说不出自己是空的
who_am_i_must_be_0x70	陌生器件在任何写之前就被拒绝

configure() 对着 mock 跑的就是真实 bring-up 序列，mock 忠实地模拟了「写被丢掉」这种情形。

cargo test        # 22 个测试，无硬件
cargo clippy --all-targets
cargo fmt --check

---

九、诚实的未知项

没测过的东西写在这里，而不是留给人猜：

• MOUNT 逐板而定。 它描述芯片怎么被拧上去。你的几乎肯定不一样，去测它。
• 故意不加滤波器。 陀螺 LPF1 与加速度计 LPF2 都关着：同类板的滤波设置通常不公开，所以这里任何选择都无从对照。原始计数直接进解码器，由解码器用「对三个取中位数」挡掉单采样尖刺。
• 不发 SW_RESET / BOOT。 configure() 把它依赖的每个寄存器都写一遍，所以一颗被别的进程留成热态的芯片，与冷启动一样良定义——这是构造上的保证，不是测量。
• 陀螺噪声的温漂没有刻画。 中位数滤波和融合四元数覆盖了温度扫频要覆盖的同一片地，但那是论证，不是测量。
• imu2uart 的参考 C 驱动没有被收录。 它的著作权归属未定，而本 crate 也不需要它。凡是本 crate 与它有意不同的地方（满量程、SFLP ODR 字段、滤波器），差异都被点名了。

---

十、仓库与许可

• 仓库：https://github.com/sim336/lsm6dsv16x-sflp
• 文档：docs/design.md（设计推理）、docs/bring-up.md（接线与四步检查）、docs/register-map.md（每个寄存器与理由）
• 中英双语 README；tools/ 里五个 Python 探针，不需要编译，在目标板上直接跑
• 许可：Apache-2.0（SflpDecoder 等派生自 pollen-robotics/microduck，同样 Apache-2.0，逐文件改动列在 NOTICE）

结语

这三条陷阱有个共同点：它们都不会让编译失败，也不会让总线报错。 所以我把它们都写成了测试——注释不会失败，测试会。

如果这篇帮你省掉一次「姿态冻住却找不到原因」的调试，那就值了。