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

refactor: 重构架构并整理仓库

Browse Source
This commit is contained in:
Horldsence
2026-09-23 10:56:57 +08:00
parent 680adae339
commit f8aeafdfc2
101 changed files with 14417 additions and 11399 deletions
Download Patch File Download Diff File Expand all files Collapse all files
crates/chassisd/Cargo.toml
+10 -10
View File
@@ -1,28 +1,28 @@
[package]
name = "chassisd"
description = "Mecanum chassis daemon"
version.workspace = true
edition.workspace = true
rust-version.workspace = true
license.workspace = true
description = "Mecanum chassis daemon: configuration, 1 kHz control loop, safety supervision, and mock mode"
[lints]
workspace = true
[dependencies]
chassis-config = { workspace = true }
motor-core = { workspace = true }
dji-esc = { workspace = true }
dji-esc-sim = { workspace = true }
dji-esc-socketcan = { workspace = true }
can = { workspace = true }
motor = { workspace = true }
esc-bus = { workspace = true }
motor-control = { workspace = true }
mecanum = { workspace = true }
chassis-config = { workspace = true }
sim-harness = { workspace = true }
socketcan = { workspace = true }
tokio = { workspace = true }
thiserror = { workspace = true }
clap = { workspace = true }
tracing = { workspace = true }
tracing-subscriber = { workspace = true }
clap = { workspace = true }
[dev-dependencies]
proptest = { workspace = true }
approx = { workspace = true }
tokio = { workspace = true, features = ["test-util"] }
approx = { workspace = true }
crates/chassisd/src/controller.rs
-197
View File
@@ -1,197 +0,0 @@
//! 底盘控制循环：麦轮逆解 → 四电机速度指令，含安全逻辑。
use std::time::Duration;
use mecanum::{Mecanum, Twist};
use motor_core::{Disable, MotorFeedback, OutputShaft, VelocityDrive};
use tokio::sync::watch;
use tokio::task::JoinHandle;
use tokio::time::{Instant, MissedTickBehavior, interval};
use tracing::{info, warn};
/// 底盘控制器。
///
/// 职责（1kHz 节拍）：
/// 1. 读取最新 twist 指令，**指令超时则归零**；
/// 2. 速度限幅（保持方向按比例收缩）；
/// 3. **上电门控**：四台电机都收到过反馈才下发速度，否则全部停用；
/// 4. 麦轮逆解 → 各轮目标转速（rpm，轮端）→ 经 [`OutputShaft`] 换算到转子。
///
/// 电机反馈离线安全在驱动层完成（强制 0 电流）。
pub struct ChassisController<W> {
kinematics: Mecanum,
/// 顺序固定：FL、FR、RL、RR。轮端单位（经 `OutputShaft` 包装）。
wheels: [W; 4],
/// (max_vx, max_vy, max_omega)。
limits: (f32, f32, f32),
command_timeout: Duration,
}
impl<W> ChassisController<W>
where
W: VelocityDrive + MotorFeedback + Disable + 'static,
{
/// 组装控制器。`wheels` 顺序必须为 FL、FR、RL、RR。
#[must_use]
pub fn new(
kinematics: Mecanum,
wheels: [W; 4],
limits: (f32, f32, f32),
command_timeout: Duration,
) -> Self {
Self { kinematics, wheels, limits, command_timeout }
}
/// 启动控制循环任务。
pub fn spawn(self, commands: watch::Receiver<Twist>, period: Duration) -> JoinHandle<()> {
tokio::spawn(self.run(commands, period))
}
async fn run(self, mut commands: watch::Receiver<Twist>, period: Duration) {
let mut ticker = interval(period);
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
let mut latest = Twist::ZERO;
let mut last_command_at: Option<Instant> = None;
let mut ticks: u64 = 0;
loop {
ticker.tick().await;
let now = Instant::now();
if commands.has_changed().unwrap_or(false) {
latest = *commands.borrow_and_update();
last_command_at = Some(now);
}
let timed_out = match last_command_at {
Some(t) => now.duration_since(t) > self.command_timeout,
// 尚未收到任何指令：保持静止。
None => true,
};
let twist = if timed_out {
Twist::ZERO
} else {
latest.clamped(self.limits.0, self.limits.1, self.limits.2)
};
let all_seen_feedback = self.wheels.iter().all(|m| m.state().has_feedback());
if all_seen_feedback {
let wheel_rpms = self.kinematics.wheel_rpms(&twist);
for (motor, wheel_rpm) in self.wheels.iter().zip(wheel_rpms) {
motor.set_velocity(wheel_rpm);
}
} else {
for motor in &self.wheels {
motor.disable();
}
if ticks.is_multiple_of(1000) {
warn!("waiting for first feedback from all motors");
}
}
ticks += 1;
if ticks.is_multiple_of(1000) {
self.log_status();
}
}
}
fn log_status(&self) {
let (rotor_rpm, effort_raw): (Vec<f32>, Vec<i32>) = self
.wheels
.iter()
.map(|m| {
let st = m.state();
(st.rotor_speed_rpm, i32::from(st.effort_raw))
})
.unzip();
info!(?rotor_rpm, ?effort_raw, "chassis status");
}
}
/// 由 `EscMotor` 列表组装轮端包装（`gear_ratio` = 转子 : 输出轴）。
#[must_use]
pub fn wrap_wheels<T: Clone, const N: usize>(
motors: [T; N],
gear_ratios: [f32; N],
) -> [OutputShaft<T>; N] {
let mut out = Vec::with_capacity(N);
let mut gears = gear_ratios.into_iter();
for m in motors {
let g = gears.next().unwrap_or(1.0);
out.push(OutputShaft::new(m, g));
}
out.try_into().unwrap_or_else(|_| unreachable!())
}
#[cfg(test)]
mod tests {
use super::*;
use motor_core::{Command, MockMotor, MotorState};
const GEAR: f32 = 19.203;
fn mock_wheel(id: u8) -> MockMotor {
let m = MockMotor::new(id);
m.inject_state(MotorState { feedback_count: 1, ..MotorState::default() });
m
}
/// 证明 `ChassisController` 只依赖能力 trait：换 `MockMotor` 原样工作。
#[tokio::test(start_paused = true)]
async fn controller_drives_wheels_through_capability_traits() {
let kinematics = Mecanum::new(0.2, 0.25, 0.0762);
let mocks = [mock_wheel(1), mock_wheel(2), mock_wheel(3), mock_wheel(4)];
let ctrl = ChassisController::new(
kinematics,
wrap_wheels(mocks.clone(), [GEAR; 4]),
(3.0, 3.0, 6.0),
Duration::from_millis(200),
);
let (tx, rx) = watch::channel(Twist::ZERO);
let handle = ctrl.spawn(rx, Duration::from_millis(1));
let twist = Twist { vx: 1.0, ..Twist::ZERO };
tx.send_replace(twist);
tokio::time::sleep(Duration::from_millis(5)).await;
let expected = kinematics.wheel_rpms(&twist);
for (i, mock) in mocks.iter().enumerate() {
match mock.last_command() {
Command::Velocity(rotor_rpm) => {
let want = expected[i] * GEAR;
assert!(
(rotor_rpm - want).abs() < 1e-2,
"wheel {i}: rotor_rpm {rotor_rpm} != {want}"
);
}
ref cmd => panic!("wheel {i} expected Velocity, got {cmd:?}"),
}
}
handle.abort();
}
/// 反馈未到齐时必须全部停用，不得下发速度。
#[tokio::test(start_paused = true)]
async fn disables_wheels_until_all_feedback_seen() {
let kinematics = Mecanum::new(0.2, 0.25, 0.0762);
let ready = mock_wheel(1);
let missing = MockMotor::new(2); // 无反馈
let mocks_ready = [ready.clone(), mock_wheel(3), mock_wheel(4)];
let _ = mocks_ready;
let wheels =
wrap_wheels([ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)], [GEAR; 4]);
let ctrl =
ChassisController::new(kinematics, wheels, (3.0, 3.0, 6.0), Duration::from_millis(200));
let (tx, rx) = watch::channel(Twist::ZERO);
let handle = ctrl.spawn(rx, Duration::from_millis(1));
tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
tokio::time::sleep(Duration::from_millis(5)).await;
assert_eq!(ready.last_command(), Command::Disabled);
assert_eq!(missing.last_command(), Command::Disabled);
handle.abort();
}
}
crates/chassisd/src/lib.rs
-20
View File
@@ -1,20 +0,0 @@
//! `chassisd` 库部分：底盘控制循环与指令源抽象。
//!
//! 配置 schema 已独立为 [`chassis_config`]（守护进程与整定工具共用一份
//! 定义），这里以 `config` 别名重新导出，保持 `chassisd::config::Config`
//! 这一历史路径可用。
//!
//! 二进制入口见 `src/main.rs`。
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
pub use chassis_config as config;
pub mod controller;
pub mod source;
pub use chassis_config::{
CanConfig, ChassisConfig, Config, ConfigError, FunctionBusConfig, MecanumConfig, MotorConfig,
ProtocolFamily,
};
pub use controller::{ChassisController, wrap_wheels};
pub use source::{ChannelSource, TwistSource, ZeroSource, pump_source};
crates/chassisd/src/main.rs
+980 -13
View File
File diff suppressed because it is too large Load Diff
crates/chassisd/src/source.rs
-81
View File
@@ -1,81 +0,0 @@
//! 上层速度指令源（twist source）。
//!
//! 守护进程通过 [`watch`] 通道把指令交给控制循环；指令源在独立任务中
//! 持续产出 twist。遥控器（DR16）、自主导航等后续接入时各自实现
//! [`TwistSource`] 即可。
use std::future::Future;
use mecanum::Twist;
use tokio::sync::{mpsc, watch};
/// 速度指令源。
pub trait TwistSource: Send {
/// 取下一条指令；`None` 表示源关闭。
fn next_twist(&mut self) -> impl Future<Output = Option<Twist>> + Send;
}
/// 恒零占位源：永不产出（watch 保持初值 `Twist::ZERO`，指令超时逻辑兜底）。
pub struct ZeroSource;
impl TwistSource for ZeroSource {
async fn next_twist(&mut self) -> Option<Twist> {
std::future::pending().await
}
}
/// 通道指令源（测试 / 简单进程内注入用）。
pub struct ChannelSource {
rx: mpsc::Receiver<Twist>,
}
impl ChannelSource {
/// 由 mpsc 接收端构造。
#[must_use]
pub const fn new(rx: mpsc::Receiver<Twist>) -> Self {
Self { rx }
}
/// 创建一对（发送端, 源）。
#[must_use]
pub fn pair(bound: usize) -> (mpsc::Sender<Twist>, Self) {
let (tx, rx) = mpsc::channel(bound);
(tx, Self { rx })
}
}
impl TwistSource for ChannelSource {
async fn next_twist(&mut self) -> Option<Twist> {
self.rx.recv().await
}
}
/// 把任意指令源持续泵入 watch 通道（源关闭或接收端关闭时退出）。
pub async fn pump_source(mut source: impl TwistSource, tx: watch::Sender<Twist>) {
while let Some(twist) = source.next_twist().await {
if tx.send(twist).is_err() {
break;
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[tokio::test(start_paused = true)]
async fn channel_source_pumps_into_watch() {
let (tx, source) = ChannelSource::pair(8);
let (watch_tx, mut watch_rx) = watch::channel(Twist::ZERO);
let pump = tokio::spawn(pump_source(source, watch_tx));
tx.send(Twist { vx: 1.0, vy: 0.0, omega: 0.0 }).await.unwrap();
tokio::time::sleep(std::time::Duration::from_millis(1)).await;
assert_eq!(*watch_rx.borrow_and_update(), Twist { vx: 1.0, vy: 0.0, omega: 0.0 });
// 源关闭 → 泵任务退出。
drop(tx);
pump.await.unwrap();
assert!((watch_rx.borrow().vx - 1.0).abs() < 1e-6); // watch 保持最后值
}
}
crates/chassisd/tests/daemon.rs
-170
View File
@@ -1,170 +0,0 @@
//! 守护进程端到端测试：`--mock` 同构栈（配置 → EscBus → 通道 → 仿真电机
//! → 控制循环 → 麦轮运动学），全平台可跑。
#![allow(clippy::unwrap_used, clippy::expect_used)]
use std::time::Duration;
use chassisd::config::Config;
use chassisd::controller::{ChassisController, wrap_wheels};
use dji_esc::{CanLink, EscBus, EscBusHandle};
use dji_esc_sim::spawn_plant;
use mecanum::Twist;
use motor_core::MotorFeedback;
use tokio::sync::watch;
use tokio::time::sleep;
/// 搭建与 `chassisd --mock` 完全同构的软件栈。
///
/// `command_timeout_ms` 由调用方指定：稳态测试用大值（模拟持续刷新的
/// 指令源），超时测试用配置原值。
fn stack(config: &Config, command_timeout_ms: u64) -> (EscBusHandle, watch::Sender<Twist>) {
let (bus_side, plant_side) = CanLink::pair(512);
let esc_ids: Vec<u8> = config.motors.iter().map(|m| m.esc_id).collect();
spawn_plant(plant_side, &esc_ids, config.bus_options().period);
let bus = EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options()).unwrap();
let (twist_tx, twist_rx) = watch::channel(Twist::ZERO);
let motors: [dji_esc::EscMotor; 4] =
bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors"));
let gears: [f32; 4] = config
.motors
.iter()
.map(|m| m.gear_ratio)
.collect::<Vec<_>>()
.try_into()
.unwrap_or_else(|_| unreachable!("exactly 4 motors"));
let controller = ChassisController::new(
config.kinematics(),
wrap_wheels(motors, gears),
(config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega),
Duration::from_millis(command_timeout_ms),
);
controller.spawn(twist_rx, config.bus_options().period);
(bus, twist_tx)
}
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()
}
fn wheel_rpms(bus: &EscBusHandle) -> [f32; 4] {
bus.motors()
.iter()
.map(|m| m.state().rotor_speed_rpm / m.config().gear_ratio)
.collect::<Vec<_>>()
.try_into()
.unwrap()
}
/// mock 默认底盘（r = 0.0762 m）上 1 m/s 对应的轮转速。
const RPM_PER_MPS: f32 = 60.0 / (std::f32::consts::TAU * 0.0762);
#[tokio::test(start_paused = true)]
async fn forward_command_drives_all_wheels_equally() {
let config = Config::mock_default();
let (bus, twist_tx) = stack(&config, 10_000);
twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
let target = RPM_PER_MPS;
let ok = wait_until(3000, || {
wheel_rpms(&bus).iter().all(|&rpm| (rpm - target).abs() < 0.05 * target)
})
.await;
let rpm = wheel_rpms(&bus);
assert!(ok, "未收敛到 {target} rpm: {rpm:?}");
// 停车：指令归零后各轮减速到停。
twist_tx.send_replace(Twist::ZERO);
let stopped = wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
let rpm = wheel_rpms(&bus);
assert!(stopped, "未停车: {rpm:?}");
bus.shutdown().await;
}
#[tokio::test(start_paused = true)]
async fn rotation_command_respects_wheel_signs() {
let config = Config::mock_default();
let (bus, twist_tx) = stack(&config, 10_000);
twist_tx.send_replace(Twist { omega: 1.0, ..Twist::ZERO });
// a·ω = (0.2 + 0.25) m/s 的轮线速度。
let target = 0.45 * RPM_PER_MPS;
let ok = wait_until(3000, || {
let rpm = wheel_rpms(&bus);
// 逆时针：左侧（FL、RL）为负，右侧（FR、RR）为正。
[
(rpm[0] + target).abs(),
(rpm[1] - target).abs(),
(rpm[2] + target).abs(),
(rpm[3] - target).abs(),
]
.iter()
.all(|e| *e < 0.05 * target)
})
.await;
let rpm = wheel_rpms(&bus);
assert!(ok, "旋转轮序符号错误, 期望 ±{target}: {rpm:?}");
bus.shutdown().await;
}
#[tokio::test(start_paused = true)]
async fn command_timeout_stops_chassis() {
let config = Config::mock_default();
// 指令超时使用配置原值（200ms）。
let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms);
// 只发一次指令，随后沉默 → 超过 command_timeout（200ms）后应停车。
twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
let moving =
wait_until(1000, || wheel_rpms(&bus).iter().any(|rpm| rpm.abs() > 0.2 * RPM_PER_MPS)).await;
assert!(moving, "指令后底盘应当运动");
sleep(Duration::from_millis(400)).await; // 超过 200ms 超时
let stopped = wait_until(1000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
let rpm = wheel_rpms(&bus);
assert!(stopped, "指令超时后未停车: {rpm:?}");
bus.shutdown().await;
}
#[tokio::test(start_paused = true)]
async fn speed_limits_scale_motion() {
let mut config = Config::mock_default();
config.chassis.max_vx = 1.0; // 限速 1 m/s
let (bus, twist_tx) = stack(&config, 10_000);
twist_tx.send_replace(Twist {
vx: 3.0, // 超限指令
vy: 2.0,
omega: 0.0,
});
// 限幅按比例收缩：(3, 2) → (1, 2/3)。四轮 = vx ∓ vy：
// FL/RR = (1 - 2/3) m/s，FR/RL = (1 + 2/3) m/s。
let diag_slow = (1.0 - 2.0 / 3.0) * RPM_PER_MPS;
let diag_fast = (1.0 + 2.0 / 3.0) * RPM_PER_MPS;
let tol = 0.08 * diag_fast;
let ok = wait_until(3000, || {
let rpm = wheel_rpms(&bus);
[
(rpm[0] - diag_slow).abs(),
(rpm[1] - diag_fast).abs(),
(rpm[2] - diag_fast).abs(),
(rpm[3] - diag_slow).abs(),
]
.iter()
.all(|e| *e < tol)
})
.await;
let rpm = wheel_rpms(&bus);
assert!(ok, "限幅后未收敛到 [{diag_slow}, {diag_fast}, {diag_fast}, {diag_slow}] rpm: {rpm:?}");
bus.shutdown().await;
}