解读 105 · imu.rs:SFLP 解码与滤波
解读对象:
duck-control/src/imu.rs(355 行)
需要的前置:解读 06(同文件导览)、解读 05(消费 gravity 的检测器)
导览见解读 06,本篇逐函数下钻,不再重复其例证;重点放在它没展开的部分:状态字段、合法性检查的次序、以及文件后半那一排数学小函数。
1. 状态:解码器记住什么、为什么
ImuData(imu.rs:26-35)只有三个字段:gyro(躯干系角速度)、gravity(躯干系投影重力,直立是 [0,0,-1])、quat(scalar-first 的躯干→世界四元数);它的 Default(imu.rs:37-45)恰好是"直立",但这个默认值只有 ready() 之前有效——见第 2 节的门。解码器本体 SflpDecoder(imu.rs:55-67)的五个字段各管一事:mount 是挂载旋转;last_quat 持有芯片最近一次真正产出的四元数;quat_samples 数过多少个活块;gyro_history/gravity_history 是长度为 2 的历史,给中值滤波用。DEFAULT_MOUNT = [√½, 0, √½, 0](imu.rs:78-83)即 w=z=√2/2 的绕 Y 轴 +90° 旋转,对应安装关系 "trunk = [+raw_z, +raw_y, −raw_x]"。
2. decode:四道门的次序
decode(imu.rs:103)先解陀螺仪(前 6 字节,i16::from_le_bytes 乘 GYRO_RAD_PER_LSB,imu.rs:104-108),随后四元数部分是四道串联的门:块不全零(imu.rs:118,全零意味着 SFLP 未写表,保留 last_quat);三个分量 is_finite 且 norm_sq ≤ 1.02(imu.rs:123,容忍半精度舍入);w 用 w = (1.0 − norm_sq).max(0.0).sqrt() 补回(imu.rs:124,max(0.0) 挡住舍入出的负数开方);做挂载校正后范数要 > 0.5(imu.rs:133)才允许写入 last_quat 并 saturating_add(饱和计数,到顶不回绕)一次 quat_samples。挂载校正有个省事的写法:逆四元数 mount_inv 只把向量部分变号(imu.rs:125-130)——单位四元数的共轭就是这样。ready()(imu.rs:99)要求 quat_samples ≥ 25,约 100 Hz 下的四分之一秒;在此之前姿态是默认值而非测量值。
重力由 normalise(rotate_inverse(self.last_quat, [0.0, 0.0, -1.0]))(imu.rs:146)算出:把世界系的"下"变到躯干系再归一化。归一化放在中值之前,瞬态期间逐分量中值会略短于单位长——解读 06 讲过这个被刻意保留的行为,测试把边界钉在 0.5 与 1.0 之间(imu.rs:324-334)。
3. 五个数学小函数与一枚编译期断言
文件后半是一排无状态工具,每个都值得认识:half(imu.rs:171-181)手写 IEEE 754 binary16 解码,match 按指数分派——指数 0 走次正规数 frac × 2⁻²⁴,指数 0x1F 且尾数为 0 是无穷大、否则是 NaN,其余按 1 + frac/1024 乘 2^(exp−15);测试钉到 0x3555≈0.333(imu.rs:255)。mul(imu.rs:184-192)是 Hamilton 积,开头一行解构赋值 let ([aw, ax, ay, az], [bw, bx, by, bz]) = (a, b);(imu.rs:185)把两个数组的分量一次性拆成标量。rotate(q·v·q⁻¹,imu.rs:195)与 rotate_inverse(q⁻¹·v·q,imu.rs:205)共用 cross_terms(imu.rs:214-227):用 t = 2(v×l)、c = l×t 两步叉积替代完整四元数乘法,是四元数旋转向量的标准优化。median3_each(imu.rs:161-168)的三数中值压成一行 a.max(b).min(c).max(a.min(b))(imu.rs:162),逐分量各取一次。normalise(imu.rs:230-237)在模长小于 0.1 时回退到"直立"而不是除以近零。最后一枚断言值得念出声:const _: () = assert!(NUM_JOINTS > 0);(imu.rs:241)——注释说这是"关节总数与本模块保持独立的编译期保证":IMU 块是定长的,不管总线上还挂多少舵机。
你带走的收获
- 解码器的正确性藏在"门的次序"里:非零、有限、范数、再范数,每道门挡一类坏数据,顺序换了就会放进坏值。
- 逆变换不用完整四元数乘法:共轭就是向量部分变号,配合叉积展开,旋转一个三维向量只要两次叉积。
- 饱和计数(
saturating_add)是计数器阀门的标准写法:只数"发生过",不回绕。 const断言可以声明模块间"独立"这种关系——把架构意图写进编译期。