refactor: enforce layered crates and Rust audit gates
This commit is contained in:
Horldsence
@@ -0,0 +1,25 @@
|
||||
[package]
|
||||
name = "esc-bus"
|
||||
description = "Asynchronous ESC frame pump, state, leases, and safety"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
motor-protocol = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
approx = { workspace = true }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "device/esc"
|
||||
@@ -0,0 +1,461 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! Generic asynchronous ESC frame pump, state, leases, and safety.
|
||||
|
||||
use std::marker::PhantomData;
|
||||
use std::sync::{Arc, Mutex, MutexGuard};
|
||||
use std::time::Duration;
|
||||
|
||||
use can::{CanFrame, CanLink};
|
||||
use motor::{
|
||||
Command, CurrentEscDrive, Disable, EffortDrive, EffortPolicy, MotorFeedback, MotorSpec,
|
||||
MotorState, PolicyInput, PolicySample, PositionDrive, VelocityDrive,
|
||||
};
|
||||
use motor_protocol::{EffortScale, FeedbackFields, MotorProtocol, TurnTracker};
|
||||
use thiserror::Error;
|
||||
use tokio::sync::{mpsc, watch};
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{Instant, Interval, MissedTickBehavior};
|
||||
use tracing::{debug, info, warn};
|
||||
|
||||
fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
|
||||
mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
|
||||
}
|
||||
|
||||
/// Configuration for one typed motor.
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct EscConfig<S: MotorSpec = motor::Gm3508> {
|
||||
pub name: String,
|
||||
pub esc_id: u8,
|
||||
pub gear_ratio: f32,
|
||||
pub direction: f32,
|
||||
pub current_limit: i16,
|
||||
marker: PhantomData<S>,
|
||||
}
|
||||
|
||||
impl<S: MotorSpec> EscConfig<S> {
|
||||
#[must_use]
|
||||
pub fn new(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
|
||||
Self {
|
||||
name: name.into(),
|
||||
esc_id,
|
||||
gear_ratio: S::GEAR_RATIO,
|
||||
direction,
|
||||
current_limit: S::scale().max_raw() / 4 * 3,
|
||||
marker: PhantomData,
|
||||
}
|
||||
}
|
||||
|
||||
fn validate(&self) -> Result<(), String> {
|
||||
if <S::Protocol as MotorProtocol>::command_frame_id(self.esc_id).is_none() {
|
||||
return Err(format!("esc_id {} out of range 1..=8", self.esc_id));
|
||||
}
|
||||
if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) {
|
||||
return Err(format!("gear_ratio must be > 0, got {}", self.gear_ratio));
|
||||
}
|
||||
let dir_bits = self.direction.to_bits();
|
||||
if dir_bits != 1.0f32.to_bits() && dir_bits != (-1.0f32).to_bits() {
|
||||
return Err(format!("direction must be +1.0 or -1.0, got {}", self.direction));
|
||||
}
|
||||
if !(1..=S::scale().max_raw()).contains(&self.current_limit) {
|
||||
return Err(format!(
|
||||
"current_limit {} out of range 1..={}",
|
||||
self.current_limit,
|
||||
S::scale().max_raw()
|
||||
));
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl EscConfig<motor::Gm3508> {
|
||||
#[must_use]
|
||||
pub fn gm3508(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
|
||||
let mut cfg = Self::new(name, esc_id, direction);
|
||||
cfg.current_limit = 12_000;
|
||||
cfg
|
||||
}
|
||||
}
|
||||
|
||||
impl EscConfig<motor::M2006> {
|
||||
#[must_use]
|
||||
pub fn m2006(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
|
||||
let mut cfg = Self::new(name, esc_id, direction);
|
||||
cfg.current_limit = 8_000;
|
||||
cfg
|
||||
}
|
||||
}
|
||||
|
||||
/// Bus timing and lease options.
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct BusOptions {
|
||||
pub period: Duration,
|
||||
pub feedback_timeout: Duration,
|
||||
pub command_timeout: Duration,
|
||||
}
|
||||
|
||||
impl Default for BusOptions {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
period: Duration::from_millis(1),
|
||||
feedback_timeout: Duration::from_millis(100),
|
||||
command_timeout: Duration::from_millis(50),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Startup error.
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Error)]
|
||||
pub enum EscBusError {
|
||||
#[error("duplicate ESC id {0}")]
|
||||
DuplicateEscId(u8),
|
||||
#[error("invalid config for motor {name}: {reason}")]
|
||||
InvalidConfig { name: String, reason: String },
|
||||
#[error("no motors configured")]
|
||||
NoMotors,
|
||||
}
|
||||
|
||||
struct MotorShared<S: MotorSpec> {
|
||||
cfg: EscConfig<S>,
|
||||
state: Mutex<MotorState>,
|
||||
cmd: Mutex<Command>,
|
||||
cmd_at: Mutex<Option<Instant>>,
|
||||
turn: Mutex<TurnTracker>,
|
||||
last_feedback: Mutex<Option<Instant>>,
|
||||
offline_warned: std::sync::atomic::AtomicBool,
|
||||
}
|
||||
|
||||
impl<S: MotorSpec> MotorShared<S> {
|
||||
fn new(cfg: EscConfig<S>) -> Self {
|
||||
Self {
|
||||
cfg,
|
||||
state: Mutex::new(MotorState::default()),
|
||||
cmd: Mutex::new(Command::Disabled),
|
||||
cmd_at: Mutex::new(None),
|
||||
turn: Mutex::new(TurnTracker::new()),
|
||||
last_feedback: Mutex::new(None),
|
||||
offline_warned: std::sync::atomic::AtomicBool::new(false),
|
||||
}
|
||||
}
|
||||
|
||||
fn store_command(&self, cmd: Command) {
|
||||
*lock(&self.cmd) = cmd;
|
||||
*lock(&self.cmd_at) = Some(Instant::now());
|
||||
}
|
||||
|
||||
fn apply_feedback<F: FeedbackFields>(&self, fb: &F, now: Instant) {
|
||||
let esc_rev = lock(&self.turn).update(fb.angle_raw());
|
||||
let dir = self.cfg.direction;
|
||||
{
|
||||
let mut st = lock(&self.state);
|
||||
st.rotor_angle_raw = fb.angle_raw();
|
||||
st.rotor_revolutions = dir * esc_rev;
|
||||
st.rotor_speed_rpm = dir * f32::from(fb.speed_rpm());
|
||||
st.effort_raw = (dir * f32::from(fb.current_raw())) as i16;
|
||||
st.temperature_c = fb.temperature_c();
|
||||
st.feedback_count = st.feedback_count.saturating_add(1);
|
||||
}
|
||||
*lock(&self.last_feedback) = Some(now);
|
||||
self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed);
|
||||
}
|
||||
|
||||
fn sample(
|
||||
&self,
|
||||
feedback_timeout: Duration,
|
||||
command_timeout: Duration,
|
||||
now: Instant,
|
||||
) -> PolicySample {
|
||||
let online = matches!(
|
||||
*lock(&self.last_feedback),
|
||||
Some(t) if now.saturating_duration_since(t) <= feedback_timeout
|
||||
);
|
||||
if !online && !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) {
|
||||
warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero output");
|
||||
}
|
||||
let fresh = matches!(
|
||||
*lock(&self.cmd_at),
|
||||
Some(t) if now.saturating_duration_since(t) <= command_timeout
|
||||
);
|
||||
let command = if online && fresh { *lock(&self.cmd) } else { Command::Disabled };
|
||||
PolicySample { id: self.cfg.esc_id, state: *lock(&self.state), command, online }
|
||||
}
|
||||
}
|
||||
|
||||
/// Typed motor handle.
|
||||
pub struct EscMotor<S: MotorSpec = motor::Gm3508> {
|
||||
shared: Arc<MotorShared<S>>,
|
||||
}
|
||||
|
||||
impl<S: MotorSpec> Clone for EscMotor<S> {
|
||||
fn clone(&self) -> Self {
|
||||
Self { shared: Arc::clone(&self.shared) }
|
||||
}
|
||||
}
|
||||
|
||||
impl<S: MotorSpec> EscMotor<S> {
|
||||
#[must_use]
|
||||
pub fn config(&self) -> &EscConfig<S> {
|
||||
&self.shared.cfg
|
||||
}
|
||||
#[must_use]
|
||||
pub fn name(&self) -> &str {
|
||||
&self.shared.cfg.name
|
||||
}
|
||||
#[must_use]
|
||||
pub fn command(&self) -> Command {
|
||||
*lock(&self.shared.cmd)
|
||||
}
|
||||
#[must_use]
|
||||
pub fn last_feedback(&self) -> Option<Instant> {
|
||||
*lock(&self.shared.last_feedback)
|
||||
}
|
||||
}
|
||||
|
||||
impl<S: MotorSpec> MotorFeedback for EscMotor<S> {
|
||||
fn id(&self) -> u8 {
|
||||
self.shared.cfg.esc_id
|
||||
}
|
||||
fn state(&self) -> MotorState {
|
||||
*lock(&self.shared.state)
|
||||
}
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
Some(Instant::now().saturating_duration_since(*lock(&self.shared.last_feedback).as_ref()?))
|
||||
}
|
||||
}
|
||||
|
||||
impl<S: MotorSpec> VelocityDrive for EscMotor<S> {
|
||||
fn set_velocity(&self, rotor_rpm: f32) {
|
||||
self.shared.store_command(Command::Velocity(rotor_rpm));
|
||||
}
|
||||
}
|
||||
|
||||
impl<S: MotorSpec> EffortDrive for EscMotor<S> {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
self.shared.store_command(Command::EffortRaw(raw));
|
||||
}
|
||||
}
|
||||
|
||||
impl<S: MotorSpec> PositionDrive for EscMotor<S> {
|
||||
fn set_position(&self, rotor_rev: f32) {
|
||||
self.shared.store_command(Command::Position(rotor_rev));
|
||||
}
|
||||
}
|
||||
|
||||
impl<S: MotorSpec> Disable for EscMotor<S> {
|
||||
fn disable(&self) {
|
||||
self.shared.store_command(Command::Disabled);
|
||||
}
|
||||
}
|
||||
|
||||
/// Generic current-frame ESC bus.
|
||||
pub struct EscBus<S: MotorSpec = motor::Gm3508>(PhantomData<S>);
|
||||
|
||||
impl<S: CurrentEscDrive> EscBus<S> {
|
||||
/// Start receive and transmit tasks.
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// Returns an error when configuration is empty, duplicated, or invalid.
|
||||
pub fn spawn<P: EffortPolicy>(
|
||||
link: CanLink,
|
||||
configs: Vec<EscConfig<S>>,
|
||||
options: BusOptions,
|
||||
policy: P,
|
||||
) -> Result<EscBusHandle<P, S>, EscBusError> {
|
||||
if configs.is_empty() {
|
||||
return Err(EscBusError::NoMotors);
|
||||
}
|
||||
let mut motors = Vec::with_capacity(configs.len());
|
||||
let mut seen = std::collections::HashSet::new();
|
||||
for cfg in configs {
|
||||
cfg.validate()
|
||||
.map_err(|reason| EscBusError::InvalidConfig { name: cfg.name.clone(), reason })?;
|
||||
if !seen.insert(cfg.esc_id) {
|
||||
return Err(EscBusError::DuplicateEscId(cfg.esc_id));
|
||||
}
|
||||
motors.push(EscMotor { shared: Arc::new(MotorShared::new(cfg)) });
|
||||
}
|
||||
|
||||
let (shutdown_tx, shutdown_rx) = watch::channel(false);
|
||||
let CanLink { outgoing_tx, incoming_rx } = link;
|
||||
let motors = Arc::new(motors);
|
||||
let policy = Arc::new(Mutex::new(policy));
|
||||
let rx_handle =
|
||||
tokio::spawn(rx_loop::<S>(incoming_rx, Arc::clone(&motors), shutdown_rx.clone()));
|
||||
let tx_handle = tokio::spawn(tx_loop::<S, P>(
|
||||
outgoing_tx,
|
||||
Arc::clone(&motors),
|
||||
Arc::clone(&policy),
|
||||
options,
|
||||
shutdown_rx,
|
||||
));
|
||||
Ok(EscBusHandle {
|
||||
motors,
|
||||
policy,
|
||||
shutdown_tx,
|
||||
tasks: vec![rx_handle, tx_handle],
|
||||
marker: PhantomData,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
/// Control handle for a running bus.
|
||||
pub struct EscBusHandle<P, S: MotorSpec = motor::Gm3508> {
|
||||
motors: Arc<Vec<EscMotor<S>>>,
|
||||
policy: Arc<Mutex<P>>,
|
||||
shutdown_tx: watch::Sender<bool>,
|
||||
tasks: Vec<JoinHandle<()>>,
|
||||
marker: PhantomData<S>,
|
||||
}
|
||||
|
||||
impl<P: EffortPolicy, S: CurrentEscDrive> EscBusHandle<P, S> {
|
||||
#[must_use]
|
||||
pub fn motor(&self, esc_id: u8) -> Option<&EscMotor<S>> {
|
||||
self.motors.iter().find(|m| m.id() == esc_id)
|
||||
}
|
||||
|
||||
#[must_use]
|
||||
pub fn motors(&self) -> &[EscMotor<S>] {
|
||||
&self.motors
|
||||
}
|
||||
|
||||
pub fn with_policy<R>(&self, update: impl FnOnce(&mut P) -> R) -> R {
|
||||
update(&mut lock(&self.policy))
|
||||
}
|
||||
|
||||
pub async fn shutdown(self) {
|
||||
let _ = self.shutdown_tx.send(true);
|
||||
for task in self.tasks {
|
||||
let _ = task.await;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
async fn rx_loop<S: CurrentEscDrive>(
|
||||
mut incoming: mpsc::Receiver<CanFrame>,
|
||||
motors: Arc<Vec<EscMotor<S>>>,
|
||||
mut shutdown: watch::Receiver<bool>,
|
||||
) {
|
||||
loop {
|
||||
let frame = tokio::select! {
|
||||
_ = shutdown.changed() => break,
|
||||
frame = incoming.recv() => match frame {
|
||||
Some(frame) => frame,
|
||||
None => break,
|
||||
},
|
||||
};
|
||||
match <S::Protocol as MotorProtocol>::decode_feedback(&frame) {
|
||||
Ok((esc_id, fb)) => {
|
||||
if let Some(motor) = motors.iter().find(|m| m.id() == esc_id) {
|
||||
motor.shared.apply_feedback(&fb, Instant::now());
|
||||
}
|
||||
}
|
||||
Err(e) => debug!(error = %e, id = frame.id, "ignore non-feedback frame"),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
async fn tx_loop<S: CurrentEscDrive, P: EffortPolicy>(
|
||||
outgoing: mpsc::Sender<CanFrame>,
|
||||
motors: Arc<Vec<EscMotor<S>>>,
|
||||
policy: Arc<Mutex<P>>,
|
||||
options: BusOptions,
|
||||
mut shutdown: watch::Receiver<bool>,
|
||||
) {
|
||||
let nominal_dt = options.period.as_secs_f32();
|
||||
let mut ticker = tick_every(options.period);
|
||||
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;
|
||||
let dt = raw_dt.clamp(nominal_dt * 0.5, nominal_dt * 4.0);
|
||||
let mut samples: [PolicySample; 8] = std::array::from_fn(|index| PolicySample {
|
||||
id: u8::try_from(index + 1).unwrap_or(0),
|
||||
state: MotorState::default(),
|
||||
command: Command::Disabled,
|
||||
online: false,
|
||||
});
|
||||
for motor in motors.iter() {
|
||||
let sample =
|
||||
motor.shared.sample(options.feedback_timeout, options.command_timeout, now);
|
||||
if let Some(slot) = samples.get_mut(usize::from(sample.id.saturating_sub(1))) {
|
||||
*slot = sample;
|
||||
}
|
||||
}
|
||||
let efforts = lock(&policy).step(&PolicyInput { samples, dt });
|
||||
for group_start in [1u8, 5u8] {
|
||||
let Some(frame_id) = <S::Protocol as MotorProtocol>::command_frame_id(group_start)
|
||||
else {
|
||||
continue;
|
||||
};
|
||||
let group: Vec<&EscMotor<S>> = motors
|
||||
.iter()
|
||||
.filter(|m| (group_start..group_start + 4).contains(&m.id()))
|
||||
.collect();
|
||||
if group.is_empty() {
|
||||
continue;
|
||||
}
|
||||
let mut commands = [S::Protocol::command_from_raw(0); 4];
|
||||
for motor in &group {
|
||||
let slot = usize::from(motor.id() - group_start);
|
||||
let policy_index = usize::from(motor.id().saturating_sub(1));
|
||||
let physical = efforts.get(policy_index).copied().unwrap_or(0);
|
||||
let directed = (f32::from(physical) * motor.shared.cfg.direction) as i16;
|
||||
let limited =
|
||||
directed.clamp(-motor.shared.cfg.current_limit, motor.shared.cfg.current_limit);
|
||||
commands[slot] = S::Protocol::command_from_raw(S::scale().clamp_raw(limited));
|
||||
}
|
||||
if outgoing.send(S::Protocol::encode_group(frame_id, commands)).await.is_err() {
|
||||
info!("can link closed, tx loop exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn tick_every(period: Duration) -> Interval {
|
||||
let mut ticker = tokio::time::interval(period);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
ticker
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor::{Gm3508, M2006};
|
||||
|
||||
#[test]
|
||||
fn typed_defaults_are_sane() {
|
||||
let cfg = EscConfig::<Gm3508>::gm3508("front_left", 1, -1.0);
|
||||
assert!((cfg.gear_ratio - 19.2).abs() < 0.01);
|
||||
assert_eq!(cfg.current_limit, 12_000);
|
||||
assert!(cfg.validate().is_ok());
|
||||
|
||||
let cfg = EscConfig::<M2006>::m2006("feeder", 5, 1.0);
|
||||
assert!((cfg.gear_ratio - 36.0).abs() < 0.01);
|
||||
assert!(cfg.validate().is_ok());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn config_validate_rejects_bad_values() {
|
||||
let mut cfg = EscConfig::<Gm3508>::gm3508("m", 1, 1.0);
|
||||
cfg.esc_id = 0;
|
||||
assert!(cfg.validate().is_err());
|
||||
cfg.esc_id = 5;
|
||||
cfg.gear_ratio = 0.0;
|
||||
assert!(cfg.validate().is_err());
|
||||
cfg.gear_ratio = 19.2;
|
||||
cfg.direction = 0.5;
|
||||
assert!(cfg.validate().is_err());
|
||||
cfg.direction = -1.0;
|
||||
cfg.current_limit = 0;
|
||||
assert!(cfg.validate().is_err());
|
||||
cfg.current_limit = 16_385;
|
||||
assert!(cfg.validate().is_err());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
[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 }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "device/imu"
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
[package]
|
||||
name = "motor"
|
||||
description = "Protocol-independent motor device contract and registry"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
motor-protocol = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "device/motor"
|
||||
@@ -0,0 +1,912 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! Protocol-independent motor device contracts.
|
||||
|
||||
pub mod traits {
|
||||
//! 能力面 trait：小而正交，按硬件事实拆分。
|
||||
//!
|
||||
//! 消费者只声明自己用到的能力（静态分发，无 `dyn`）：
|
||||
//! - 底盘轮 → [`VelocityDrive`] + [`MotorFeedback`]
|
||||
//! - 云台 → [`PositionDrive`] + [`EffortDrive`]（+ 上层 [`crate::HeadingSource`]）
|
||||
//! - 拨弹 → [`VelocityDrive`] + [`PositionDrive`]
|
||||
//! - 台架 → [`VelocityDrive`] + [`MotorFeedback`] + [`Disable`]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use crate::{Command, MotorState};
|
||||
|
||||
/// 读反馈。所有可观察设备的最小面。
|
||||
pub trait MotorFeedback: Send + Sync {
|
||||
/// 控制器内的电机 ID（对 DJI 电调即电调 ID）。
|
||||
fn id(&self) -> u8;
|
||||
|
||||
/// 最新反馈快照。
|
||||
fn state(&self) -> MotorState;
|
||||
|
||||
/// 距最近一帧反馈的时长；`None` 表示从未收到。
|
||||
///
|
||||
/// 用于在线判定（反馈新鲜度），与 [`MotorState::has_feedback`]
|
||||
/// （历史上是否收到过）区分：上电门控之后运行中掉线要看本方法。
|
||||
fn feedback_age(&self) -> Option<Duration>;
|
||||
}
|
||||
|
||||
/// 开环控制量（协议 raw：ESC=电流，GM6020=电压）。
|
||||
pub trait EffortDrive: Send + Sync {
|
||||
/// 直接下发开环 raw（协议量程，物理方向）。
|
||||
fn set_effort_raw(&self, raw: i16);
|
||||
}
|
||||
|
||||
/// 速度目标（转子 rpm，物理方向）；由驱动层速度环跟踪。
|
||||
pub trait VelocityDrive: Send + Sync {
|
||||
/// 设定目标转子转速。
|
||||
fn set_velocity(&self, rotor_rpm: f32);
|
||||
}
|
||||
|
||||
/// 位置目标（转子多圈，圈；物理方向）。
|
||||
pub trait PositionDrive: Send + Sync {
|
||||
/// 设定目标转子多圈位置。
|
||||
fn set_position(&self, rotor_rev: f32);
|
||||
}
|
||||
|
||||
/// 停用（零输出）。
|
||||
pub trait Disable: Send + Sync {
|
||||
/// 停用电机。
|
||||
fn disable(&self);
|
||||
}
|
||||
|
||||
/// 便利别名（blanket impl，零成本）。实现方**不必**实现它。
|
||||
pub trait Motor: MotorFeedback + VelocityDrive + EffortDrive + Disable {}
|
||||
|
||||
impl<T> Motor for T where T: MotorFeedback + VelocityDrive + EffortDrive + Disable {}
|
||||
|
||||
/// One motor's data for one control-policy evaluation.
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct PolicySample {
|
||||
/// ESC identifier.
|
||||
pub id: u8,
|
||||
/// Latest direction-corrected physical feedback.
|
||||
pub state: MotorState,
|
||||
/// Latest command lease value.
|
||||
pub command: Command,
|
||||
/// Whether fresh feedback is available.
|
||||
pub online: bool,
|
||||
}
|
||||
|
||||
/// Snapshot passed to an external effort policy once per bus tick.
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct PolicyInput {
|
||||
/// Samples indexed by ESC id minus one.
|
||||
pub samples: [PolicySample; 8],
|
||||
/// Measured control period in seconds.
|
||||
pub dt: f32,
|
||||
}
|
||||
|
||||
/// Computes raw physical effort from motor commands and feedback.
|
||||
///
|
||||
/// `esc-bus` owns transport and safety but deliberately has no control-law
|
||||
/// dependency. Control crates implement this trait and the application composes
|
||||
/// the two sides.
|
||||
pub trait EffortPolicy: Send + 'static {
|
||||
/// Return physical effort raw values indexed by ESC id minus one.
|
||||
fn step(&mut self, input: &PolicyInput) -> [i16; 8];
|
||||
/// Clear controller state after an offline event or command-mode change.
|
||||
fn reset(&mut self, esc_id: u8);
|
||||
}
|
||||
}
|
||||
|
||||
pub mod state {
|
||||
//! 电机反馈快照（协议无关，方向修正后的物理量）。
|
||||
//!
|
||||
//! 只含**转子轴**物理量——减速比 / 输出轴换算由 [`crate::gearbox::OutputShaft`]
|
||||
//! 或上层完成，不在此泄漏底盘语义。
|
||||
|
||||
/// 电机反馈快照。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct MotorState {
|
||||
/// 最近一帧反馈的转子机械原始角（编码器计数，量程由具体协议决定）。
|
||||
pub rotor_angle_raw: u16,
|
||||
/// 连续累计转子角（多圈回绕处理，单位：圈；随时间单调于转动方向）。
|
||||
pub rotor_revolutions: f32,
|
||||
/// 转子转速（rpm，物理方向）。
|
||||
pub rotor_speed_rpm: f32,
|
||||
/// 开环控制量反馈 raw（ESC=转矩电流，GM6020=电流相关量；物理方向）。
|
||||
pub effort_raw: i16,
|
||||
/// 电机温度（℃）。
|
||||
pub temperature_c: i8,
|
||||
/// 已收到的反馈帧总数；`0` 表示尚未收到任何反馈（上电门控用）。
|
||||
pub feedback_count: u64,
|
||||
}
|
||||
|
||||
impl MotorState {
|
||||
/// 是否已收到过反馈（上电后首帧门控）。
|
||||
#[must_use]
|
||||
pub fn has_feedback(&self) -> bool {
|
||||
self.feedback_count > 0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod command {
|
||||
//! 驱动层命令寄存器（最后写入胜出）。
|
||||
|
||||
/// 发给驱动层的电机命令。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub enum Command {
|
||||
/// 停用：目标清零，下发 0 输出。
|
||||
Disabled,
|
||||
/// 直接下发开环 raw（协议量程，物理方向），绕过闭环。
|
||||
/// ESC = 电流 raw；GM6020 = 电压 raw。
|
||||
EffortRaw(i16),
|
||||
/// 速度环跟踪目标转子转速（rpm，物理方向）。
|
||||
Velocity(f32),
|
||||
/// 位置环跟踪目标转子多圈位置（圈，物理方向）。
|
||||
Position(f32),
|
||||
}
|
||||
|
||||
/// 命令模式标签（不含设定值）——用于切换时复位控制器。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum CommandMode {
|
||||
/// 停用。
|
||||
Disabled,
|
||||
/// 开环 raw。
|
||||
EffortRaw,
|
||||
/// 速度环。
|
||||
Velocity,
|
||||
/// 位置环。
|
||||
Position,
|
||||
}
|
||||
|
||||
impl Command {
|
||||
/// 模式标签。
|
||||
#[must_use]
|
||||
pub const fn mode(self) -> CommandMode {
|
||||
match self {
|
||||
Self::Disabled => CommandMode::Disabled,
|
||||
Self::EffortRaw(_) => CommandMode::EffortRaw,
|
||||
Self::Velocity(_) => CommandMode::Velocity,
|
||||
Self::Position(_) => CommandMode::Position,
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod gearbox {
|
||||
//! 输出轴单位适配器：减速比换算。
|
||||
//!
|
||||
//! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。
|
||||
//! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive};
|
||||
|
||||
/// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。
|
||||
pub struct OutputShaft<T> {
|
||||
inner: T,
|
||||
gear_ratio: f32,
|
||||
}
|
||||
|
||||
impl<T> OutputShaft<T> {
|
||||
/// 包装底层电机。`gear_ratio` = 转子 : 输出轴（> 0）。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// `gear_ratio` 非有限或 ≤ 0 时 panic。
|
||||
#[must_use]
|
||||
pub fn new(inner: T, gear_ratio: f32) -> Self {
|
||||
assert!(
|
||||
gear_ratio.is_finite() && gear_ratio > 0.0,
|
||||
"gear_ratio must be > 0, got {gear_ratio}"
|
||||
);
|
||||
Self { inner, gear_ratio }
|
||||
}
|
||||
|
||||
/// 减速比。
|
||||
#[must_use]
|
||||
pub fn gear_ratio(&self) -> f32 {
|
||||
self.gear_ratio
|
||||
}
|
||||
|
||||
/// 底层电机引用。
|
||||
#[must_use]
|
||||
pub fn inner(&self) -> &T {
|
||||
&self.inner
|
||||
}
|
||||
|
||||
/// 转子量 → 输出轴量。
|
||||
#[must_use]
|
||||
pub fn to_output(&self, rotor: f32) -> f32 {
|
||||
rotor / self.gear_ratio
|
||||
}
|
||||
|
||||
/// 输出轴量 → 转子量。
|
||||
#[must_use]
|
||||
pub fn to_rotor(&self, output: f32) -> f32 {
|
||||
output * self.gear_ratio
|
||||
}
|
||||
|
||||
/// 由反馈快照推算输出轴转速（rpm）。
|
||||
#[must_use]
|
||||
pub fn output_speed_rpm(&self, state: &MotorState) -> f32 {
|
||||
state.rotor_speed_rpm / self.gear_ratio
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: MotorFeedback> MotorFeedback for OutputShaft<T> {
|
||||
fn id(&self) -> u8 {
|
||||
self.inner.id()
|
||||
}
|
||||
|
||||
fn state(&self) -> MotorState {
|
||||
// 只透传转子量；不伪造输出轴字段。
|
||||
self.inner.state()
|
||||
}
|
||||
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
self.inner.feedback_age()
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: VelocityDrive> VelocityDrive for OutputShaft<T> {
|
||||
/// `output_rpm` 为**输出轴**转速。
|
||||
fn set_velocity(&self, output_rpm: f32) {
|
||||
self.inner.set_velocity(self.to_rotor(output_rpm));
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: EffortDrive> EffortDrive for OutputShaft<T> {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
self.inner.set_effort_raw(raw);
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: PositionDrive> PositionDrive for OutputShaft<T> {
|
||||
/// `output_rev` 为**输出轴**圈数。
|
||||
fn set_position(&self, output_rev: f32) {
|
||||
self.inner.set_position(self.to_rotor(output_rev));
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: Disable> Disable for OutputShaft<T> {
|
||||
fn disable(&self) {
|
||||
self.inner.disable();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::Command;
|
||||
|
||||
struct Stub {
|
||||
cmd: std::sync::Mutex<Command>,
|
||||
}
|
||||
|
||||
impl Stub {
|
||||
fn new() -> Self {
|
||||
Self { cmd: std::sync::Mutex::new(Command::Disabled) }
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorFeedback for Stub {
|
||||
fn id(&self) -> u8 {
|
||||
1
|
||||
}
|
||||
fn state(&self) -> MotorState {
|
||||
MotorState::default()
|
||||
}
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
Some(Duration::ZERO)
|
||||
}
|
||||
}
|
||||
impl VelocityDrive for Stub {
|
||||
fn set_velocity(&self, rotor_rpm: f32) {
|
||||
*self.cmd.lock().unwrap() = Command::Velocity(rotor_rpm);
|
||||
}
|
||||
}
|
||||
impl EffortDrive for Stub {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
*self.cmd.lock().unwrap() = Command::EffortRaw(raw);
|
||||
}
|
||||
}
|
||||
impl PositionDrive for Stub {
|
||||
fn set_position(&self, rotor_rev: f32) {
|
||||
*self.cmd.lock().unwrap() = Command::Position(rotor_rev);
|
||||
}
|
||||
}
|
||||
impl Disable for Stub {
|
||||
fn disable(&self) {
|
||||
*self.cmd.lock().unwrap() = Command::Disabled;
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn velocity_scales_by_gear() {
|
||||
let shaft = OutputShaft::new(Stub::new(), 19.203);
|
||||
shaft.set_velocity(100.0);
|
||||
match *shaft.inner().cmd.lock().unwrap() {
|
||||
Command::Velocity(rpm) => assert!((rpm - 1920.3).abs() < 0.1, "{rpm}"),
|
||||
ref c => panic!("unexpected {c:?}"),
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn position_scales_by_gear() {
|
||||
let shaft = OutputShaft::new(Stub::new(), 36.0);
|
||||
shaft.set_position(2.0);
|
||||
match *shaft.inner().cmd.lock().unwrap() {
|
||||
Command::Position(rev) => assert!((rev - 72.0).abs() < 1e-3, "{rev}"),
|
||||
ref c => panic!("unexpected {c:?}"),
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn effort_passes_through() {
|
||||
let shaft = OutputShaft::new(Stub::new(), 10.0);
|
||||
shaft.set_effort_raw(-1234);
|
||||
assert_eq!(*shaft.inner().cmd.lock().unwrap(), Command::EffortRaw(-1234));
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[should_panic = "gear_ratio"]
|
||||
fn rejects_nonpositive_gear() {
|
||||
let _ = OutputShaft::new(Stub::new(), 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod heading {
|
||||
//! 航向观测源（IMU / 磁力计 / 视觉等）——云台绝对角融合 seam。
|
||||
//!
|
||||
//! 电机抽象只保证 [`crate::MotorState::rotor_revolutions`] 连续（上电相对）。
|
||||
//! 场地绝对角由上层（未来 `gimbal` crate）融合 IMU 与电机多圈得到。
|
||||
|
||||
use std::future::Future;
|
||||
|
||||
/// 绝对航向观测源。
|
||||
pub trait HeadingSource: Send {
|
||||
/// 机体航向角（rad，连续；与 chassis ISO 约定一致：x 前、逆时针为正）。
|
||||
fn heading_rad(&self) -> impl Future<Output = f32> + Send;
|
||||
}
|
||||
|
||||
/// 云台目标角的参考系。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum HeadingFrame {
|
||||
/// 电机转子多圈（上电相对）。
|
||||
RotorMultiTurn,
|
||||
/// 场地绝对角（经 [`HeadingSource`] 对齐到转子基准后再下发）。
|
||||
AbsoluteField,
|
||||
}
|
||||
}
|
||||
|
||||
pub mod registry {
|
||||
//! 电机注册表：**按总线 + 反馈帧 ID** 找回设备（ARCHITECTURE.md §5.4）。
|
||||
//!
|
||||
//! # 为什么需要它
|
||||
//!
|
||||
//! 单总线底盘上，`EscBus` 自己就能把反馈帧分发给对应的 `EscMotor`。但整机是
|
||||
//! **双 CAN**（§2.1：底盘 `can0` + 功能总线 `can1`），云台 / 拨弹的帧与底盘的
|
||||
//! 帧可能落在**相同的 ID** 上（GM6020 与 C620 都可能是 0x2xx）。此时"这帧是谁的"
|
||||
//! 只能由 `(总线, 帧 ID)` 共同决定。
|
||||
//!
|
||||
//! [`MotorRegistry`] 就是这张映射表：注册时校验冲突，分发时 O(1) 命中。
|
||||
//! 当前 `chassisd` 的单总线底盘路径不经过它；它是云台 / 拨弹接入功能总线时的
|
||||
//! seam（先有接口与测试，避免将来把分发逻辑硬编码进每个任务）。
|
||||
|
||||
use can::CanFrame;
|
||||
use motor_protocol::feedback_frame_id;
|
||||
|
||||
/// 物理总线标识（ARCHITECTURE.md §2.1 的双 CAN 拓扑）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)]
|
||||
pub enum BusId {
|
||||
/// 底盘总线（C620/C610 驱动轮）。
|
||||
Chassis,
|
||||
/// 功能总线（云台 GM6020 + 拨弹 M2006/C610）。
|
||||
Function,
|
||||
}
|
||||
|
||||
impl BusId {
|
||||
/// 文本名。
|
||||
#[must_use]
|
||||
pub const fn name(self) -> &'static str {
|
||||
match self {
|
||||
Self::Chassis => "chassis",
|
||||
Self::Function => "function",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 注册表条目。
|
||||
#[derive(Debug, Clone, PartialEq, Eq)]
|
||||
pub struct RegisteredMotor {
|
||||
/// 所属总线。
|
||||
pub bus: BusId,
|
||||
/// 反馈帧 ID。
|
||||
pub frame_id: u32,
|
||||
/// 轮名 / 设备名（配置里的 `name`）。
|
||||
pub name: String,
|
||||
}
|
||||
|
||||
/// 注册错误。
|
||||
///
|
||||
/// 手写 `Display` / `Error` 而不是 `derive(thiserror::Error)`：注册表属于
|
||||
/// **总是可用**的设备抽象（关掉 `bus` feature 也要能编译），不应为一个派生宏
|
||||
/// 引入可选依赖。
|
||||
#[derive(Debug, Clone, PartialEq, Eq)]
|
||||
pub enum RegistryError {
|
||||
/// 同一 `(总线, 帧 ID)` 被注册两次——几乎总是接线或配置写重了。
|
||||
Duplicate {
|
||||
/// 总线。
|
||||
bus: &'static str,
|
||||
/// 冲突的帧 ID。
|
||||
frame_id: u32,
|
||||
/// 先注册的名字。
|
||||
existing: String,
|
||||
/// 后注册的名字。
|
||||
name: String,
|
||||
},
|
||||
/// 电调 ID 越界（1–8）。
|
||||
BadEscId(u8),
|
||||
}
|
||||
|
||||
impl std::fmt::Display for RegistryError {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
match self {
|
||||
Self::Duplicate { bus, frame_id, existing, name } => write!(
|
||||
f,
|
||||
"总线 {bus} 上的帧 ID 0x{frame_id:03X} 已被 '{existing}' 占用，无法再注册 '{name}'"
|
||||
),
|
||||
Self::BadEscId(id) => write!(f, "电调 ID {id} 越界（合法范围 1–8）"),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl std::error::Error for RegistryError {}
|
||||
|
||||
/// 设备注册表。
|
||||
#[derive(Debug, Default, Clone)]
|
||||
pub struct MotorRegistry {
|
||||
entries: Vec<RegisteredMotor>,
|
||||
}
|
||||
|
||||
impl MotorRegistry {
|
||||
/// 空表。
|
||||
#[must_use]
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// 按电调 ID 注册（反馈帧 ID = `0x200 + 电调ID`，C620/C610 约定）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 电调 ID 越界或该帧 ID 已被占用时返回错误。
|
||||
pub fn register_esc(
|
||||
&mut self,
|
||||
bus: BusId,
|
||||
esc_id: u8,
|
||||
name: impl Into<String>,
|
||||
) -> Result<(), RegistryError> {
|
||||
let frame_id = feedback_frame_id(esc_id).ok_or(RegistryError::BadEscId(esc_id))?;
|
||||
self.register_frame(bus, frame_id, name)
|
||||
}
|
||||
|
||||
/// 按反馈帧 ID 注册（GM6020 或非 1–8 的 ID 用它）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 该 `(总线, 帧 ID)` 已被占用时返回错误。
|
||||
pub fn register_frame(
|
||||
&mut self,
|
||||
bus: BusId,
|
||||
frame_id: u32,
|
||||
name: impl Into<String>,
|
||||
) -> Result<(), RegistryError> {
|
||||
let name = name.into();
|
||||
if let Some(existing) = self.resolve(bus, frame_id) {
|
||||
return Err(RegistryError::Duplicate {
|
||||
bus: bus.name(),
|
||||
frame_id,
|
||||
existing: existing.name.clone(),
|
||||
name,
|
||||
});
|
||||
}
|
||||
self.entries.push(RegisteredMotor { bus, frame_id, name });
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 按 `(总线, 帧 ID)` 查找。
|
||||
#[must_use]
|
||||
pub fn resolve(&self, bus: BusId, frame_id: u32) -> Option<&RegisteredMotor> {
|
||||
self.entries.iter().find(|entry| entry.bus == bus && entry.frame_id == frame_id)
|
||||
}
|
||||
|
||||
/// 从一帧反馈找出它的归属设备（ARCHITECTURE.md §5.4 的 `update_from_frame`）。
|
||||
///
|
||||
/// 返回 `None` 表示这帧不属于任何已注册设备（例如 IMU 帧，或没配的电机）。
|
||||
#[must_use]
|
||||
pub fn dispatch(&self, bus: BusId, frame: &CanFrame) -> Option<&RegisteredMotor> {
|
||||
self.resolve(bus, frame.id)
|
||||
}
|
||||
|
||||
/// 已注册设备数。
|
||||
#[must_use]
|
||||
pub fn len(&self) -> usize {
|
||||
self.entries.len()
|
||||
}
|
||||
|
||||
/// 是否为空。
|
||||
#[must_use]
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.entries.is_empty()
|
||||
}
|
||||
|
||||
/// 全部条目（按注册顺序）。
|
||||
#[must_use]
|
||||
pub fn entries(&self) -> &[RegisteredMotor] {
|
||||
&self.entries
|
||||
}
|
||||
|
||||
/// 某条总线上的设备名。
|
||||
#[must_use]
|
||||
pub fn names_on(&self, bus: BusId) -> Vec<&str> {
|
||||
self.entries
|
||||
.iter()
|
||||
.filter(|entry| entry.bus == bus)
|
||||
.map(|entry| entry.name.as_str())
|
||||
.collect()
|
||||
}
|
||||
}
|
||||
|
||||
/// 底盘四轮的注册顺序（与 `mecanum` 的 FL/FR/RL/RR 一致）。
|
||||
pub const CHASSIS_WHEEL_NAMES: [&str; 4] =
|
||||
["front_left", "front_right", "rear_left", "rear_right"];
|
||||
|
||||
/// 把底盘四轮按顺序注册进表（电调 ID 从 1 开始连续）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 任一电调 ID 越界或冲突时返回错误。
|
||||
pub fn register_chassis_wheels(
|
||||
registry: &mut MotorRegistry,
|
||||
esc_ids: [u8; 4],
|
||||
) -> Result<(), RegistryError> {
|
||||
for (name, esc_id) in CHASSIS_WHEEL_NAMES.iter().zip(esc_ids) {
|
||||
registry.register_esc(BusId::Chassis, esc_id, *name)?;
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor_protocol::{MAX_ESC_ID, MIN_ESC_ID};
|
||||
|
||||
#[test]
|
||||
fn registers_and_dispatches_chassis_wheels() {
|
||||
let mut registry = MotorRegistry::new();
|
||||
register_chassis_wheels(&mut registry, [1, 2, 3, 4]).unwrap();
|
||||
assert_eq!(registry.len(), 4);
|
||||
assert_eq!(registry.names_on(BusId::Chassis).len(), 4);
|
||||
assert!(registry.names_on(BusId::Function).is_empty());
|
||||
|
||||
let frame = CanFrame::new(0x203, [0; 8]);
|
||||
let found = registry.dispatch(BusId::Chassis, &frame).unwrap();
|
||||
assert_eq!(found.name, "rear_left", "0x203 = 电调 3 = 后左");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn same_frame_id_on_another_bus_is_not_a_conflict() {
|
||||
let mut registry = MotorRegistry::new();
|
||||
registry.register_esc(BusId::Chassis, 1, "front_left").unwrap();
|
||||
// 功能总线上的 GM6020 ID 1 反馈同样是 0x205 —— 不同总线，互不冲突。
|
||||
registry.register_frame(BusId::Function, 0x205, "gimbal_yaw").unwrap();
|
||||
assert_eq!(registry.len(), 2);
|
||||
assert_eq!(registry.resolve(BusId::Chassis, 0x205), None);
|
||||
assert_eq!(registry.resolve(BusId::Function, 0x205).unwrap().name, "gimbal_yaw");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn duplicates_on_the_same_bus_are_rejected_with_both_names() {
|
||||
let mut registry = MotorRegistry::new();
|
||||
registry.register_esc(BusId::Chassis, 2, "front_right").unwrap();
|
||||
let error = registry.register_esc(BusId::Chassis, 2, "typo_front_right").unwrap_err();
|
||||
let text = error.to_string();
|
||||
assert!(text.contains("front_right"), "{text}");
|
||||
assert!(text.contains("typo_front_right"), "{text}");
|
||||
assert_eq!(registry.len(), 1, "失败的注册不得写入");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn out_of_range_esc_ids_are_rejected() {
|
||||
let mut registry = MotorRegistry::new();
|
||||
assert_eq!(
|
||||
registry.register_esc(BusId::Chassis, 0, "ghost"),
|
||||
Err(RegistryError::BadEscId(0))
|
||||
);
|
||||
assert_eq!(
|
||||
registry.register_esc(BusId::Chassis, 9, "ghost"),
|
||||
Err(RegistryError::BadEscId(9))
|
||||
);
|
||||
assert!(registry.is_empty());
|
||||
assert!(feedback_frame_id(MIN_ESC_ID).is_some());
|
||||
assert!(feedback_frame_id(MAX_ESC_ID).is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn unknown_frames_dispatch_to_nothing() {
|
||||
let mut registry = MotorRegistry::new();
|
||||
register_chassis_wheels(&mut registry, [1, 2, 3, 4]).unwrap();
|
||||
// DETA10 的 IMU 帧不是任何电机。
|
||||
let imu = CanFrame::new(0x100, [0; 8]);
|
||||
assert!(registry.dispatch(BusId::Chassis, &imu).is_none());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod testing {
|
||||
//! 测试替身：内存电机与航向源。
|
||||
|
||||
use std::future::Future;
|
||||
use std::sync::{Arc, Mutex, MutexGuard};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
use crate::{
|
||||
Command, Disable, EffortDrive, HeadingSource, MotorFeedback, MotorState, PositionDrive,
|
||||
VelocityDrive,
|
||||
};
|
||||
|
||||
/// 锁辅助：中毒锁直接恢复内部数据。
|
||||
fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
|
||||
mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
|
||||
}
|
||||
|
||||
/// 内存电机：记录命令、可注入状态。供上层单测，无需总线。
|
||||
///
|
||||
/// 句柄可克隆（内部 `Arc` 共享），便于一边注入/断言一边把所有权交给控制器。
|
||||
#[derive(Clone)]
|
||||
pub struct MockMotor {
|
||||
id: u8,
|
||||
published: Arc<Mutex<(MotorState, Command)>>,
|
||||
last_feedback: Arc<Mutex<Option<Instant>>>,
|
||||
}
|
||||
|
||||
impl MockMotor {
|
||||
/// 创建指定 ID 的空电机。
|
||||
#[must_use]
|
||||
pub fn new(id: u8) -> Self {
|
||||
Self {
|
||||
id,
|
||||
published: Arc::new(Mutex::new((MotorState::default(), Command::Disabled))),
|
||||
last_feedback: Arc::new(Mutex::new(None)),
|
||||
}
|
||||
}
|
||||
|
||||
/// 注入反馈快照（记为「刚刚收到」）。
|
||||
pub fn inject_state(&self, state: MotorState) {
|
||||
self.inject_state_with_age(state, Duration::ZERO);
|
||||
}
|
||||
|
||||
/// 注入反馈快照并指定其“已过去”的时长（用于在线/超时测试）。
|
||||
pub fn inject_state_with_age(&self, state: MotorState, age: Duration) {
|
||||
lock(&self.published).0 = state;
|
||||
let at = Instant::now().checked_sub(age);
|
||||
*lock(&self.last_feedback) = Some(at.unwrap_or_else(Instant::now));
|
||||
}
|
||||
|
||||
/// 最近一次命令。
|
||||
#[must_use]
|
||||
pub fn last_command(&self) -> Command {
|
||||
lock(&self.published).1
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorFeedback for MockMotor {
|
||||
fn id(&self) -> u8 {
|
||||
self.id
|
||||
}
|
||||
fn state(&self) -> MotorState {
|
||||
lock(&self.published).0
|
||||
}
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
let at = *lock(&self.last_feedback);
|
||||
Some(at?.elapsed())
|
||||
}
|
||||
}
|
||||
|
||||
impl VelocityDrive for MockMotor {
|
||||
fn set_velocity(&self, rotor_rpm: f32) {
|
||||
lock(&self.published).1 = Command::Velocity(rotor_rpm);
|
||||
}
|
||||
}
|
||||
|
||||
impl EffortDrive for MockMotor {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
lock(&self.published).1 = Command::EffortRaw(raw);
|
||||
}
|
||||
}
|
||||
|
||||
impl PositionDrive for MockMotor {
|
||||
fn set_position(&self, rotor_rev: f32) {
|
||||
lock(&self.published).1 = Command::Position(rotor_rev);
|
||||
}
|
||||
}
|
||||
|
||||
impl Disable for MockMotor {
|
||||
fn disable(&self) {
|
||||
lock(&self.published).1 = Command::Disabled;
|
||||
}
|
||||
}
|
||||
|
||||
/// 恒定航向测试源。
|
||||
pub struct MockHeading {
|
||||
rad: f32,
|
||||
}
|
||||
|
||||
impl MockHeading {
|
||||
/// 创建固定航向源。
|
||||
#[must_use]
|
||||
pub const fn new(rad: f32) -> Self {
|
||||
Self { rad }
|
||||
}
|
||||
}
|
||||
|
||||
impl HeadingSource for MockHeading {
|
||||
fn heading_rad(&self) -> impl Future<Output = f32> + Send {
|
||||
std::future::ready(self.rad)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn mock_motor_records_commands() {
|
||||
let m = MockMotor::new(3);
|
||||
m.set_velocity(1000.0);
|
||||
assert_eq!(m.last_command(), Command::Velocity(1000.0));
|
||||
m.disable();
|
||||
assert_eq!(m.last_command(), Command::Disabled);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mock_motor_injects_state() {
|
||||
let m = MockMotor::new(1);
|
||||
assert!(!m.state().has_feedback());
|
||||
assert!(m.feedback_age().is_none());
|
||||
m.inject_state(MotorState { feedback_count: 5, ..MotorState::default() });
|
||||
assert!(m.state().has_feedback());
|
||||
assert!(m.feedback_age().is_some_and(|age| age < Duration::from_millis(50)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mock_motor_feedback_age_can_be_stale() {
|
||||
let m = MockMotor::new(1);
|
||||
m.inject_state_with_age(
|
||||
MotorState { feedback_count: 1, ..MotorState::default() },
|
||||
Duration::from_millis(250),
|
||||
);
|
||||
assert!(m.feedback_age().is_some_and(|age| age >= Duration::from_millis(200)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use command::*;
|
||||
pub use gearbox::*;
|
||||
pub use heading::*;
|
||||
pub use registry::*;
|
||||
pub use state::*;
|
||||
pub use testing::*;
|
||||
pub use traits::*;
|
||||
|
||||
pub mod spec {
|
||||
//! Generic motor identity and mechanical characteristics.
|
||||
|
||||
use motor_protocol::{CurrentC610, CurrentC620, CurrentEsc, Gm6020Voltage, MotorProtocol};
|
||||
|
||||
/// Control quantity represented by a motor's wire command.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum ControlMode {
|
||||
/// Current command.
|
||||
Current,
|
||||
/// Voltage command.
|
||||
Voltage,
|
||||
}
|
||||
|
||||
/// Mechanical and protocol identity for one motor model.
|
||||
pub trait MotorSpec: Send + Sync + 'static {
|
||||
type Protocol: MotorProtocol;
|
||||
const NAME: &'static str;
|
||||
const GEAR_RATIO: f32;
|
||||
const ENCODER_TICKS: u16;
|
||||
const CONTROL_MODE: ControlMode;
|
||||
|
||||
fn scale() -> <Self::Protocol as MotorProtocol>::Scale;
|
||||
}
|
||||
|
||||
/// Marker for models driven by the shared C620/C610 current frame path.
|
||||
pub trait CurrentEscDrive: MotorSpec {}
|
||||
|
||||
/// Marker for models driven by a voltage frame path.
|
||||
pub trait VoltageEscDrive: MotorSpec {}
|
||||
|
||||
/// GM3508 + C620.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct Gm3508;
|
||||
|
||||
/// M2006 + C610.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct M2006;
|
||||
|
||||
/// GM6020 direct-drive voltage motor.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct Gm6020;
|
||||
|
||||
impl MotorSpec for Gm3508 {
|
||||
type Protocol = CurrentEsc<CurrentC620>;
|
||||
const NAME: &'static str = "GM3508";
|
||||
const GEAR_RATIO: f32 = 3591.0 / 187.0;
|
||||
const ENCODER_TICKS: u16 = 8192;
|
||||
const CONTROL_MODE: ControlMode = ControlMode::Current;
|
||||
fn scale() -> CurrentC620 {
|
||||
CurrentC620
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorSpec for M2006 {
|
||||
type Protocol = CurrentEsc<CurrentC610>;
|
||||
const NAME: &'static str = "M2006";
|
||||
const GEAR_RATIO: f32 = 36.0;
|
||||
const ENCODER_TICKS: u16 = 8192;
|
||||
const CONTROL_MODE: ControlMode = ControlMode::Current;
|
||||
fn scale() -> CurrentC610 {
|
||||
CurrentC610
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorSpec for Gm6020 {
|
||||
type Protocol = Gm6020Voltage;
|
||||
const NAME: &'static str = "GM6020";
|
||||
const GEAR_RATIO: f32 = 1.0;
|
||||
const ENCODER_TICKS: u16 = 8192;
|
||||
const CONTROL_MODE: ControlMode = ControlMode::Voltage;
|
||||
fn scale() -> motor_protocol::Voltage30000 {
|
||||
motor_protocol::Voltage30000
|
||||
}
|
||||
}
|
||||
|
||||
impl CurrentEscDrive for Gm3508 {}
|
||||
impl CurrentEscDrive for M2006 {}
|
||||
impl VoltageEscDrive for Gm6020 {}
|
||||
|
||||
/// Keep scale construction available to generic callers.
|
||||
pub fn scale_of<S: MotorSpec>() -> <S::Protocol as MotorProtocol>::Scale {
|
||||
S::scale()
|
||||
}
|
||||
|
||||
/// Compile-time assertion helper for current-path models.
|
||||
pub fn assert_current_drive<S: CurrentEscDrive>() {}
|
||||
|
||||
/// Compile-time assertion helper for voltage-path models.
|
||||
pub fn assert_voltage_drive<S: VoltageEscDrive>() {}
|
||||
}
|
||||
|
||||
pub use spec::{ControlMode, CurrentEscDrive, Gm3508, Gm6020, M2006, MotorSpec, VoltageEscDrive};
|
||||
|
||||
#[cfg(test)]
|
||||
mod spec_tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn motor_markers_select_the_right_protocol_family() {
|
||||
spec::assert_current_drive::<Gm3508>();
|
||||
spec::assert_current_drive::<M2006>();
|
||||
spec::assert_voltage_drive::<Gm6020>();
|
||||
assert_eq!(Gm3508::NAME, "GM3508");
|
||||
assert_eq!(M2006::NAME, "M2006");
|
||||
assert_eq!(Gm6020::NAME, "GM6020");
|
||||
assert_eq!(Gm3508::CONTROL_MODE, ControlMode::Current);
|
||||
assert_eq!(Gm6020::CONTROL_MODE, ControlMode::Voltage);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
[package]
|
||||
name = "dr16"
|
||||
description = "DJI DR16 D-BUS serial receiver driver"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
dr16-protocol = { workspace = true }
|
||||
serial = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "device/remote"
|
||||
@@ -0,0 +1,244 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! DJI DR16 D-BUS 串口接收驱动。
|
||||
//!
|
||||
//! DR16 使用非标准 100000 baud、8 data bits、even parity、1 stop bit。阻塞串口
|
||||
//! I/O 运行在专用线程上，异步侧只消费**最新**样本，避免遥控数据形成积压队列。
|
||||
|
||||
use std::io::{self, Read};
|
||||
use std::thread;
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
use dr16_protocol::{Dr16Sample, FrameDecoder};
|
||||
use serial::prelude::*;
|
||||
use tokio::sync::watch;
|
||||
use tracing::{debug, error};
|
||||
|
||||
/// D-BUS UART 波特率。
|
||||
pub const BAUD_RATE: usize = 100_000;
|
||||
|
||||
/// 串口读取参数。
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct Dr16Options {
|
||||
/// 阻塞串口读超时；也用于让读取线程定期观察接收端关闭。
|
||||
pub read_timeout: Duration,
|
||||
}
|
||||
|
||||
impl Default for Dr16Options {
|
||||
fn default() -> Self {
|
||||
Self { read_timeout: Duration::from_millis(20) }
|
||||
}
|
||||
}
|
||||
|
||||
/// 带接收时间的 DR16 样本。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct StampedSample {
|
||||
/// 解码后的遥控输入。
|
||||
pub sample: Dr16Sample,
|
||||
/// 样本到达用户态读取循环的时刻。
|
||||
pub received_at: Instant,
|
||||
}
|
||||
|
||||
/// 最新样本发送端。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct SampleSender {
|
||||
tx: watch::Sender<Option<StampedSample>>,
|
||||
}
|
||||
|
||||
impl SampleSender {
|
||||
/// 发布一个新样本并返回带时间戳的副本。
|
||||
#[must_use]
|
||||
pub fn send(&self, sample: Dr16Sample) -> StampedSample {
|
||||
let stamped = StampedSample { sample, received_at: Instant::now() };
|
||||
self.tx.send_replace(Some(stamped));
|
||||
stamped
|
||||
}
|
||||
|
||||
/// 当前是否有异步消费者。
|
||||
#[must_use]
|
||||
pub fn is_closed(&self) -> bool {
|
||||
self.tx.is_closed()
|
||||
}
|
||||
}
|
||||
|
||||
/// 创建一对最新样本发送端和接收流。
|
||||
#[must_use]
|
||||
pub fn sample_channel() -> (SampleSender, SampleStream) {
|
||||
let (tx, rx) = watch::channel(None);
|
||||
(SampleSender { tx }, SampleStream { rx })
|
||||
}
|
||||
|
||||
/// 最新 DR16 样本流。
|
||||
///
|
||||
/// 内部使用 `watch`：慢消费者看到的是最新遥控状态，不会执行延迟的旧摇杆位置。
|
||||
#[derive(Debug)]
|
||||
pub struct SampleStream {
|
||||
rx: watch::Receiver<Option<StampedSample>>,
|
||||
}
|
||||
|
||||
impl SampleStream {
|
||||
/// 等待下一个新样本；读取线程退出且没有样本时返回 `None`。
|
||||
pub async fn recv(&mut self) -> Option<StampedSample> {
|
||||
loop {
|
||||
self.rx.changed().await.ok()?;
|
||||
if let Some(sample) = *self.rx.borrow_and_update() {
|
||||
return Some(sample);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 读取当前最新样本；尚未收到帧时返回 `None`。
|
||||
#[must_use]
|
||||
pub fn latest(&self) -> Option<StampedSample> {
|
||||
*self.rx.borrow()
|
||||
}
|
||||
}
|
||||
|
||||
/// 打开 DR16 串口并开始接收。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 设备不存在、权限不足、串口不支持 100000 baud 或配置 8E1 失败。
|
||||
pub fn open(device: &str) -> io::Result<SampleStream> {
|
||||
open_with(device, Dr16Options::default())
|
||||
}
|
||||
|
||||
/// 使用指定读取参数打开 DR16 串口。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 同 [`open`]。
|
||||
pub fn open_with(device: &str, options: Dr16Options) -> io::Result<SampleStream> {
|
||||
let mut port = serial::open(device)?;
|
||||
port.reconfigure(&|settings| {
|
||||
settings.set_baud_rate(serial::BaudOther(BAUD_RATE))?;
|
||||
settings.set_char_size(serial::Bits8);
|
||||
settings.set_parity(serial::ParityEven);
|
||||
settings.set_stop_bits(serial::Stop1);
|
||||
settings.set_flow_control(serial::FlowNone);
|
||||
Ok(())
|
||||
})?;
|
||||
port.set_timeout(options.read_timeout)?;
|
||||
debug!(device, baud = BAUD_RATE, "DR16 serial opened");
|
||||
Ok(spawn_reader(port))
|
||||
}
|
||||
|
||||
/// 在专用线程上泵送任意字节 reader，返回最新样本流。
|
||||
#[must_use]
|
||||
pub fn spawn_reader<R>(reader: R) -> SampleStream
|
||||
where
|
||||
R: Read + Send + 'static,
|
||||
{
|
||||
let (tx, stream) = sample_channel();
|
||||
let thread = thread::Builder::new().name("dr16-reader".into()).spawn(move || {
|
||||
pump_reader(reader, &tx);
|
||||
});
|
||||
if let Err(error) = thread {
|
||||
error!(%error, "failed to start DR16 reader thread");
|
||||
}
|
||||
stream
|
||||
}
|
||||
|
||||
fn pump_reader<R: Read>(mut reader: R, tx: &SampleSender) {
|
||||
let mut decoder = FrameDecoder::new();
|
||||
let mut buffer = [0u8; 64];
|
||||
loop {
|
||||
match reader.read(&mut buffer) {
|
||||
Ok(0) => {
|
||||
debug!("DR16 reader reached end of input");
|
||||
return;
|
||||
}
|
||||
Ok(count) => {
|
||||
for sample in decoder.push(&buffer[..count]) {
|
||||
let _ = tx.send(sample);
|
||||
}
|
||||
}
|
||||
Err(error) if is_transient(&error) => {
|
||||
// 读超时内没有任何字节，已经超过正常 D-BUS 帧间隔；清掉半帧，避免错位。
|
||||
decoder.clear();
|
||||
if tx.is_closed() {
|
||||
debug!("DR16 sample consumer closed");
|
||||
return;
|
||||
}
|
||||
}
|
||||
Err(error) => {
|
||||
error!(%error, "DR16 serial read failed");
|
||||
return;
|
||||
}
|
||||
}
|
||||
if tx.is_closed() {
|
||||
debug!("DR16 sample consumer closed");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn is_transient(error: &io::Error) -> bool {
|
||||
matches!(
|
||||
error.kind(),
|
||||
io::ErrorKind::TimedOut | io::ErrorKind::WouldBlock | io::ErrorKind::Interrupted
|
||||
)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use std::cmp;
|
||||
|
||||
use dr16_protocol::{FRAME_LEN, SwitchPosition};
|
||||
|
||||
use super::*;
|
||||
|
||||
struct ChunkReader {
|
||||
chunks: Vec<Vec<u8>>,
|
||||
index: usize,
|
||||
}
|
||||
|
||||
impl Read for ChunkReader {
|
||||
fn read(&mut self, output: &mut [u8]) -> io::Result<usize> {
|
||||
let Some(chunk) = self.chunks.get(self.index) else {
|
||||
return Ok(0);
|
||||
};
|
||||
self.index += 1;
|
||||
let count = cmp::min(output.len(), chunk.len());
|
||||
output[..count].copy_from_slice(&chunk[..count]);
|
||||
Ok(count)
|
||||
}
|
||||
}
|
||||
|
||||
fn frame() -> [u8; FRAME_LEN] {
|
||||
let mut frame = [0u8; FRAME_LEN];
|
||||
frame[1] = 0x04;
|
||||
frame[2] = 0x20;
|
||||
frame[4] = 0x01;
|
||||
frame[5] = 0x78;
|
||||
frame
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn generic_reader_publishes_decoded_latest_sample() {
|
||||
let raw = frame();
|
||||
let reader = ChunkReader {
|
||||
chunks: vec![raw[..5].to_vec(), raw[5..13].to_vec(), raw[13..].to_vec()],
|
||||
index: 0,
|
||||
};
|
||||
let mut stream = spawn_reader(reader);
|
||||
let stamped = stream.recv().await.unwrap();
|
||||
assert_eq!(stamped.sample.s1, SwitchPosition::Up);
|
||||
assert_eq!(stamped.sample.s2, SwitchPosition::Mid);
|
||||
assert_eq!(stream.latest(), Some(stamped));
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn closed_reader_reports_end_of_stream() {
|
||||
let reader = ChunkReader { chunks: Vec::new(), index: 0 };
|
||||
let mut stream = spawn_reader(reader);
|
||||
assert_eq!(stream.recv().await, None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn transient_read_errors_are_not_fatal() {
|
||||
assert!(is_transient(&io::Error::from(io::ErrorKind::TimedOut)));
|
||||
assert!(is_transient(&io::Error::from(io::ErrorKind::WouldBlock)));
|
||||
assert!(!is_transient(&io::Error::from(io::ErrorKind::BrokenPipe)));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user