refactor: 重构架构并整理仓库

2026-09-23 10:56:57 +08:00
parent 680adae339
commit f8aeafdfc2
101 changed files with 14417 additions and 11399 deletions
@@ -0,0 +1,17 @@
+[package]
+name = "delta10"
+description = "DETA10 IMU driver"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+
+[lints]
+workspace = true
+
+[dependencies]
+can = { workspace = true }
+imu = { workspace = true }
+delta10-protocol = { workspace = true }
+tokio = { workspace = true }
+thiserror = { workspace = true }
@@ -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);
+    }
+}