ELCT/linux_sentry_chassis
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Horldsence
2026-09-23 00:42:26 +08:00
parent 7d87987e41
commit 680adae339
56 changed files with 13664 additions and 245 deletions
Download Patch File Download Diff File Expand all files Collapse all files
crates/dji-esc/src/bus.rs
+115 -9
View File
@@ -14,7 +14,7 @@ use std::time::Duration;
use motor_core::{
Command, CommandMode, Disable, EffortDrive, MotorFeedback, MotorState, Pid, PositionController,
PositionDrive, VelocityController, VelocityDrive,
PositionDrive, VelocityController, VelocityDrive, VelocityGains,
};
use thiserror::Error;
use tokio::sync::{mpsc, watch};
@@ -44,6 +44,9 @@ pub struct EscConfig {
/// 电调型号。
pub model: EscModel,
/// 减速比（电机转子 : 输出轴）。
///
/// 驱动层**不使用**此字段；由上层 [`motor_core::OutputShaft`] 应用。
/// 保留于此作为配置元数据，`chassisd` 包装轮子时必须读取本字段（单一来源）。
pub gear_ratio: f32,
/// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转，
/// 上层永远以"输出轴正转 = 前进方向"工作。
@@ -99,6 +102,12 @@ impl EscConfig {
}
}
/// 速度环增益组（可整定参数）。
#[must_use]
pub fn velocity_gains(&self) -> VelocityGains {
VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward)
}
fn validate(&self) -> Result<(), String> {
if command_frame_id(self.esc_id).is_none() {
return Err(format!("esc_id {} out of range 1..=8", self.esc_id));
@@ -130,11 +139,25 @@ pub struct BusOptions {
pub period: Duration,
/// 反馈离线阈值：超过该时长未收到某电机反馈 → 强制 0 输出。
pub feedback_timeout: Duration,
/// **命令租约**：上层超过该时长未重新下发命令 → 视为 `Disabled`（0 输出）。
///
/// 命令是**租约**而非一次性设定值：`set_velocity` / `set_effort_raw` /
/// `set_position` 每次调用都会续租。上层必须按自己的控制节拍周期性下发
/// （`chassisd` 的 1kHz 循环天然满足；台架脚本每拍重发即可），
/// 否则驱动会判定"上层任务已死"并强制零输出——这是防失控的安全设计。
///
/// 设置为 `Duration::MAX` 可退化为纯设定值语义（仅建议在无失控风险的台架场景）。
pub command_timeout: Duration,
}
impl Default for BusOptions {
fn default() -> Self {
Self { period: Duration::from_millis(1), feedback_timeout: Duration::from_millis(100) }
Self {
period: Duration::from_millis(1),
feedback_timeout: Duration::from_millis(100),
// 1kHz 下约 50 拍未刷新即失效。
command_timeout: Duration::from_millis(50),
}
}
}
@@ -162,6 +185,8 @@ struct MotorShared {
cfg: EscConfig,
state: Mutex<MotorState>,
cmd: Mutex<Command>,
/// 命令写入时刻（看门狗用）。
cmd_at: Mutex<Option<Instant>>,
last_mode: Mutex<CommandMode>,
vel_ctrl: Mutex<VelocityController>,
pos_ctrl: Mutex<PositionController>,
@@ -181,6 +206,7 @@ impl MotorShared {
cfg,
state: Mutex::new(MotorState::default()),
cmd: Mutex::new(Command::Disabled),
cmd_at: Mutex::new(None),
last_mode: Mutex::new(CommandMode::Disabled),
turn: Mutex::new(TurnTracker::new()),
last_feedback: Mutex::new(None),
@@ -188,6 +214,26 @@ impl MotorShared {
}
}
fn store_command(&self, cmd: Command) {
*lock(&self.cmd) = cmd;
*lock(&self.cmd_at) = Some(Instant::now());
}
/// 当前速度环增益（来自控制器，而非启动时的 `cfg`）。
fn velocity_gains(&self) -> VelocityGains {
lock(&self.vel_ctrl).gains()
}
/// 热更新速度环增益：重建控制器（等价于 reset，避免积分冲击）。
///
/// 只替换增益，不动限幅 / 型号 / 方向 —— 那些是安全与机械约束，
/// 不属于可在线试错的整定量。
fn set_velocity_gains(&self, gains: VelocityGains) {
let limit = self.cfg.current_limit;
let mut controller = lock(&self.vel_ctrl);
*controller = VelocityController::from_gains(limit, gains);
}
/// 应用一帧反馈（原始电调侧值 → 方向修正后的物理量）。
fn apply_feedback(&self, fb: EscFeedback, now: Instant) {
let esc_rev = lock(&self.turn).update(fb.angle_raw);
@@ -212,7 +258,13 @@ impl MotorShared {
}
/// 求值本节拍应下发的 effort 命令（电调侧 raw，含方向翻转与限幅）。
fn compute_effort(&self, dt: f32, feedback_timeout: Duration, now: Instant) -> i16 {
fn compute_effort(
&self,
dt: f32,
feedback_timeout: Duration,
command_timeout: Duration,
now: Instant,
) -> i16 {
let online = matches!(*lock(&self.last_feedback), Some(t) if now.duration_since(t) <= feedback_timeout);
if !online {
if !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) {
@@ -223,6 +275,14 @@ impl MotorShared {
*lock(&self.last_mode) = CommandMode::Disabled;
return 0;
}
// 命令看门狗：上层任务死亡/卡死后旧指令不得持续输出。
let cmd_fresh =
matches!(*lock(&self.cmd_at), Some(t) if now.duration_since(t) <= command_timeout);
if !cmd_fresh {
self.reset_controllers();
*lock(&self.last_mode) = CommandMode::Disabled;
return 0;
}
let cmd = *lock(&self.cmd);
// 模式切换复位：避免积分/微分记忆冲击。
{
@@ -257,6 +317,13 @@ impl MotorShared {
}
/// 电机的公共句柄：可克隆，跨任务共享。
///
/// # 命令租约
///
/// 下发命令（[`VelocityDrive::set_velocity`] / [`EffortDrive::set_effort_raw`] /
/// [`PositionDrive::set_position`]）即续租，租期见 [`BusOptions::command_timeout`]。
/// 只在启动时下发一次、之后不再刷新的脚本会在租约到期后自动失电（0 输出），
/// 因此台架工具必须在控制循环里逐拍重发同一个设定值。
#[derive(Clone)]
pub struct EscMotor {
shared: Arc<MotorShared>,
@@ -286,6 +353,25 @@ impl EscMotor {
pub fn last_feedback(&self) -> Option<Instant> {
*lock(&self.shared.last_feedback)
}
/// 当前速度环增益。
#[must_use]
pub fn velocity_gains(&self) -> VelocityGains {
self.shared.velocity_gains()
}
/// 热更新速度环增益（**不需重启总线**，下一发送节拍生效）。
///
/// 用于自动整定：一次真机试验只改增益，不必反复开关串口 / CAN 接口。
/// 控制器被重建，历史积分被丢弃——这正是一次新试验想要的起点。
///
/// # Panics
///
/// 增益含 NaN / inf 时 panic（同 [`Pid::new`]）；调用方应先检查
/// [`VelocityGains::is_finite`]。
pub fn set_velocity_gains(&self, gains: VelocityGains) {
self.shared.set_velocity_gains(gains);
}
}
impl MotorFeedback for EscMotor {
@@ -296,29 +382,38 @@ impl MotorFeedback for EscMotor {
fn state(&self) -> MotorState {
*lock(&self.shared.state)
}
fn feedback_age(&self) -> Option<std::time::Duration> {
let t = *lock(&self.shared.last_feedback);
Some(Instant::now().saturating_duration_since(t?))
}
}
impl VelocityDrive for EscMotor {
/// 设定目标转子转速（**租约**，见 [`EscMotor`] 的命令租约说明）。
fn set_velocity(&self, rotor_rpm: f32) {
*lock(&self.shared.cmd) = Command::Velocity(rotor_rpm);
self.shared.store_command(Command::Velocity(rotor_rpm));
}
}
impl EffortDrive for EscMotor {
/// 直发开环 raw（**租约**，见 [`EscMotor`] 的命令租约说明）。
fn set_effort_raw(&self, raw: i16) {
*lock(&self.shared.cmd) = Command::EffortRaw(raw);
self.shared.store_command(Command::EffortRaw(raw));
}
}
impl PositionDrive for EscMotor {
/// 设定目标转子多圈位置（**租约**，见 [`EscMotor`] 的命令租约说明）。
fn set_position(&self, rotor_rev: f32) {
*lock(&self.shared.cmd) = Command::Position(rotor_rev);
self.shared.store_command(Command::Position(rotor_rev));
}
}
impl Disable for EscMotor {
/// 停用（0 输出）；同样受租约保护——上层若已死，租约到期后依旧归零。
fn disable(&self) {
*lock(&self.shared.cmd) = Command::Disabled;
self.shared.store_command(Command::Disabled);
}
}
@@ -423,14 +518,20 @@ async fn tx_loop(
options: BusOptions,
mut shutdown: watch::Receiver<bool>,
) {
let dt = options.period.as_secs_f32();
let nominal_dt = options.period.as_secs_f32();
let mut ticker = tick_every(options.period);
// 实测 dt：丢拍/负载抖动时 PID 不得按名义 1ms 硬算（F-1）。
let mut last_tick = Instant::now();
loop {
tokio::select! {
_ = shutdown.changed() => break,
_ = ticker.tick() => {}
}
let now = Instant::now();
let raw_dt = now.saturating_duration_since(last_tick).as_secs_f32();
last_tick = now;
// 夹在 [0.5, 4] × 名义周期，避免首拍/长卡死把积分或微分打飞。
let dt = raw_dt.clamp(nominal_dt * 0.5, nominal_dt * 4.0);
for (frame_id, group_start) in
[(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)]
{
@@ -444,7 +545,12 @@ async fn tx_loop(
let mut currents = [0i16; 4];
for motor in &group {
let slot = usize::from(motor.id() - group_start);
currents[slot] = motor.shared.compute_effort(dt, options.feedback_timeout, now);
currents[slot] = motor.shared.compute_effort(
dt,
options.feedback_timeout,
options.command_timeout,
now,
);
}
if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() {
info!("can link closed, tx loop exiting");
crates/dji-esc/src/lib.rs
+4 -4
View File
@@ -5,17 +5,18 @@
//! - [`codec`]：纯函数帧编解码（C620 手册协议）；
//! - [`turn`]：单圈编码器 → 连续多圈角；
//! - [`frame`]：协议帧 ID 常量（帧/链路类型见 [`can_link`]）；
//! - [`bus`]：[`EscBus`] 驱动——反馈分发、主机速度/位置环、分组组帧、离线安全；
//! - [`sim`]：被控对象仿真（测试与 `--mock` 演示复用）。
//! - [`bus`]：[`EscBus`] 驱动——反馈分发、主机速度/位置环、分组组帧、离线安全。
//!
//! 电机以 [`motor_core`] 能力 trait 呈现，物理量均已按 `EscConfig::direction`
//! 完成方向修正。
//!
//! 被控对象仿真（假电调）**不在本 crate**：见 `dji-esc-sim`。本 crate 只描述
//! 真实电调在总线上的行为，不假设对面是谁。
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
pub mod bus;
pub mod codec;
pub mod frame;
pub mod sim;
pub mod turn;
pub use bus::{BusOptions, EscBus, EscBusError, EscBusHandle, EscConfig, EscMotor};
@@ -25,5 +26,4 @@ pub use codec::{
decode_feedback, encode_current_frame, encode_feedback_frame,
};
pub use frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, FEEDBACK_FRAME_ID_BASE};
pub use sim::{PlantMotor, spawn_plant};
pub use turn::{ENCODER_TICKS, TurnTracker};
crates/dji-esc/src/sim.rs
-116
View File
@@ -1,116 +0,0 @@
//! 电机被控对象仿真：一阶转速模型 + 编码器积分。
//!
//! 与 `EscBus` 接在同一对 [`CanLink`] 上即可构成全链路闭环
//! （测试、`chassisd --mock` 均复用）。
use std::collections::HashMap;
use std::time::Duration;
use tokio::task::JoinHandle;
use tokio::time::{MissedTickBehavior, interval};
use tracing::trace;
use crate::codec::{EscFeedback, encode_feedback_frame};
use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink};
/// 单台电机被控对象（转子侧、电调坐标系：正电流 → 正转速）。
///
/// 模型：`rpm_target = current_raw × gain`（限幅于 `max_rpm`），转速以时间常数
/// `tau` 一阶趋近目标；机械角按转速积分并按 8192 取模。
#[derive(Debug, Clone)]
pub struct PlantMotor {
gain_rpm_per_raw: f32,
tau: f32,
max_rpm: f32,
speed_rpm: f32,
angle_raw: f32,
}
impl PlantMotor {
/// GM3508 量级：16384 raw 空载约 9254 rpm，机电时间常数约 120ms。
#[must_use]
pub fn gm3508() -> Self {
Self {
gain_rpm_per_raw: 9254.0 / 16384.0,
tau: 0.12,
max_rpm: 9254.0,
speed_rpm: 0.0,
angle_raw: 0.0,
}
}
/// 当前转子转速（rpm，电调侧）。
#[must_use]
pub fn speed_rpm(&self) -> f32 {
self.speed_rpm
}
/// 推进一个时间步，返回本步反馈帧内容。
pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback {
let target =
(f32::from(current_raw) * self.gain_rpm_per_raw).clamp(-self.max_rpm, self.max_rpm);
let alpha = 1.0 - (-dt / self.tau).exp();
self.speed_rpm += (target - self.speed_rpm) * alpha;
self.angle_raw = (self.angle_raw + self.speed_rpm / 60.0 * 8192.0 * dt).rem_euclid(8192.0);
EscFeedback {
angle_raw: self.angle_raw as u16,
speed_rpm: self.speed_rpm as i16,
current_raw,
temperature_c: 30,
}
}
}
/// 仿真整条电调总线：消费命令帧（0x200/0x1FF），按 `tick` 节拍产出反馈帧。
///
/// 返回任务句柄，测试结束或退出演示时 `abort()` 即可。
pub fn spawn_plant(mut link: CanLink, esc_ids: &[u8], tick: Duration) -> JoinHandle<()> {
let mut plants: HashMap<u8, PlantMotor> =
esc_ids.iter().map(|&id| (id, PlantMotor::gm3508())).collect();
let mut commands: HashMap<u8, i16> = esc_ids.iter().map(|&id| (id, 0)).collect();
let dt = tick.as_secs_f32();
let mut ticker = interval(tick);
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
tokio::spawn(async move {
loop {
tokio::select! {
frame = link.incoming_rx.recv() => match frame {
Some(frame) => apply_command_frame(&frame, &mut commands),
None => break,
},
_ = ticker.tick() => {
for (&id, plant) in &mut plants {
let cmd = commands.get(&id).copied().unwrap_or(0);
let feedback = plant.step(cmd, dt);
if link
.outgoing_tx
.send(encode_feedback_frame(id, &feedback))
.await
.is_err()
{
return;
}
}
}
}
}
trace!("plant exiting");
})
}
fn apply_command_frame(frame: &CanFrame, commands: &mut HashMap<u8, i16>) {
let group_start = match frame.id {
COMMAND_FRAME_ID_PRIMARY => 1u8,
COMMAND_FRAME_ID_SECONDARY => 5u8,
_ => return,
};
for slot in 0..4u8 {
if let Some(cmd) = commands.get_mut(&(group_start + slot)) {
*cmd = i16::from_be_bytes([
frame.data[(slot * 2) as usize],
frame.data[(slot * 2 + 1) as usize],
]);
}
}
}
crates/dji-esc/tests/closed_loop.rs
-275
View File
@@ -1,275 +0,0 @@
//! 闭环集成测试：`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, spawn_plant,
};
use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive};
use tokio::time::{sleep, timeout};
/// 与 `PlantMotor::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()
}
/// 手动向总线注入一帧所有电机的反馈（新鲜度 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 = wait_until(2000, || 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;
sleep(Duration::from_millis(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 = wait_until(2000, || 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 {
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;
}
// ---------------------------------------------------------------- 组帧与路由
#[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 {
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:?}"
);
}