feat: 添加了自动超参数整定

2026-09-23 00:42:26 +08:00
parent 7d87987e41
commit 680adae339
56 changed files with 13664 additions and 245 deletions
@@ -0,0 +1,419 @@
+//! 闭环集成测试：`EscBus` + 内存通道 + 一阶电机仿真。
+//!
+//! 全部可在任意平台（macOS / Linux / CI）运行，无需硬件。
+
+#![allow(clippy::unwrap_used, clippy::expect_used)]
+
+use std::time::Duration;
+
+use dji_esc::{
+    BusOptions, CanFrame, CanLink, EscBus, EscBusError, EscConfig, EscFeedback,
+    encode_feedback_frame,
+};
+use dji_esc_sim::spawn_plant;
+use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive, VelocityGains};
+use tokio::sync::mpsc;
+use tokio::task::JoinHandle;
+use tokio::time::{MissedTickBehavior, interval, sleep, timeout};
+
+/// 与 `PlantParams::gm3508` 增益精确匹配的速度环参数（ff = 16384 / 9254）。
+fn tuned(esc_id: u8, direction: f32) -> EscConfig {
+    let mut cfg = EscConfig::gm3508(format!("m{esc_id}"), esc_id, direction);
+    cfg.kp = 8.0;
+    cfg.ki = 2.0;
+    cfg.kd = 0.0;
+    cfg.feedforward = 16384.0 / 9254.0;
+    cfg
+}
+
+/// 轮询直到条件满足或超时（毫秒，仿真时间）。
+async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool {
+    let mut waited = 0u64;
+    while waited < total_ms {
+        if cond() {
+            return true;
+        }
+        sleep(Duration::from_millis(5)).await;
+        waited += 5;
+    }
+    cond()
+}
+
+/// 与 [`wait_until`] 相同，但每次轮询都重新下发 `target_rpm`。
+///
+/// **命令是租约**：驱动在 [`BusOptions::command_timeout`]（默认 50ms）内未收到
+/// 新命令就强制 0 输出。真实底盘循环每 1ms 重发一次，这里以 5ms 节拍模拟。
+async fn hold_until<M: VelocityDrive>(
+    total_ms: u64,
+    motor: &M,
+    target_rpm: f32,
+    mut cond: impl FnMut() -> bool,
+) -> bool {
+    let mut waited = 0u64;
+    loop {
+        motor.set_velocity(target_rpm); // 续租
+        if cond() {
+            return true;
+        }
+        if waited >= total_ms {
+            return false;
+        }
+        sleep(Duration::from_millis(5)).await;
+        waited += 5;
+    }
+}
+
+/// 保持 `target_rpm` 共 `ms` 毫秒（期间持续续租），仅推进仿真时间。
+async fn hold<M: VelocityDrive>(motor: &M, target_rpm: f32, ms: u64) {
+    let _ = hold_until(ms, motor, target_rpm, || false).await;
+}
+
+/// 手动向总线注入一帧所有电机的反馈（新鲜度 100ms 内有效）。
+async fn send_feedback(peer: &CanLink, esc_ids: &[u8]) {
+    for &id in esc_ids {
+        let fb = EscFeedback { angle_raw: 0, speed_rpm: 0, current_raw: 0, temperature_c: 30 };
+        peer.outgoing_tx.send(encode_feedback_frame(id, &fb)).await.unwrap();
+    }
+}
+
+async fn recv_command(peer: &mut CanLink) -> CanFrame {
+    timeout(Duration::from_millis(100), peer.incoming_rx.recv())
+        .await
+        .expect("timed out waiting for command frame")
+        .expect("link closed")
+}
+
+fn slot_current(frame: &CanFrame, slot: usize) -> i16 {
+    i16::from_be_bytes([frame.data[slot * 2], frame.data[slot * 2 + 1]])
+}
+
+// ---------------------------------------------------------------- 收敛性
+
+#[tokio::test(start_paused = true)]
+async fn velocity_loop_converges() {
+    let (bus_link, plant_link) = CanLink::pair(512);
+    let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
+    let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
+    let motor = bus.motor(1).unwrap().clone();
+
+    motor.set_velocity(3000.0);
+    let ok = hold_until(2000, &motor, 3000.0, || motor.state().rotor_speed_rpm >= 2950.0).await;
+    assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm);
+
+    // 稳态保持（速度无漂移、反馈持续流入）；期间必须继续续租命令。
+    let count_before = motor.state().feedback_count;
+    hold(&motor, 3000.0, 300).await;
+    let st = motor.state();
+    assert!((2950.0..=3050.0).contains(&st.rotor_speed_rpm), "steady state drifted: {st:?}");
+    assert!(
+        st.feedback_count >= count_before + 280,
+        "feedback should keep flowing: {} → {}",
+        count_before,
+        st.feedback_count
+    );
+
+    plant.abort();
+    bus.shutdown().await;
+}
+
+#[tokio::test(start_paused = true)]
+async fn reversed_motor_converges_in_physical_frame() {
+    // direction = -1：物理 +2000 rpm ⇔ 电调侧 -2000 rpm。
+    let (bus_link, plant_link) = CanLink::pair(512);
+    let plant = spawn_plant(plant_link, &[2], Duration::from_millis(1));
+    let bus = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
+    let motor = bus.motor(2).unwrap().clone();
+
+    motor.set_velocity(2000.0);
+    let ok = hold_until(2000, &motor, 2000.0, || motor.state().rotor_speed_rpm >= 1950.0).await;
+    assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm);
+
+    let st = motor.state();
+    assert!((1950.0..=2050.0).contains(&st.rotor_speed_rpm), "{st:?}");
+    // 物理转速为正 ⇒ 电调电流为负 ⇒ 物理电流 = dir × esc > 0。
+    assert!(st.effort_raw > 0, "{st:?}");
+
+    plant.abort();
+    bus.shutdown().await;
+}
+
+// ---------------------------------------------------------------- 安全
+
+#[tokio::test(start_paused = true)]
+async fn motor_without_feedback_commands_zero_current() {
+    let (bus_link, mut peer) = CanLink::pair(64);
+    // 不接 plant → 永远没有反馈。
+    let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
+
+    bus.motor(1).unwrap().set_velocity(5000.0);
+    for _ in 0..5 {
+        let frame = recv_command(&mut peer).await;
+        assert_eq!(frame.id, 0x200);
+        assert_eq!(frame.data, [0; 8], "must send zero current while offline");
+    }
+    bus.shutdown().await;
+}
+
+#[tokio::test(start_paused = true)]
+async fn current_limit_clamps_saturation() {
+    let (bus_link, mut peer) = CanLink::pair(64);
+    let mut cfg = tuned(1, 1.0);
+    cfg.current_limit = 5000;
+    let bus = EscBus::spawn(bus_link, vec![cfg], BusOptions::default()).unwrap();
+
+    send_feedback(&peer, &[1]).await;
+    bus.motor(1).unwrap().set_velocity(30000.0); // 不可达目标 → 深度饱和
+    sleep(Duration::from_millis(5)).await;
+
+    let mut saw_saturated = false;
+    for _ in 0..5 {
+        bus.motor(1).unwrap().set_velocity(30000.0); // 续租，避免读到租约到期后的 0
+        let frame = recv_command(&mut peer).await;
+        let raw = slot_current(&frame, 0);
+        assert!(raw <= 5000, "current {raw} exceeds limit 5000");
+        saw_saturated |= raw == 5000;
+    }
+    assert!(saw_saturated, "loop should reach the configured limit");
+    bus.shutdown().await;
+}
+
+// ---------------------------------------------------------------- 命令租约
+
+/// 反馈喂食器：每 5ms 注入一帧所有电机的反馈，让电机保持"在线"，
+/// 从而把「租约到期归零」与「离线保护归零」区分开。
+fn spawn_feedback(yield_tx: mpsc::Sender<CanFrame>, esc_ids: Vec<u8>) -> JoinHandle<()> {
+    tokio::spawn(async move {
+        let mut ticker = interval(Duration::from_millis(5));
+        ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
+        let fb = EscFeedback { angle_raw: 0, speed_rpm: 0, current_raw: 0, temperature_c: 30 };
+        loop {
+            ticker.tick().await;
+            for &id in &esc_ids {
+                if yield_tx.send(encode_feedback_frame(id, &fb)).await.is_err() {
+                    return;
+                }
+            }
+        }
+    })
+}
+
+/// 一次性 `set_velocity` 在租约内出力，租约到期后必须自动归零；
+/// 重新下发又能恢复出力——这就是"命令是租约"契约的可执行规范。
+#[tokio::test(start_paused = true)]
+async fn command_lease_expires_and_renews() {
+    let (bus_link, mut peer) = CanLink::pair(256);
+    let feeder = spawn_feedback(peer.outgoing_tx.clone(), vec![1]);
+    let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
+    let motor = bus.motor(1).unwrap().clone();
+
+    // 1) 下发后 30ms 内（< 50ms 租约）应看到非零电流。
+    motor.set_velocity(3000.0);
+    let mut live = false;
+    for _ in 0..30 {
+        if slot_current(&recv_command(&mut peer).await, 0) != 0 {
+            live = true;
+            break;
+        }
+    }
+    assert!(live, "租约内应输出非零电流");
+
+    // 2) 沉默 200ms（> 50ms 租约）→ 必须强制归零。
+    sleep(Duration::from_millis(200)).await;
+    // 排空沉默期间积压的帧：其中前段仍带着租约到期前的旧命令。
+    while peer.incoming_rx.try_recv().is_ok() {}
+    sleep(Duration::from_millis(10)).await;
+    for _ in 0..5 {
+        let raw = slot_current(&recv_command(&mut peer).await, 0);
+        assert_eq!(raw, 0, "租约到期后必须归零，实测 {raw}");
+    }
+
+    // 3) 续租 → 恢复出力。
+    motor.set_velocity(3000.0);
+    let mut renewed = false;
+    for _ in 0..30 {
+        if slot_current(&recv_command(&mut peer).await, 0) != 0 {
+            renewed = true;
+            break;
+        }
+    }
+    assert!(renewed, "续租后应恢复非零电流");
+
+    feeder.abort();
+    bus.shutdown().await;
+}
+
+// ---------------------------------------------------------------- 组帧与路由
+
+#[tokio::test(start_paused = true)]
+async fn current_commands_route_to_group_slots() {
+    let (bus_link, mut peer) = CanLink::pair(256);
+    let configs = (1..=5).map(|id| tuned(id, 1.0)).collect();
+    let bus = EscBus::spawn(bus_link, configs, BusOptions::default()).unwrap();
+
+    bus.motor(3).unwrap().set_effort_raw(1234);
+    bus.motor(5).unwrap().set_effort_raw(-567);
+    send_feedback(&peer, &(1..=5).collect::<Vec<_>>()).await;
+    sleep(Duration::from_millis(5)).await;
+
+    let mut saw_200 = false;
+    let mut saw_1ff = false;
+    for _ in 0..20 {
+        // 续租两条直发命令，避免读到租约到期后的 0 槽。
+        bus.motor(3).unwrap().set_effort_raw(1234);
+        bus.motor(5).unwrap().set_effort_raw(-567);
+        let frame = recv_command(&mut peer).await;
+        match frame.id {
+            0x200 if !saw_200 => {
+                assert_eq!(frame.data, [0, 0, 0, 0, 0x04, 0xD2, 0, 0], "esc3 slot = 1234");
+                saw_200 = true;
+            }
+            0x1FF if !saw_1ff => {
+                // -567 = 0xFDC9（i16 补码，大端）。
+                assert_eq!(frame.data, [0xFD, 0xC9, 0, 0, 0, 0, 0, 0], "esc5 slot = -567");
+                saw_1ff = true;
+            }
+            _ => {}
+        }
+        if saw_200 && saw_1ff {
+            break;
+        }
+    }
+    assert!(saw_200, "no 0x200 frame observed");
+    assert!(saw_1ff, "no 0x1FF frame observed");
+
+    // disable 后槽位清零。
+    bus.motor(3).unwrap().disable();
+    let zeroed = wait_until(200, || {
+        try_find_frame(&mut peer, |frame| frame.id == 0x200 && slot_current(frame, 2) == 0)
+    })
+    .await;
+    assert!(zeroed, "esc3 slot should return to 0 after disable");
+    bus.shutdown().await;
+}
+
+/// 非阻塞地扫一眼通道里已有的帧。
+fn try_find_frame(peer: &mut CanLink, pred: impl Fn(&CanFrame) -> bool) -> bool {
+    while let Ok(frame) = peer.incoming_rx.try_recv() {
+        if pred(&frame) {
+            return true;
+        }
+    }
+    false
+}
+
+// ---------------------------------------------------------------- 反馈 → 状态
+
+#[tokio::test]
+async fn feedback_updates_state_with_direction() {
+    let (bus_link, peer) = CanLink::pair(64);
+    let bus = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
+
+    // 两帧正向跨圈：8000 → 100，电调侧圈数 ≈ 1.0122。
+    let fb = EscFeedback { angle_raw: 8000, speed_rpm: 600, current_raw: -300, temperature_c: 47 };
+    peer.outgoing_tx.send(encode_feedback_frame(2, &fb)).await.unwrap();
+    let fb2 = EscFeedback { angle_raw: 100, ..fb };
+    peer.outgoing_tx.send(encode_feedback_frame(2, &fb2)).await.unwrap();
+    sleep(Duration::from_millis(50)).await;
+
+    let st = bus.motor(2).unwrap().state();
+    assert_eq!(st.rotor_angle_raw, 100);
+    assert!((st.rotor_speed_rpm + 600.0).abs() < 1e-3); // direction = -1
+    assert_eq!(st.effort_raw, 300);
+    assert_eq!(st.temperature_c, 47);
+    assert_eq!(st.feedback_count, 2);
+    let esc_rev = 1.0 + 100.0 / 8192.0;
+    assert!(
+        (st.rotor_revolutions + esc_rev).abs() < 1e-3,
+        "revolutions = {}",
+        st.rotor_revolutions
+    );
+    // 输出轴换算由 `OutputShaft` 负责（见 motor-core gearbox 单测）。
+    bus.shutdown().await;
+}
+
+// ---------------------------------------------------------------- 生命周期
+
+#[tokio::test(start_paused = true)]
+async fn shutdown_stops_tasks_and_closes_link() {
+    let (bus_link, mut peer) = CanLink::pair(128);
+    // 不接 plant，直接验证任务生命周期。
+    let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
+    bus.motor(1).unwrap().set_velocity(1000.0);
+    sleep(Duration::from_millis(20)).await;
+    // 有命令帧在流动。
+    assert!(peer.incoming_rx.recv().await.is_some());
+
+    bus.shutdown().await;
+    // 排空残留缓冲帧后应立即得到 None（发送端已全部退出）；
+    // 若任务仍在运行，帧会持续流入并触发超时。
+    loop {
+        match timeout(Duration::from_millis(100), peer.incoming_rx.recv()).await {
+            Ok(None) => break,
+            Ok(Some(_)) => {} // 排空残留缓冲帧
+            Err(elapsed) => panic!("bus tasks still running after shutdown (waited {elapsed:?})"),
+        }
+    }
+}
+
+#[test]
+fn spawn_rejects_bad_configs() {
+    let link = || CanLink::pair(8).0;
+    assert_eq!(
+        EscBus::spawn(link(), vec![], BusOptions::default()).err(),
+        Some(EscBusError::NoMotors)
+    );
+    assert_eq!(
+        EscBus::spawn(link(), vec![tuned(2, 1.0), tuned(2, -1.0)], BusOptions::default()).err(),
+        Some(EscBusError::DuplicateEscId(2))
+    );
+    let mut bad = tuned(1, 1.0);
+    bad.direction = 0.0;
+    let err = EscBus::spawn(link(), vec![bad], BusOptions::default()).err();
+    assert!(
+        matches!(err, Some(EscBusError::InvalidConfig { .. })),
+        "expected InvalidConfig, got {err:?}"
+    );
+}
+
+// ---------------------------------------------------------------- 增益热更新
+
+/// 自动整定的真机后端靠这条路径逐次试验：不重启总线，只换增益。
+/// 契约：① 读取即得到最新增益；② 换增益后控制器**状态复位**（无旧积分残留）；
+/// ③ 限幅等安全字段不受影响。
+#[tokio::test(start_paused = true)]
+async fn velocity_gains_can_be_hot_swapped() {
+    let (bus_link, plant_link) = CanLink::pair(256);
+    let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
+    let cfg = tuned(1, 1.0);
+    let limit = i32::from(cfg.current_limit);
+    let bus = EscBus::spawn(bus_link, vec![cfg.clone()], BusOptions::default()).unwrap();
+    let motor = bus.motor(1).unwrap().clone();
+
+    // ① 初始增益来自 EscConfig（含前馈）。
+    assert_eq!(motor.velocity_gains(), cfg.velocity_gains());
+
+    // 让旧增益的积分先积累起来（大静差 + 深度饱和）。
+    motor.set_velocity(9000.0);
+    hold(&motor, 9000.0, 200).await;
+    let stale = motor.velocity_gains();
+
+    // ② 热更新：控制器被重建。
+    let swapped = VelocityGains::new(1.0, 0.0, 0.0, 0.0);
+    motor.set_velocity_gains(swapped);
+    assert_eq!(motor.velocity_gains(), swapped);
+    assert_ne!(motor.velocity_gains(), stale);
+
+    // 新增益生效：误差 e 时输出 = kp·e = e（无前馈、无积分），并仍被限幅夹住。
+    motor.set_velocity(30_000.0);
+    let saturated =
+        hold_until(500, &motor, 30_000.0, || i32::from(motor.state().effort_raw) >= limit).await;
+    assert!(saturated, "effort = {} 未达到限幅 {}", motor.state().effort_raw, limit);
+
+    // ③ 限幅仍是启动时的 current_limit：热更新不得放宽安全边界。
+    let effort = i32::from(motor.state().effort_raw);
+    assert_eq!(effort, limit, "饱和值必须正好是 current_limit");
+    assert_eq!(motor.velocity_gains(), swapped, "热更新后增益保持不变");
+
+    plant.abort();
+    bus.shutdown().await;
+}