refactor: enforce layered crates and Rust audit gates
This commit is contained in:
Horldsence
@@ -0,0 +1,193 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! DETA10 IMU 驱动：把总线上的 IMU 帧变成 [`ImuData`]（ARCHITECTURE.md §5.3）。
|
||||
//!
|
||||
//! 本驱动只负责**收发与筛选**：帧格式与载荷布局的解析在
|
||||
//! [`motor_protocol::delta10`]，那里也记录了为什么载荷布局仍是待确认的决策点 D1。
|
||||
//! 因此本模块在默认配置下会返回 [`ImuError::LayoutUnconfirmed`]——
|
||||
//! 这是**有意的**：宁可明确报错，也不把猜出来的姿态角送进云台闭环。
|
||||
|
||||
use can::CanBus;
|
||||
use delta10_protocol::{self as delta10, Delta10Config};
|
||||
use imu::{Imu, ImuData, ImuError};
|
||||
|
||||
fn now_ns() -> u64 {
|
||||
use std::time::{SystemTime, UNIX_EPOCH};
|
||||
SystemTime::now().duration_since(UNIX_EPOCH).map(|d| d.as_nanos() as u64).unwrap_or_default()
|
||||
}
|
||||
|
||||
/// 挂在任意 [`CanBus`] 上的 DETA10 驱动。
|
||||
///
|
||||
/// 与电机共享总线（ARCHITECTURE.md §2.1），因此 [`Delta10Driver::read`] 会
|
||||
/// **跳过**所有不是 IMU 的帧（总线上大部分帧是电机反馈）。
|
||||
#[derive(Debug)]
|
||||
pub struct Delta10Driver<B> {
|
||||
bus: B,
|
||||
config: Delta10Config,
|
||||
}
|
||||
|
||||
impl<B> Delta10Driver<B> {
|
||||
/// 组装。
|
||||
#[must_use]
|
||||
pub fn new(bus: B, config: Delta10Config) -> Self {
|
||||
Self { bus, config }
|
||||
}
|
||||
|
||||
/// 当前配置（D1 确认后由上层换成带布局的配置）。
|
||||
#[must_use]
|
||||
pub const fn config(&self) -> &Delta10Config {
|
||||
&self.config
|
||||
}
|
||||
|
||||
/// 热更新配置（例如运行期从配置中心注入确认后的布局）。
|
||||
pub fn set_config(&mut self, config: Delta10Config) {
|
||||
self.config = config;
|
||||
}
|
||||
|
||||
/// 取回底层总线。
|
||||
#[must_use]
|
||||
pub fn into_inner(self) -> B {
|
||||
self.bus
|
||||
}
|
||||
}
|
||||
|
||||
impl<B: CanBus> Delta10Driver<B> {
|
||||
/// 阻塞读取下一帧**姿态**数据，跳过非 IMU 帧。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// - 总线关闭 → [`ImuError::Closed`]；
|
||||
/// - 帧 ID 命中但布局未确认 → [`ImuError::LayoutUnconfirmed`]（D1）；
|
||||
/// - 帧不合法 → [`ImuError::BadFrame`]。
|
||||
pub async fn read(&mut self) -> Result<ImuData, ImuError> {
|
||||
loop {
|
||||
let frame = self.bus.recv().await.map_err(|_| ImuError::Closed)?;
|
||||
// 非 IMU 帧（总线上大部分是电机反馈）直接丢弃，继续等下一帧。
|
||||
if let Some(data) = delta10::try_decode(&self.config, &frame, now_ns())? {
|
||||
return Ok(data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 非阻塞读取：本轮没有 IMU 帧时返回 `Ok(None)`。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 同 [`Delta10Driver::read`]（只是不会等待）。
|
||||
pub fn try_read(&mut self) -> Result<Option<ImuData>, ImuError> {
|
||||
loop {
|
||||
let frame = match self.bus.try_recv() {
|
||||
Ok(Some(frame)) => frame,
|
||||
Ok(None) => return Ok(None),
|
||||
Err(_) => return Err(ImuError::Closed),
|
||||
};
|
||||
if let Some(data) = delta10::try_decode(&self.config, &frame, now_ns())? {
|
||||
return Ok(Some(data));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<B: CanBus> Imu for Delta10Driver<B> {
|
||||
type Error = ImuError;
|
||||
|
||||
async fn read(&mut self) -> Result<ImuData, Self::Error> {
|
||||
Self::read(self).await
|
||||
}
|
||||
|
||||
fn try_read(&mut self) -> Result<Option<ImuData>, Self::Error> {
|
||||
Self::try_read(self)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use can::CanFrame;
|
||||
use delta10_protocol::{Delta10Layout, FieldLayout, FieldWidth};
|
||||
|
||||
use can::MockBus;
|
||||
|
||||
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),
|
||||
}
|
||||
}
|
||||
|
||||
fn imu_frame(roll_mdeg: i16) -> CanFrame {
|
||||
let mut data = [0u8; 8];
|
||||
data[0..2].copy_from_slice(&roll_mdeg.to_le_bytes());
|
||||
CanFrame::new(0x100, data)
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn read_skips_motor_frames_and_returns_attitude() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.inject(CanFrame::new(0x201, [0; 8])); // 电机反馈：必须被跳过
|
||||
bus.inject(CanFrame::new(0x202, [0; 8]));
|
||||
bus.inject(imu_frame(2500));
|
||||
let mut driver = Delta10Driver::new(
|
||||
bus,
|
||||
Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()),
|
||||
);
|
||||
let data = driver.read().await.unwrap();
|
||||
assert!((data.roll - 2.5).abs() < 1e-6, "{}", data.roll);
|
||||
assert!(data.is_finite());
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn default_config_refuses_to_guess() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.inject(imu_frame(1000));
|
||||
let mut driver = Delta10Driver::new(bus, Delta10Config::default().with_can_id(0x100));
|
||||
assert_eq!(driver.read().await.unwrap_err(), ImuError::LayoutUnconfirmed);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn try_read_polls_without_blocking() {
|
||||
let bus = MockBus::new();
|
||||
let mut driver = Delta10Driver::new(
|
||||
bus,
|
||||
Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()),
|
||||
);
|
||||
assert_eq!(driver.try_read().unwrap(), None, "空总线不是错误");
|
||||
|
||||
driver.into_inner().inject(imu_frame(-1500));
|
||||
// 重新组装以取回总线（inject 需要 &mut）。
|
||||
let mut bus = MockBus::new();
|
||||
bus.inject(imu_frame(-1500));
|
||||
let mut driver = Delta10Driver::new(
|
||||
bus,
|
||||
Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()),
|
||||
);
|
||||
let data = driver.try_read().unwrap().unwrap();
|
||||
assert!((data.roll + 1.5).abs() < 1e-6, "{}", data.roll);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn closed_bus_is_reported_as_closed() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.close();
|
||||
let mut driver = Delta10Driver::new(
|
||||
bus,
|
||||
Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()),
|
||||
);
|
||||
assert_eq!(driver.read().await.unwrap_err(), ImuError::Closed);
|
||||
assert_eq!(driver.try_read().unwrap_err(), ImuError::Closed);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn works_through_the_imu_trait() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.inject(imu_frame(3000));
|
||||
let mut driver = Delta10Driver::new(
|
||||
bus,
|
||||
Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()),
|
||||
);
|
||||
// 经 `Imu` trait 调用（证明驱动满足 L1 的抽象边界）。
|
||||
let data: ImuData = Imu::read(&mut driver).await.unwrap();
|
||||
assert!((data.roll - 3.0).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user