refactor: enforce layered crates and Rust audit gates
This commit is contained in:
Horldsence
@@ -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