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());
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user