refactor: enforce layered crates and Rust audit gates
This commit is contained in:
@@ -0,0 +1,22 @@
|
||||
[package]
|
||||
name = "can"
|
||||
description = "CAN frame, bus interface, channel link, and test bus"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[features]
|
||||
mock = []
|
||||
|
||||
[dependencies]
|
||||
tokio = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "contract/can"
|
||||
@@ -0,0 +1,463 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod frame {
|
||||
//! CAN 抽象:经典数据帧与**总线 trait**。
|
||||
//!
|
||||
//! [`CanFrame`] 是全系统唯一的帧表示(协议层、驱动层、仿真层、设备层共享),
|
||||
//! 它**不含任何协议知识**(没有 DJI 帧 ID 常量)。字段与 ARCHITECTURE.md
|
||||
//! §5.1 一致:`id` / `data` / `dlc` / `is_extended`。
|
||||
//!
|
||||
//! [`CanBus`] 是 ARCHITECTURE.md §5.1 定义的硬件访问边界;`driver crates` 为
|
||||
//! 真实总线实现它,`can::mock` 为假电调实现它,
|
||||
//! [`CanLink`](crate::link::CanLink) 是它的通道式实现。
|
||||
|
||||
/// 经典 CAN 数据帧数据域长度(字节)。
|
||||
pub const CAN_DATA_LEN: usize = 8;
|
||||
|
||||
/// 经典 CAN 数据帧。
|
||||
///
|
||||
/// 当前所有设备(C620/C610、GM6020、DETA10)都用「11 位标准 ID + 8 字节」,
|
||||
/// 但字段保留完整形态:将来接扩展帧或短帧时不必改动所有构造点。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
|
||||
pub struct CanFrame {
|
||||
/// 标准 11 位 / 扩展 29 位 ID。
|
||||
pub id: u32,
|
||||
/// 数据域(未使用的字节为 0)。
|
||||
pub data: [u8; CAN_DATA_LEN],
|
||||
/// 数据长度码(0..=8)。
|
||||
pub dlc: u8,
|
||||
/// 是否扩展帧(29 位 ID)。
|
||||
pub is_extended: bool,
|
||||
}
|
||||
|
||||
impl CanFrame {
|
||||
/// 构造一帧标准帧:8 字节数据、`dlc = 8`。
|
||||
///
|
||||
/// 这是仓库内**唯一**的常规构造方式;`dlc` / 扩展帧只在特殊场景用
|
||||
/// [`CanFrame::with_dlc`] / [`CanFrame::extended`] 调整。
|
||||
#[must_use]
|
||||
pub const fn new(id: u32, data: [u8; CAN_DATA_LEN]) -> Self {
|
||||
Self { id, data, dlc: CAN_DATA_LEN as u8, is_extended: false }
|
||||
}
|
||||
|
||||
/// 换一个数据长度码(链式)。
|
||||
#[must_use]
|
||||
pub const fn with_dlc(mut self, dlc: u8) -> Self {
|
||||
self.dlc = if dlc > CAN_DATA_LEN as u8 { CAN_DATA_LEN as u8 } else { dlc };
|
||||
self
|
||||
}
|
||||
|
||||
/// 标记为扩展帧(链式)。
|
||||
#[must_use]
|
||||
pub const fn extended(mut self) -> Self {
|
||||
self.is_extended = true;
|
||||
self
|
||||
}
|
||||
|
||||
/// 长度码是否覆盖完整数据域。
|
||||
#[must_use]
|
||||
pub const fn is_complete(&self) -> bool {
|
||||
self.dlc == CAN_DATA_LEN as u8
|
||||
}
|
||||
|
||||
/// ID 是否在 11 位标准帧范围内。
|
||||
#[must_use]
|
||||
pub const fn is_standard_id(&self) -> bool {
|
||||
self.id <= 0x7FF
|
||||
}
|
||||
}
|
||||
|
||||
/// CAN 总线访问错误。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum CanError {
|
||||
/// 链路已关闭(对端释放)。
|
||||
Closed,
|
||||
/// ID 与帧类型不匹配(如标准帧却给了 29 位 ID)。
|
||||
BadId(u32),
|
||||
/// 数据长度码非法(> 8)。
|
||||
BadDlc(u8),
|
||||
/// 底层收发失败(驱动自行补充上下文)。
|
||||
Io,
|
||||
}
|
||||
|
||||
impl core::fmt::Display for CanError {
|
||||
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
||||
match self {
|
||||
Self::Closed => write!(f, "CAN 链路已关闭"),
|
||||
Self::BadId(id) => write!(f, "CAN ID {id:#x} 与帧类型不匹配"),
|
||||
Self::BadDlc(dlc) => write!(f, "CAN 数据长度码 {dlc} 非法(应 ≤ 8)"),
|
||||
Self::Io => write!(f, "CAN 收发失败"),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl core::error::Error for CanError {}
|
||||
|
||||
/// 硬件无关的 CAN 总线边界(ARCHITECTURE.md §5.1)。
|
||||
///
|
||||
/// 只描述"发一帧 / 收一帧 / 非阻塞收一帧",**不规定**底层是 SocketCAN、
|
||||
/// 串口 slcan、内存通道还是假电调。`async fn in trait` 是本仓库对
|
||||
/// ARCHITECTURE.md §3.2 的直接采纳(Rust 1.75+ 稳定)。
|
||||
///
|
||||
/// # 关于非阻塞
|
||||
///
|
||||
/// 控制循环严格要求"读反馈不阻塞"(§6.3 第 2–5 步必须非阻塞)。
|
||||
/// 通道式实现天然支持 [`CanBus::try_recv`];对不支持的系统调用,
|
||||
/// 默认实现返回 `Ok(None)` 而不是报错,让上层按"本轮没新帧"处理。
|
||||
pub trait CanBus {
|
||||
/// 底层错误类型。
|
||||
type Error: core::error::Error + Send + Sync + 'static;
|
||||
|
||||
/// 发送一帧。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 链路关闭、帧类型非法或底层写失败时返回错误。
|
||||
fn send(
|
||||
&mut self,
|
||||
frame: &CanFrame,
|
||||
) -> impl core::future::Future<Output = Result<(), Self::Error>>;
|
||||
|
||||
/// 接收一帧(阻塞直到有帧或链路关闭)。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 链路关闭或底层读失败时返回错误。
|
||||
fn recv(&mut self) -> impl core::future::Future<Output = Result<CanFrame, Self::Error>>;
|
||||
|
||||
/// 非阻塞接收:本轮没有新帧时返回 `Ok(None)`。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 仅当链路**确定**已关闭 / 损坏时返回错误;"暂时没数据"不是错误。
|
||||
fn try_recv(&mut self) -> Result<Option<CanFrame>, Self::Error> {
|
||||
Ok(None)
|
||||
}
|
||||
}
|
||||
|
||||
/// 帧是否符合"标准 ID + 完整 8 字节"的常规约定。
|
||||
#[must_use]
|
||||
pub const fn is_regular_standard_frame(frame: &CanFrame) -> bool {
|
||||
frame.is_standard_id() && !frame.is_extended && frame.is_complete()
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn new_defaults_to_a_complete_standard_frame() {
|
||||
let frame = CanFrame::new(0x200, [1, 2, 3, 4, 5, 6, 7, 8]);
|
||||
assert_eq!(frame.id, 0x200);
|
||||
assert_eq!(frame.data, [1, 2, 3, 4, 5, 6, 7, 8]);
|
||||
assert_eq!(frame.dlc, 8);
|
||||
assert!(!frame.is_extended);
|
||||
assert!(frame.is_complete());
|
||||
assert!(is_regular_standard_frame(&frame));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn builders_are_chainable_and_clamp_dlc() {
|
||||
let short = CanFrame::new(0x201, [0; 8]).with_dlc(4);
|
||||
assert_eq!(short.dlc, 4);
|
||||
assert!(!short.is_complete());
|
||||
assert!(!is_regular_standard_frame(&short));
|
||||
|
||||
let over = CanFrame::new(0x201, [0; 8]).with_dlc(9);
|
||||
assert_eq!(over.dlc, 8, "dlc 必须被夹到数据域长度");
|
||||
|
||||
let extended = CanFrame::new(0x1_2345, [0; 8]).extended();
|
||||
assert!(extended.is_extended);
|
||||
assert!(!is_regular_standard_frame(&extended));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn standard_id_boundary() {
|
||||
assert!(CanFrame::new(0x7FF, [0; 8]).is_standard_id());
|
||||
assert!(!CanFrame::new(0x800, [0; 8]).is_standard_id());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn errors_render_in_chinese() {
|
||||
let text = CanError::BadId(0x1234).to_string();
|
||||
assert!(text.contains("0x1234"), "{text}");
|
||||
assert!(CanError::BadDlc(9).to_string().contains('9'));
|
||||
assert!(!CanError::Closed.to_string().is_empty());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use frame::*;
|
||||
|
||||
pub mod link {
|
||||
//! 通道式总线端点:[`CanLink`] 与它对 [`CanBus`] 的实现。
|
||||
//!
|
||||
//! 一对 tokio 通道构成一条链路:本端发出的帧由对端收到,反之亦然。
|
||||
//! 这是本仓库"**帧传输的唯一接缝**"——协议层与设备层只面向通道工作,
|
||||
//! 因此同一套协议 / 控制 / 整定代码可以对接真实总线(`driver crates` 的
|
||||
//! SocketCAN / slcan 桥)或纯内存假电调(`can::mock`),
|
||||
//! 无需任何条件编译。
|
||||
|
||||
use tokio::sync::mpsc;
|
||||
|
||||
use crate::frame::{CanBus, CanError, CanFrame};
|
||||
|
||||
/// CAN 链路的**一端**。
|
||||
///
|
||||
/// - `outgoing_tx`:本端发往总线的帧;
|
||||
/// - `incoming_rx`:本端从总线收到的帧。
|
||||
#[derive(Debug)]
|
||||
pub struct CanLink {
|
||||
/// 发往总线。
|
||||
pub outgoing_tx: mpsc::Sender<CanFrame>,
|
||||
/// 来自总线。
|
||||
pub incoming_rx: mpsc::Receiver<CanFrame>,
|
||||
}
|
||||
|
||||
impl CanLink {
|
||||
/// 由已有通道组装端点。
|
||||
#[must_use]
|
||||
pub const fn new(
|
||||
outgoing_tx: mpsc::Sender<CanFrame>,
|
||||
incoming_rx: mpsc::Receiver<CanFrame>,
|
||||
) -> Self {
|
||||
Self { outgoing_tx, incoming_rx }
|
||||
}
|
||||
|
||||
/// 创建一对互联端点:`a` 发出的帧会被 `b` 收到,反之亦然。
|
||||
///
|
||||
/// `bound` 为每方向缓冲帧数。命令方向必须**浅**(1kHz 生产、总线消费
|
||||
/// 不过来时靠 `send().await` 回压,见 `slcan` 的说明);
|
||||
/// 反馈方向是采样流,可以深一些。
|
||||
#[must_use]
|
||||
pub fn pair(bound: usize) -> (Self, Self) {
|
||||
let (tx_a_to_b, rx_at_b) = mpsc::channel(bound);
|
||||
let (tx_b_to_a, rx_at_a) = mpsc::channel(bound);
|
||||
(Self::new(tx_a_to_b, rx_at_a), Self::new(tx_b_to_a, rx_at_b))
|
||||
}
|
||||
}
|
||||
|
||||
impl CanBus for CanLink {
|
||||
type Error = CanError;
|
||||
|
||||
async fn send(&mut self, frame: &CanFrame) -> Result<(), Self::Error> {
|
||||
if frame.dlc > 8 {
|
||||
return Err(CanError::BadDlc(frame.dlc));
|
||||
}
|
||||
self.outgoing_tx.send(*frame).await.map_err(|_| CanError::Closed)
|
||||
}
|
||||
|
||||
async fn recv(&mut self) -> Result<CanFrame, Self::Error> {
|
||||
self.incoming_rx.recv().await.ok_or(CanError::Closed)
|
||||
}
|
||||
|
||||
fn try_recv(&mut self) -> Result<Option<CanFrame>, Self::Error> {
|
||||
match self.incoming_rx.try_recv() {
|
||||
Ok(frame) => Ok(Some(frame)),
|
||||
Err(mpsc::error::TryRecvError::Empty) => Ok(None),
|
||||
Err(mpsc::error::TryRecvError::Disconnected) => Err(CanError::Closed),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[tokio::test]
|
||||
async fn pair_loops_both_directions() {
|
||||
let (mut a, mut b) = CanLink::pair(8);
|
||||
a.outgoing_tx.send(CanFrame::new(0x200, [1, 0, 0, 0, 0, 0, 0, 0])).await.unwrap();
|
||||
b.outgoing_tx.send(CanFrame::new(0x201, [2, 0, 0, 0, 0, 0, 0, 0])).await.unwrap();
|
||||
assert_eq!(b.incoming_rx.recv().await.unwrap().id, 0x200);
|
||||
assert_eq!(a.incoming_rx.recv().await.unwrap().id, 0x201);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn can_bus_impl_sends_receives_and_polls() {
|
||||
let (mut a, mut b) = CanLink::pair(8);
|
||||
assert_eq!(a.try_recv().unwrap(), None, "空通道不是错误");
|
||||
a.send(&CanFrame::new(0x1FF, [7; 8])).await.unwrap();
|
||||
assert_eq!(b.try_recv().unwrap().map(|f| f.id), Some(0x1FF));
|
||||
assert_eq!(b.try_recv().unwrap(), None, "取走之后就没有了");
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn recv_reports_closed_link() {
|
||||
let (mut a, b) = CanLink::pair(8);
|
||||
drop(b);
|
||||
assert_eq!(a.recv().await.unwrap_err(), CanError::Closed);
|
||||
assert_eq!(a.try_recv().unwrap_err(), CanError::Closed);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn send_rejects_illegal_dlc() {
|
||||
let (mut a, _b) = CanLink::pair(8);
|
||||
let bad = CanFrame::new(0x200, [0; 8]).with_dlc(8).with_dlc(8);
|
||||
assert!(a.send(&bad).await.is_ok());
|
||||
// `with_dlc` 本身会夹住越界值,因此非法 dlc 只能由手工构造的帧产生。
|
||||
let mut manual = CanFrame::new(0x200, [0; 8]);
|
||||
manual.dlc = 9;
|
||||
assert_eq!(a.send(&manual).await.unwrap_err(), CanError::BadDlc(9));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use link::*;
|
||||
|
||||
pub mod mock {
|
||||
//! 驱动级测试替身:[`MockBus`](`mock` feature)。
|
||||
//!
|
||||
//! **这里没有电机模型**——那是 `sim-harness` 的事(`sim-harness::rig` 的异步假电调带
|
||||
//! 摩擦 / 延迟物理模型)。本模块只做驱动层该做的事:
|
||||
//!
|
||||
//! - 记录所有**发出去**的帧(断言"上层到底写了什么");
|
||||
//! - 按需**注入**要收到的帧(模拟总线上的反馈);
|
||||
//! - 可以设置成"链路已断开",用于测试离线 / 安全路径。
|
||||
//!
|
||||
//! 有了它,协议层与设备层的测试不必启动 tokio 任务,也不必构造真实总线。
|
||||
|
||||
use std::collections::VecDeque;
|
||||
|
||||
use crate::frame::{CanBus, CanError, CanFrame};
|
||||
|
||||
/// 记录发送、可注入接收的假总线。
|
||||
#[derive(Debug, Default)]
|
||||
pub struct MockBus {
|
||||
/// 上层通过 [`CanBus::send`] 发出的帧(按时间顺序)。
|
||||
sent: Vec<CanFrame>,
|
||||
/// 等待被上层收到的帧。
|
||||
pending: VecDeque<CanFrame>,
|
||||
/// 置位后所有操作都返回 [`CanError::Closed`]。
|
||||
closed: bool,
|
||||
}
|
||||
|
||||
impl MockBus {
|
||||
/// 空总线。
|
||||
#[must_use]
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// 注入一帧"来自总线"的数据。
|
||||
pub fn inject(&mut self, frame: CanFrame) {
|
||||
self.pending.push_back(frame);
|
||||
}
|
||||
|
||||
/// 模拟链路断开。
|
||||
pub fn close(&mut self) {
|
||||
self.closed = true;
|
||||
}
|
||||
|
||||
/// 已发出的帧。
|
||||
#[must_use]
|
||||
pub fn sent(&self) -> &[CanFrame] {
|
||||
&self.sent
|
||||
}
|
||||
|
||||
/// 最近发出的一帧。
|
||||
#[must_use]
|
||||
pub fn last_sent(&self) -> Option<&CanFrame> {
|
||||
self.sent.last()
|
||||
}
|
||||
|
||||
/// 清空发送记录。
|
||||
pub fn clear_sent(&mut self) {
|
||||
self.sent.clear();
|
||||
}
|
||||
|
||||
/// 还剩多少帧没被取走。
|
||||
#[must_use]
|
||||
pub fn pending_len(&self) -> usize {
|
||||
self.pending.len()
|
||||
}
|
||||
}
|
||||
|
||||
impl CanBus for MockBus {
|
||||
type Error = CanError;
|
||||
|
||||
// 这里用 `std::future::ready` 而不是 `async fn`:内存总线没有真正的等待,
|
||||
// 返回一个已完成的 future 更诚实(也避免"async 函数里没有 await"的告警)。
|
||||
fn send(
|
||||
&mut self,
|
||||
frame: &CanFrame,
|
||||
) -> impl std::future::Future<Output = Result<(), Self::Error>> {
|
||||
let result = if self.closed {
|
||||
Err(CanError::Closed)
|
||||
} else if frame.dlc > 8 {
|
||||
Err(CanError::BadDlc(frame.dlc))
|
||||
} else if !frame.is_standard_id() && !frame.is_extended {
|
||||
Err(CanError::BadId(frame.id))
|
||||
} else {
|
||||
self.sent.push(*frame);
|
||||
Ok(())
|
||||
};
|
||||
std::future::ready(result)
|
||||
}
|
||||
|
||||
fn recv(&mut self) -> impl std::future::Future<Output = Result<CanFrame, Self::Error>> {
|
||||
let result = if self.closed {
|
||||
Err(CanError::Closed)
|
||||
} else {
|
||||
self.pending.pop_front().ok_or(CanError::Closed)
|
||||
};
|
||||
std::future::ready(result)
|
||||
}
|
||||
|
||||
fn try_recv(&mut self) -> Result<Option<CanFrame>, Self::Error> {
|
||||
if self.closed {
|
||||
return Err(CanError::Closed);
|
||||
}
|
||||
Ok(self.pending.pop_front())
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[tokio::test]
|
||||
async fn records_sent_frames_in_order() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.send(&CanFrame::new(0x200, [1; 8])).await.unwrap();
|
||||
bus.send(&CanFrame::new(0x200, [2; 8])).await.unwrap();
|
||||
assert_eq!(bus.sent().len(), 2);
|
||||
assert_eq!(bus.last_sent().unwrap().data[0], 2);
|
||||
bus.clear_sent();
|
||||
assert!(bus.sent().is_empty());
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn injected_frames_come_back_once() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.inject(CanFrame::new(0x201, [9; 8]));
|
||||
assert_eq!(bus.pending_len(), 1);
|
||||
let frame = bus.recv().await.unwrap();
|
||||
assert_eq!(frame.id, 0x201);
|
||||
assert_eq!(bus.try_recv().unwrap(), None, "取走之后就没有了");
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn closed_bus_reports_errors_on_every_path() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.close();
|
||||
assert_eq!(
|
||||
bus.send(&CanFrame::new(0x200, [0; 8])).await.unwrap_err(),
|
||||
CanError::Closed
|
||||
);
|
||||
assert_eq!(bus.recv().await.unwrap_err(), CanError::Closed);
|
||||
assert_eq!(bus.try_recv().unwrap_err(), CanError::Closed);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn illegal_dlc_is_rejected_before_recording() {
|
||||
let mut bus = MockBus::new();
|
||||
let mut bad = CanFrame::new(0x200, [0; 8]);
|
||||
bad.dlc = 9;
|
||||
assert_eq!(bus.send(&bad).await.unwrap_err(), CanError::BadDlc(9));
|
||||
assert!(bus.sent().is_empty(), "非法帧不得进入记录");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use mock::*;
|
||||
@@ -0,0 +1,16 @@
|
||||
[package]
|
||||
name = "imu"
|
||||
description = "IMU data contract"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "contract/imu"
|
||||
@@ -0,0 +1,131 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! IMU 抽象:姿态数据与读取接口(ARCHITECTURE.md §5.1)。
|
||||
//!
|
||||
//! `driver crates::delta10` 为 DETA10 实现 [`Imu`];测试可用
|
||||
//! `motor::testing` 风格的替身。
|
||||
//!
|
||||
//! # 单位与坐标系
|
||||
//!
|
||||
//! 与 ARCHITECTURE.md §5.1 的 [`ImuData`] 一致:
|
||||
//!
|
||||
//! | 字段 | 单位 | 坐标系 |
|
||||
//! | --- | --- | --- |
|
||||
//! | `accel` | m/s² | 机体系(x 前、y 左、z 上) |
|
||||
//! | `gyro` | rad/s | 机体系 |
|
||||
//! | `mag` | µT | 机体系 |
|
||||
//! | `roll` / `pitch` / `yaw` | 度 | 机体 → 世界(yaw 逆时针为正) |
|
||||
//!
|
||||
//! 时间戳为单调时钟纳秒(见 [`crate::time`]),用于和电机反馈对齐。
|
||||
|
||||
/// 一帧 IMU 数据。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct ImuData {
|
||||
/// 加速度(m/s²,机体系 x/y/z)。
|
||||
pub accel: [f32; 3],
|
||||
/// 角速度(rad/s,机体系 x/y/z)。
|
||||
pub gyro: [f32; 3],
|
||||
/// 磁力计(µT,机体系 x/y/z);无磁力计时为 `[0.0; 3]`。
|
||||
pub mag: [f32; 3],
|
||||
/// 横滚角(度)。
|
||||
pub roll: f32,
|
||||
/// 俯仰角(度)。
|
||||
pub pitch: f32,
|
||||
/// 偏航角(度)。
|
||||
pub yaw: f32,
|
||||
/// 单调时钟时间戳(纳秒)。
|
||||
pub timestamp_ns: u64,
|
||||
}
|
||||
|
||||
impl ImuData {
|
||||
/// 所有字段是否有限(NaN / inf 不得进入控制律)。
|
||||
#[must_use]
|
||||
pub fn is_finite(&self) -> bool {
|
||||
self.accel.iter().chain(&self.gyro).chain(&self.mag).all(|v| v.is_finite())
|
||||
&& self.roll.is_finite()
|
||||
&& self.pitch.is_finite()
|
||||
&& self.yaw.is_finite()
|
||||
}
|
||||
}
|
||||
|
||||
/// IMU 读取错误。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum ImuError {
|
||||
/// 链路关闭。
|
||||
Closed,
|
||||
/// 帧 ID / 长度不符(多半是总线接错或协议模式不对)。
|
||||
BadFrame,
|
||||
/// **帧载荷布局尚未确认**(ARCHITECTURE.md 决策点 D1)。
|
||||
///
|
||||
/// 见 `delta10-protocol`:DETA10 手册链接已失效,载荷偏移在
|
||||
/// 拿到数据手册前不得臆造,因此解码器会显式返回本错误。
|
||||
LayoutUnconfirmed,
|
||||
/// 校验和 / 校验失败。
|
||||
Checksum,
|
||||
/// 底层 IO 失败。
|
||||
Io,
|
||||
}
|
||||
|
||||
impl core::fmt::Display for ImuError {
|
||||
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
||||
match self {
|
||||
Self::Closed => write!(f, "IMU 链路已关闭"),
|
||||
Self::BadFrame => write!(f, "IMU 帧 ID 或长度不符"),
|
||||
Self::LayoutUnconfirmed => {
|
||||
write!(f, "IMU 载荷布局未确认(ARCHITECTURE.md 决策点 D1:需 DETA10 数据手册)")
|
||||
}
|
||||
Self::Checksum => write!(f, "IMU 校验失败"),
|
||||
Self::Io => write!(f, "IMU 底层 IO 失败"),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl core::error::Error for ImuError {}
|
||||
|
||||
/// 硬件无关的 IMU 边界。
|
||||
pub trait Imu {
|
||||
/// 底层错误类型。
|
||||
type Error: core::error::Error + Send + Sync + 'static;
|
||||
|
||||
/// 读取一帧姿态数据(阻塞直到有帧或链路关闭)。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 链路关闭、帧不合法、载荷布局未确认时返回错误。
|
||||
fn read(&mut self) -> impl core::future::Future<Output = Result<ImuData, Self::Error>>;
|
||||
|
||||
/// 非阻塞读取:本轮没有新帧时返回 `Ok(None)`。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 仅在链路确定损坏时返回错误。
|
||||
fn try_read(&mut self) -> Result<Option<ImuData>, Self::Error> {
|
||||
Ok(None)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn default_data_is_zero_and_finite() {
|
||||
let data = ImuData::default();
|
||||
assert!((data.yaw).abs() < 1e-6);
|
||||
assert!(data.is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn non_finite_attitude_is_detected() {
|
||||
let data = ImuData { pitch: f32::NAN, ..ImuData::default() };
|
||||
assert!(!data.is_finite());
|
||||
let data = ImuData { gyro: [0.0, f32::INFINITY, 0.0], ..ImuData::default() };
|
||||
assert!(!data.is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn layout_unconfirmed_error_names_the_decision_point() {
|
||||
let text = ImuError::LayoutUnconfirmed.to_string();
|
||||
assert!(text.contains("D1"), "{text}");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user