refactor: enforce layered crates and Rust audit gates
This commit is contained in:
Horldsence
@@ -0,0 +1,21 @@
|
||||
[package]
|
||||
name = "delta10-protocol"
|
||||
description = "DETA10 IMU wire protocol"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
imu = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "protocol/imu"
|
||||
@@ -0,0 +1,301 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! DETA10 IMU 的 CAN 帧编解码（ARCHITECTURE.md §5.2 / 决策点 **D1**）。
|
||||
//!
|
||||
//! # 为什么这里没有"实现好"的解码
|
||||
//!
|
||||
//! ARCHITECTURE.md §11.4 把「delta10 的 CAN ID 和协议模式」列为**需要人工确认的
|
||||
//! 决策点 D1**（需查阅数据手册），而文档 §12.2 给出的手册链接
|
||||
//! `https://fdisystems.github.io/DETA10_Docs/` **已经 404**，厂商现手册为
|
||||
//! `fdisystems.cn` 上的 PDF。
|
||||
//!
|
||||
//! 在没有手册的前提下臆造字节偏移，比留一个空实现更糟：它会以"看起来能用"
|
||||
//! 的姿态把错误的 roll/pitch/yaw 送进云台闭环。因此本模块：
|
||||
//!
|
||||
//! 1. 把**能确定的部分**做实：帧 ID 匹配、DLC 校验、`ImuData` 组装路径；
|
||||
//! 2. 把**不能确定的部分**参数化为 [`Delta10Layout`]（字段偏移 + 标度）；
|
||||
//! 3. 未配置布局时解码返回 [`ImuError::LayoutUnconfirmed`]（显式、可测、不会
|
||||
//! 静默给错数据）。D1 确认后**只需填一组常量**，无需改动任何调用方。
|
||||
//!
|
||||
//! # 已知的确定事实
|
||||
//!
|
||||
//! - 设备经 CAN 接入，挂在与电机共享的总线上（ARCHITECTURE.md §2.1）；
|
||||
//! - 文档记录了两种协议模式：FDILINK 与「标准」，见 [`Delta10Mode`]；
|
||||
//! - 上层需要的是 [`ImuData`]（accel / gyro / mag / roll / pitch / yaw / 时间戳）。
|
||||
|
||||
use imu::{ImuData, ImuError};
|
||||
use thiserror::Error;
|
||||
|
||||
use can::CanFrame;
|
||||
|
||||
/// 载荷里数值字段的宽度与编码。
|
||||
///
|
||||
/// DETA10 的 FDILink 帧用 `float32`，但一个 8 字节 CAN 帧装不下三个 `f32`，
|
||||
/// 因此真实设备必然是"多帧"或"定点压缩"之一——**这正是 D1 要确认的内容**。
|
||||
/// 把宽度显式建模出来，D1 确认后只需填一张常量表。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum FieldWidth {
|
||||
/// 4 字节小端 `f32`。
|
||||
F32,
|
||||
/// 2 字节小端 `i16`（定点，配合 `scale`）。
|
||||
I16,
|
||||
}
|
||||
|
||||
impl FieldWidth {
|
||||
/// 字节数。
|
||||
#[must_use]
|
||||
pub const fn bytes(self) -> usize {
|
||||
match self {
|
||||
Self::F32 => 4,
|
||||
Self::I16 => 2,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 载荷里一个数值字段的定义（偏移 + 宽度 + 标度）。
|
||||
///
|
||||
/// 有了它，"确认手册后要改什么"就只剩一张表。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct FieldLayout {
|
||||
/// 在 8 字节载荷中的起始偏移。
|
||||
pub offset: usize,
|
||||
/// 数值宽度与编码。
|
||||
pub width: FieldWidth,
|
||||
/// 原始值 → 物理量的比例（如 0.001 表示原始值单位是 1/1000）。
|
||||
pub scale: f32,
|
||||
}
|
||||
|
||||
impl FieldLayout {
|
||||
/// 构造。
|
||||
#[must_use]
|
||||
pub const fn new(offset: usize, width: FieldWidth, scale: f32) -> Self {
|
||||
Self { offset, width, scale }
|
||||
}
|
||||
|
||||
/// 从载荷里取一个字段；越界返回 `None`。
|
||||
#[must_use]
|
||||
pub fn read(&self, data: &[u8; 8]) -> Option<f32> {
|
||||
let end = self.offset.checked_add(self.width.bytes())?;
|
||||
if end > data.len() {
|
||||
return None;
|
||||
}
|
||||
let raw = match self.width {
|
||||
FieldWidth::F32 => {
|
||||
let mut bytes = [0u8; 4];
|
||||
bytes.copy_from_slice(&data[self.offset..end]);
|
||||
f32::from_le_bytes(bytes)
|
||||
}
|
||||
FieldWidth::I16 => {
|
||||
let mut bytes = [0u8; 2];
|
||||
bytes.copy_from_slice(&data[self.offset..end]);
|
||||
f32::from(i16::from_le_bytes(bytes))
|
||||
}
|
||||
};
|
||||
Some(raw * self.scale)
|
||||
}
|
||||
}
|
||||
|
||||
/// DETA10 载荷布局（**待 D1 确认**）。
|
||||
///
|
||||
/// 单帧 8 字节装不下 9 个浮点量，因此真实设备要么用多帧、要么用定点压缩。
|
||||
/// 本结构支持"单帧 + 字段子集"的形态；多帧重组待手册确认后按同样思路扩展。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Delta10Layout {
|
||||
/// 横滚角（度）字段。
|
||||
pub roll: FieldLayout,
|
||||
/// 俯仰角（度）字段。
|
||||
pub pitch: FieldLayout,
|
||||
/// 偏航角（度）字段。
|
||||
pub yaw: FieldLayout,
|
||||
}
|
||||
|
||||
/// 协议模式（ARCHITECTURE.md §2.2 记录了两者，先按哪种接需 D1 确认）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum Delta10Mode {
|
||||
/// FDILINK 协议模式。
|
||||
Fdilink,
|
||||
/// 标准模式。
|
||||
Standard,
|
||||
}
|
||||
|
||||
/// DETA10 接入配置。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Delta10Config {
|
||||
/// 反馈帧的 CAN ID（**待 D1 确认**，默认取 `0x100` 占位）。
|
||||
pub can_id: u32,
|
||||
/// 协议模式。
|
||||
pub mode: Delta10Mode,
|
||||
/// 载荷布局；`None` 表示尚未确认，解码会返回
|
||||
/// [`ImuError::LayoutUnconfirmed`]。
|
||||
pub layout: Option<Delta10Layout>,
|
||||
}
|
||||
|
||||
impl Default for Delta10Config {
|
||||
fn default() -> Self {
|
||||
// 0x100 只是占位（ARCHITECTURE.md §10.3 的 robot.toml 里也把它写成
|
||||
// `imu_can_id = 0x100`，同样标注为待确认）。它**不应**被当作事实使用：
|
||||
// 布局为 None，因此默认配置下解码一定报 LayoutUnconfirmed。
|
||||
Self { can_id: 0x100, mode: Delta10Mode::Fdilink, layout: None }
|
||||
}
|
||||
}
|
||||
|
||||
impl Delta10Config {
|
||||
/// 换一个 CAN ID（D1 确认后由配置注入）。
|
||||
#[must_use]
|
||||
pub const fn with_can_id(mut self, can_id: u32) -> Self {
|
||||
self.can_id = can_id;
|
||||
self
|
||||
}
|
||||
|
||||
/// 换协议模式。
|
||||
#[must_use]
|
||||
pub const fn with_mode(mut self, mode: Delta10Mode) -> Self {
|
||||
self.mode = mode;
|
||||
self
|
||||
}
|
||||
|
||||
/// 装上载荷布局（D1 确认后调用）。
|
||||
#[must_use]
|
||||
pub const fn with_layout(mut self, layout: Delta10Layout) -> Self {
|
||||
self.layout = Some(layout);
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// 解码结果：`Ok(None)` = 不是 IMU 帧（正常情况，总线上大部分帧是电机反馈）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// - [`ImuError::BadFrame`]：ID 匹配但长度不足；
|
||||
/// - [`ImuError::LayoutUnconfirmed`]：ID 匹配但布局未确认（D1）；
|
||||
/// - [`ImuError::Checksum`]：字段读取越界（布局与载荷不匹配）。
|
||||
pub fn try_decode(
|
||||
config: &Delta10Config,
|
||||
frame: &CanFrame,
|
||||
timestamp_ns: u64,
|
||||
) -> Result<Option<ImuData>, ImuError> {
|
||||
if frame.id != config.can_id {
|
||||
return Ok(None);
|
||||
}
|
||||
if !frame.is_complete() {
|
||||
return Err(ImuError::BadFrame);
|
||||
}
|
||||
let Some(layout) = config.layout else {
|
||||
return Err(ImuError::LayoutUnconfirmed);
|
||||
};
|
||||
let roll = layout.roll.read(&frame.data).ok_or(ImuError::Checksum)?;
|
||||
let pitch = layout.pitch.read(&frame.data).ok_or(ImuError::Checksum)?;
|
||||
let yaw = layout.yaw.read(&frame.data).ok_or(ImuError::Checksum)?;
|
||||
Ok(Some(ImuData {
|
||||
roll,
|
||||
pitch,
|
||||
yaw,
|
||||
timestamp_ns,
|
||||
// 本帧只承载姿态角；加速度 / 角速度需要额外的帧（待手册确认）
|
||||
..ImuData::default()
|
||||
}))
|
||||
}
|
||||
|
||||
/// 配置错误：把"不确定"和"确定错"分开。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
|
||||
pub enum Delta10ConfigError {
|
||||
/// 布局字段越过 8 字节载荷。
|
||||
#[error("DETA10 字段偏移 {offset} 超出 8 字节载荷")]
|
||||
FieldOutOfRange {
|
||||
/// 越界的偏移。
|
||||
offset: usize,
|
||||
},
|
||||
/// 布局非有限标度。
|
||||
#[error("DETA10 字段标度必须为有限正值")]
|
||||
NonFiniteScale,
|
||||
}
|
||||
|
||||
/// 校验布局能否落在载荷内（D1 确认后填入常量时用）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 字段越界或标度非法时返回错误。
|
||||
pub fn validate_layout(layout: &Delta10Layout) -> Result<(), Delta10ConfigError> {
|
||||
for field in [layout.roll, layout.pitch, layout.yaw] {
|
||||
if !(field.scale.is_finite() && field.scale > 0.0) {
|
||||
return Err(Delta10ConfigError::NonFiniteScale);
|
||||
}
|
||||
if field.offset + field.width.bytes() > 8 {
|
||||
return Err(Delta10ConfigError::FieldOutOfRange { offset: field.offset });
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
/// 一个**能装进 8 字节**的合成布局：3 × i16 定点（0.001 度/LSB）。
|
||||
///
|
||||
/// 它只用来证明管道可用；真实 DETA10 的偏移/宽度/标度待 D1 确认。
|
||||
fn fitted_layout() -> Delta10Layout {
|
||||
Delta10Layout {
|
||||
roll: FieldLayout::new(0, FieldWidth::I16, 0.001),
|
||||
pitch: FieldLayout::new(2, FieldWidth::I16, 0.001),
|
||||
yaw: FieldLayout::new(4, FieldWidth::I16, 0.001),
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn non_imu_frames_are_ignored_not_errors() {
|
||||
let config = Delta10Config::default().with_can_id(0x100);
|
||||
let motor = CanFrame::new(0x201, [0; 8]);
|
||||
assert_eq!(try_decode(&config, &motor, 0), Ok(None));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn default_config_refuses_to_guess_the_layout() {
|
||||
let config = Delta10Config::default().with_can_id(0x100);
|
||||
let imu_frame = CanFrame::new(0x100, [1, 2, 3, 4, 5, 6, 7, 8]);
|
||||
assert_eq!(try_decode(&config, &imu_frame, 7), Err(ImuError::LayoutUnconfirmed));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn short_imu_frames_are_rejected() {
|
||||
let config = Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout());
|
||||
let short = CanFrame::new(0x100, [0; 8]).with_dlc(4);
|
||||
assert_eq!(try_decode(&config, &short, 0), Err(ImuError::BadFrame));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn configured_layout_decodes_attitude_and_stamps_time() {
|
||||
let config = Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout());
|
||||
let mut data = [0u8; 8];
|
||||
// roll = 1.5°, pitch = -2.25°（原始值为毫度）
|
||||
data[0..2].copy_from_slice(&1500i16.to_le_bytes());
|
||||
data[2..4].copy_from_slice(&(-2250i16).to_le_bytes());
|
||||
let frame = CanFrame::new(0x100, data);
|
||||
let decoded = try_decode(&config, &frame, 42).unwrap().unwrap();
|
||||
assert!((decoded.roll - 1.5).abs() < 1e-6, "{}", decoded.roll);
|
||||
assert!((decoded.pitch + 2.25).abs() < 1e-6, "{}", decoded.pitch);
|
||||
assert_eq!(decoded.timestamp_ns, 42);
|
||||
assert!(decoded.is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn layout_validation_catches_out_of_range_offsets() {
|
||||
// offset 6 + 4 字节 f32 = 10 > 8：越界
|
||||
let bad = Delta10Layout {
|
||||
roll: FieldLayout::new(0, FieldWidth::F32, 1.0),
|
||||
pitch: FieldLayout::new(2, FieldWidth::I16, 1.0),
|
||||
yaw: FieldLayout::new(6, FieldWidth::F32, 1.0),
|
||||
};
|
||||
assert!(matches!(
|
||||
validate_layout(&bad),
|
||||
Err(Delta10ConfigError::FieldOutOfRange { offset: 6 })
|
||||
));
|
||||
let zero_scale = Delta10Layout {
|
||||
roll: FieldLayout::new(0, FieldWidth::I16, 0.0),
|
||||
pitch: FieldLayout::new(2, FieldWidth::I16, 1.0),
|
||||
yaw: FieldLayout::new(4, FieldWidth::I16, 1.0),
|
||||
};
|
||||
assert_eq!(validate_layout(&zero_scale), Err(Delta10ConfigError::NonFiniteScale));
|
||||
assert!(validate_layout(&fitted_layout()).is_ok());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
[package]
|
||||
name = "motor-protocol"
|
||||
description = "DJI C620/C610 and GM6020 wire protocols"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
proptest = { workspace = true }
|
||||
approx = { workspace = true }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "protocol/motor"
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,19 @@
|
||||
[package]
|
||||
name = "dr16-protocol"
|
||||
description = "DJI DR16 D-BUS frame protocol"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
thiserror = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
proptest = { workspace = true }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "protocol/remote"
|
||||
@@ -0,0 +1,380 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! DJI DR16 接收机 D-BUS 帧协议。
|
||||
//!
|
||||
//! 协议帧固定为 18 字节：四个 11 bit 摇杆通道、两个三档开关、鼠标、按键与拨轮。
|
||||
//! 本实现只描述线上的字节布局，不依赖串口、Tokio 或底盘控制类型。
|
||||
//!
|
||||
//! DT7/DR16 手册没有给出 D-BUS 字节布局；字段结构与开关编码参考
|
||||
//! `RoboMaster/RoboRTS-Firmware`，本实现为独立 Rust 重写。
|
||||
|
||||
use std::collections::VecDeque;
|
||||
|
||||
use thiserror::Error;
|
||||
|
||||
/// 一帧 D-BUS 数据的字节数。
|
||||
pub const FRAME_LEN: usize = 18;
|
||||
/// 遥控器摇杆的协议中心值。
|
||||
pub const CHANNEL_CENTER: i16 = 1024;
|
||||
/// 摇杆有效范围：中心值 ±660。
|
||||
pub const CHANNEL_MAX_ABS: i16 = 660;
|
||||
|
||||
/// 三档开关位置。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
|
||||
pub enum SwitchPosition {
|
||||
/// 上档，线值 1。
|
||||
Up,
|
||||
/// 下档，线值 2。
|
||||
Down,
|
||||
/// 中档，线值 3。
|
||||
Mid,
|
||||
}
|
||||
|
||||
impl SwitchPosition {
|
||||
/// 开关的协议线值。
|
||||
#[must_use]
|
||||
pub const fn wire_value(self) -> u8 {
|
||||
match self {
|
||||
Self::Up => 1,
|
||||
Self::Down => 2,
|
||||
Self::Mid => 3,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl TryFrom<u8> for SwitchPosition {
|
||||
type Error = DecodeError;
|
||||
|
||||
fn try_from(value: u8) -> Result<Self, Self::Error> {
|
||||
match value {
|
||||
1 => Ok(Self::Up),
|
||||
2 => Ok(Self::Down),
|
||||
3 => Ok(Self::Mid),
|
||||
_ => Err(DecodeError::InvalidSwitch(value)),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// DR16 扩展输入：鼠标、按键与拨轮。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
|
||||
pub struct ExtendedInput {
|
||||
/// 鼠标 x 位移。
|
||||
pub mouse_x: i16,
|
||||
/// 鼠标 y 位移。
|
||||
pub mouse_y: i16,
|
||||
/// 鼠标 z 位移。
|
||||
pub mouse_z: i16,
|
||||
/// 鼠标左键。
|
||||
pub mouse_left: bool,
|
||||
/// 鼠标右键。
|
||||
pub mouse_right: bool,
|
||||
/// 键盘按键位图。
|
||||
pub key_code: u16,
|
||||
/// 拨轮中心值 ±1024 后的值。
|
||||
pub wheel: i16,
|
||||
}
|
||||
|
||||
/// 一帧完整 DR16 输入。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct Dr16Sample {
|
||||
/// 摇杆通道 A/E/T/R，均已减去 1024，范围 -660..=660。
|
||||
pub channels: [i16; 4],
|
||||
/// 左侧三档开关 S1。
|
||||
pub s1: SwitchPosition,
|
||||
/// 右侧三档开关 S2。
|
||||
pub s2: SwitchPosition,
|
||||
/// 鼠标、按键和拨轮。
|
||||
pub extended: ExtendedInput,
|
||||
}
|
||||
|
||||
impl Dr16Sample {
|
||||
/// 右摇杆左右通道 A/J1。
|
||||
#[must_use]
|
||||
pub const fn channel_a(self) -> i16 {
|
||||
self.channels[0]
|
||||
}
|
||||
|
||||
/// 右摇杆上下通道 E/J2。
|
||||
#[must_use]
|
||||
pub const fn channel_e(self) -> i16 {
|
||||
self.channels[1]
|
||||
}
|
||||
|
||||
/// 左摇杆上下通道 T/J3。
|
||||
#[must_use]
|
||||
pub const fn channel_t(self) -> i16 {
|
||||
self.channels[2]
|
||||
}
|
||||
|
||||
/// 左摇杆左右通道 R/J4。
|
||||
#[must_use]
|
||||
pub const fn channel_r(self) -> i16 {
|
||||
self.channels[3]
|
||||
}
|
||||
}
|
||||
|
||||
/// D-BUS 解码错误。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
|
||||
pub enum DecodeError {
|
||||
/// 输入不是 18 字节完整帧。
|
||||
#[error("invalid D-BUS frame length: expected {expected}, got {actual}")]
|
||||
InvalidLength {
|
||||
/// 协议要求的长度。
|
||||
expected: usize,
|
||||
/// 实际长度。
|
||||
actual: usize,
|
||||
},
|
||||
/// 摇杆超出遥控器有效范围。
|
||||
#[error("invalid D-BUS channel {channel}: {value}")]
|
||||
InvalidChannel {
|
||||
/// 通道下标 0..4。
|
||||
channel: usize,
|
||||
/// 中心化后的值。
|
||||
value: i16,
|
||||
},
|
||||
/// 开关线值不是 1/2/3。
|
||||
#[error("invalid D-BUS switch value: {0}")]
|
||||
InvalidSwitch(u8),
|
||||
/// 拨轮超出 ±1024。
|
||||
#[error("invalid D-BUS wheel value: {0}")]
|
||||
InvalidWheel(i16),
|
||||
/// 鼠标按键字节不是 0/1。
|
||||
#[error("invalid D-BUS mouse button byte: {0}")]
|
||||
InvalidMouseButton(u8),
|
||||
}
|
||||
|
||||
/// 解码一帧完整 D-BUS 数据。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 任一摇杆超出 ±660 或开关线值非法时返回 [`DecodeError`]。
|
||||
pub fn decode_frame(data: &[u8]) -> Result<Dr16Sample, DecodeError> {
|
||||
if data.len() != FRAME_LEN {
|
||||
return Err(DecodeError::InvalidLength { expected: FRAME_LEN, actual: data.len() });
|
||||
}
|
||||
let mut packed = [0u8; FRAME_LEN];
|
||||
packed.copy_from_slice(data);
|
||||
|
||||
let raw_channels = [
|
||||
(u16::from(packed[0]) | (u16::from(packed[1]) << 8)) & 0x07FF,
|
||||
(u16::from(packed[1]) >> 3 | (u16::from(packed[2]) << 5)) & 0x07FF,
|
||||
(u16::from(packed[2]) >> 6 | (u16::from(packed[3]) << 2) | (u16::from(packed[4]) << 10))
|
||||
& 0x07FF,
|
||||
(u16::from(packed[4]) >> 1 | (u16::from(packed[5]) << 7)) & 0x07FF,
|
||||
];
|
||||
let mut channels = [0i16; 4];
|
||||
for (index, raw) in raw_channels.into_iter().enumerate() {
|
||||
let value = raw as i16 - CHANNEL_CENTER;
|
||||
if !(-CHANNEL_MAX_ABS..=CHANNEL_MAX_ABS).contains(&value) {
|
||||
return Err(DecodeError::InvalidChannel { channel: index, value });
|
||||
}
|
||||
channels[index] = value;
|
||||
}
|
||||
|
||||
let switch_bits = packed[5] >> 4;
|
||||
let s1 = SwitchPosition::try_from((switch_bits & 0x0C) >> 2)?;
|
||||
let s2 = SwitchPosition::try_from(switch_bits & 0x03)?;
|
||||
|
||||
let wheel = u16::from_le_bytes([packed[16], packed[17]]) as i16 - CHANNEL_CENTER;
|
||||
if !(-1024..=1024).contains(&wheel) {
|
||||
return Err(DecodeError::InvalidWheel(wheel));
|
||||
}
|
||||
if packed[12] > 1 {
|
||||
return Err(DecodeError::InvalidMouseButton(packed[12]));
|
||||
}
|
||||
if packed[13] > 1 {
|
||||
return Err(DecodeError::InvalidMouseButton(packed[13]));
|
||||
}
|
||||
|
||||
Ok(Dr16Sample {
|
||||
channels,
|
||||
s1,
|
||||
s2,
|
||||
extended: ExtendedInput {
|
||||
mouse_x: i16::from_le_bytes([packed[6], packed[7]]),
|
||||
mouse_y: i16::from_le_bytes([packed[8], packed[9]]),
|
||||
mouse_z: i16::from_le_bytes([packed[10], packed[11]]),
|
||||
mouse_left: packed[12] == 1,
|
||||
mouse_right: packed[13] == 1,
|
||||
key_code: u16::from_le_bytes([packed[14], packed[15]]),
|
||||
wheel,
|
||||
},
|
||||
})
|
||||
}
|
||||
|
||||
/// 增量帧解码器：接受任意大小的串口读块，并在噪声后重新同步。
|
||||
///
|
||||
/// D-BUS 帧没有同步头或校验和，因此同步依靠固定长度和摇杆/开关的有效范围。
|
||||
#[derive(Debug, Default)]
|
||||
pub struct FrameDecoder {
|
||||
bytes: VecDeque<u8>,
|
||||
discarded_bytes: usize,
|
||||
}
|
||||
|
||||
impl FrameDecoder {
|
||||
/// 创建空解码器。
|
||||
#[must_use]
|
||||
pub const fn new() -> Self {
|
||||
Self { bytes: VecDeque::new(), discarded_bytes: 0 }
|
||||
}
|
||||
|
||||
/// 追加串口字节并返回其中可识别的完整帧。
|
||||
#[must_use]
|
||||
pub fn push(&mut self, input: &[u8]) -> Vec<Dr16Sample> {
|
||||
self.bytes.extend(input.iter().copied());
|
||||
let mut samples = Vec::new();
|
||||
|
||||
while self.bytes.len() >= FRAME_LEN {
|
||||
let mut candidate: Option<(usize, Dr16Sample, usize)> = None;
|
||||
let windows = self.bytes.len() - FRAME_LEN + 1;
|
||||
for offset in 0..windows {
|
||||
let mut frame = [0u8; FRAME_LEN];
|
||||
for (index, byte) in self.bytes.iter().skip(offset).take(FRAME_LEN).enumerate() {
|
||||
frame[index] = *byte;
|
||||
}
|
||||
if let Ok(sample) = decode_frame(&frame) {
|
||||
// D-BUS 没有同步头。多个窗口都能通过范围校验时，优先选择
|
||||
// 与 18 字节帧边界一致的候选，避免被噪声向前错一位。
|
||||
let residue = (self.bytes.len() - (offset + FRAME_LEN)) % FRAME_LEN;
|
||||
if candidate.is_none_or(|(_, _, best)| residue < best) {
|
||||
candidate = Some((offset, sample, residue));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if let Some((offset, sample, _)) = candidate {
|
||||
self.bytes.drain(0..offset + FRAME_LEN);
|
||||
self.discarded_bytes += offset;
|
||||
samples.push(sample);
|
||||
} else {
|
||||
let discard = self.bytes.len() - (FRAME_LEN - 1);
|
||||
self.bytes.drain(0..discard);
|
||||
self.discarded_bytes += discard;
|
||||
}
|
||||
}
|
||||
samples
|
||||
}
|
||||
|
||||
/// 因噪声或不完整候选帧而丢弃的累计字节数。
|
||||
#[must_use]
|
||||
pub const fn discarded_bytes(&self) -> usize {
|
||||
self.discarded_bytes
|
||||
}
|
||||
|
||||
/// 清空尚未组成帧的字节。
|
||||
pub fn clear(&mut self) {
|
||||
self.bytes.clear();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
fn encode(channels: [i16; 4], s1: u8, s2: u8) -> [u8; FRAME_LEN] {
|
||||
let raw: Vec<u16> = channels.iter().map(|value| (value + CHANNEL_CENTER) as u16).collect();
|
||||
let mut frame = [0u8; FRAME_LEN];
|
||||
frame[0] = (raw[0] & 0xFF) as u8;
|
||||
frame[1] = ((raw[0] >> 8) as u8) | ((raw[1] & 0x1F) as u8) << 3;
|
||||
frame[2] = ((raw[1] >> 5) as u8) | ((raw[2] & 0x03) as u8) << 6;
|
||||
frame[3] = (raw[2] >> 2) as u8;
|
||||
frame[4] = ((raw[2] >> 10) as u8) | ((raw[3] & 0x7F) as u8) << 1;
|
||||
frame[5] = ((raw[3] >> 7) as u8) | (s2 & 0x03) << 4 | (s1 & 0x03) << 6;
|
||||
frame
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decodes_channels_switches_mouse_keys_and_wheel() {
|
||||
let mut frame = encode([-660, -5, 321, 660], 1, 3);
|
||||
frame[5] = (frame[5] & 0x0F) | (1 << 6) | (3 << 4);
|
||||
frame[6..8].copy_from_slice(&(-123i16).to_le_bytes());
|
||||
frame[8..10].copy_from_slice(&456i16.to_le_bytes());
|
||||
frame[10..12].copy_from_slice(&(-7i16).to_le_bytes());
|
||||
frame[12] = 1;
|
||||
frame[13] = 0;
|
||||
frame[14..16].copy_from_slice(&0xA55Au16.to_le_bytes());
|
||||
frame[16..18].copy_from_slice(&(2048u16).to_le_bytes());
|
||||
|
||||
let sample = decode_frame(&frame).unwrap();
|
||||
assert_eq!(sample.channels, [-660, -5, 321, 660]);
|
||||
assert_eq!(sample.s1, SwitchPosition::Up);
|
||||
assert_eq!(sample.s2, SwitchPosition::Mid);
|
||||
assert_eq!(sample.channel_a(), -660);
|
||||
assert_eq!(sample.channel_t(), 321);
|
||||
assert_eq!(
|
||||
sample.extended,
|
||||
ExtendedInput {
|
||||
mouse_x: -123,
|
||||
mouse_y: 456,
|
||||
mouse_z: -7,
|
||||
mouse_left: true,
|
||||
mouse_right: false,
|
||||
key_code: 0xA55A,
|
||||
wheel: 1024,
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn switch_wire_values_are_explicit() {
|
||||
assert_eq!(SwitchPosition::Up.wire_value(), 1);
|
||||
assert_eq!(SwitchPosition::Down.wire_value(), 2);
|
||||
assert_eq!(SwitchPosition::Mid.wire_value(), 3);
|
||||
assert_eq!(SwitchPosition::try_from(0), Err(DecodeError::InvalidSwitch(0)));
|
||||
assert_eq!(SwitchPosition::try_from(4), Err(DecodeError::InvalidSwitch(4)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_short_frame_and_out_of_range_values() {
|
||||
assert_eq!(
|
||||
decode_frame(&[0; FRAME_LEN - 1]),
|
||||
Err(DecodeError::InvalidLength { expected: FRAME_LEN, actual: FRAME_LEN - 1 })
|
||||
);
|
||||
|
||||
let frame = encode([661, 0, 0, 0], 1, 1);
|
||||
assert!(matches!(
|
||||
decode_frame(&frame),
|
||||
Err(DecodeError::InvalidChannel { channel: 0, .. })
|
||||
));
|
||||
|
||||
let frame = encode([0, 0, 0, 0], 0, 1);
|
||||
assert_eq!(decode_frame(&frame), Err(DecodeError::InvalidSwitch(0)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decoder_accepts_fragmented_frames() {
|
||||
let frame = encode([100, -200, 300, -400], 1, 2);
|
||||
let mut decoder = FrameDecoder::new();
|
||||
assert!(decoder.push(&frame[..7]).is_empty());
|
||||
assert!(decoder.push(&frame[7..13]).is_empty());
|
||||
let samples = decoder.push(&frame[13..]);
|
||||
assert_eq!(samples.len(), 1);
|
||||
assert_eq!(samples[0].channels, [100, -200, 300, -400]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decoder_resynchronizes_after_noise() {
|
||||
let frame = encode([660, -660, 659, -659], 1, 3);
|
||||
let mut input = vec![0xFF; 37];
|
||||
input.extend_from_slice(&frame);
|
||||
let mut decoder = FrameDecoder::new();
|
||||
let samples = decoder.push(&input);
|
||||
assert_eq!(samples.len(), 1, "噪声后应找到下一帧");
|
||||
assert_eq!(samples[0].channels, [660, -660, 659, -659]);
|
||||
assert!(decoder.discarded_bytes() >= 37);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decoder_returns_multiple_frames_from_one_read() {
|
||||
let first = encode([1, 2, 3, 4], 1, 1);
|
||||
let second = encode([-1, -2, -3, -4], 2, 3);
|
||||
let mut input = first.to_vec();
|
||||
input.extend_from_slice(&second);
|
||||
|
||||
let mut decoder = FrameDecoder::new();
|
||||
let samples = decoder.push(&input);
|
||||
assert_eq!(samples.len(), 2);
|
||||
assert_eq!(samples[0].channels, [1, 2, 3, 4]);
|
||||
assert_eq!(samples[1].channels, [-1, -2, -3, -4]);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user