381 lines
12 KiB
Rust
381 lines
12 KiB
Rust
#![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]);
|
|
}
|
|
}
|