refactor: 重构架构并整理仓库
This commit is contained in:
Horldsence
@@ -0,0 +1,18 @@
|
||||
[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 }
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user