feat: 添加了自动超参数整定
This commit is contained in:
Horldsence
@@ -0,0 +1,683 @@
|
||||
//! `chassis.toml` 配置模型：解析、语义校验与到驱动配置的映射。
|
||||
//!
|
||||
//! 本 crate 是**配置 schema 的唯一来源**：`chassisd`（守护进程）与
|
||||
//! `chassis-tune`（自动整定）读同一份类型定义，因此整定工具写回的
|
||||
//! `[[motor]]` 片段必然能被守护进程读回，不需要两份 schema 手工同步。
|
||||
//!
|
||||
//! 全部类型都实现 `Serialize`，所以 [`Config::to_toml_string`] 可以把
|
||||
//! （可能已被整定工具改写过的）配置原样落盘。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc::{BusOptions, EscConfig, EscModel};
|
||||
use mecanum::Mecanum;
|
||||
use motor_core::VelocityGains;
|
||||
use serde::{Deserialize, Serialize};
|
||||
use thiserror::Error;
|
||||
|
||||
/// 配置错误。
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Error)]
|
||||
pub enum ConfigError {
|
||||
/// TOML 语法 / 结构错误。
|
||||
#[error("failed to parse config: {0}")]
|
||||
Parse(String),
|
||||
/// 语义校验失败。
|
||||
#[error("invalid config: {0}")]
|
||||
Invalid(String),
|
||||
}
|
||||
|
||||
/// 顶层配置。
|
||||
#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields)]
|
||||
pub struct Config {
|
||||
/// CAN 配置。
|
||||
pub can: CanConfig,
|
||||
/// 底盘控制参数。
|
||||
#[serde(default)]
|
||||
pub chassis: ChassisConfig,
|
||||
/// 麦轮几何。
|
||||
pub mecanum: MecanumConfig,
|
||||
/// 四电机配置，顺序固定：前左、前右、后左、后右。
|
||||
#[serde(rename = "motor")]
|
||||
pub motors: Vec<MotorConfig>,
|
||||
}
|
||||
|
||||
/// CAN 接口配置（双 CAN 总线 seam）。
|
||||
///
|
||||
/// 底盘与功能总线物理隔离，**每条总线只允许一种协议族**（见 [`ProtocolFamily`]）。
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields)]
|
||||
pub struct CanConfig {
|
||||
/// 底盘总线 SocketCAN 接口名（如 `can0`）——C620 驱动轮，ESC 电流协议族。
|
||||
pub interface: String,
|
||||
/// 功能总线（云台 / 拨弹）：与底盘物理隔离。
|
||||
///
|
||||
/// 预留 seam；填写后由上层功能模块（gimbal / feeder）使用。
|
||||
/// 典型：`can1`，承载 GM6020 云台 + M2006/C610 拨弹。
|
||||
#[serde(default)]
|
||||
pub function: Option<FunctionBusConfig>,
|
||||
}
|
||||
|
||||
/// 功能总线配置（双 CAN 之二）。
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields)]
|
||||
pub struct FunctionBusConfig {
|
||||
/// SocketCAN 接口名（如 `can1`）。
|
||||
pub interface: String,
|
||||
}
|
||||
|
||||
/// 协议族——同一总线上**不得混用**。
|
||||
///
|
||||
/// C620/C610（电流）与 GM6020（电压）的控制/反馈帧 ID 空间重叠：
|
||||
/// `0x1FF` 既可作 C620 5–8 电流也可作 GM6020 1–4 电压；`0x205` 同理。
|
||||
/// 双 CAN 物理隔离是首选消解手段；单总线时必须保证同族。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
|
||||
pub enum ProtocolFamily {
|
||||
/// C620/C610 电流协议（驱动轮、拨弹轮）。
|
||||
Esc,
|
||||
/// GM6020 电压协议（云台）。
|
||||
Gm6020,
|
||||
}
|
||||
|
||||
impl ProtocolFamily {
|
||||
/// 电调型号所属协议族。
|
||||
#[must_use]
|
||||
pub const fn of_esc_model(model: EscModelSerde) -> Self {
|
||||
match model {
|
||||
EscModelSerde::C620 | EscModelSerde::C610 => Self::Esc,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 底盘控制参数（全部可省略，采用默认值）。
|
||||
#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields, default)]
|
||||
pub struct ChassisConfig {
|
||||
/// 控制循环周期（毫秒）。
|
||||
pub control_period_ms: u64,
|
||||
/// 上层指令超时（毫秒）：超时后目标速度归零。
|
||||
pub command_timeout_ms: u64,
|
||||
/// 电机反馈离线阈值（毫秒）。
|
||||
pub feedback_timeout_ms: u64,
|
||||
/// 驱动层**命令租约**（毫秒）：控制循环超过该时长未刷新电机命令 →
|
||||
/// 驱动强制 0 电流（防控制任务死亡后旧指令持续输出）。
|
||||
///
|
||||
/// 必须 ≥ `control_period_ms`；默认 1kHz 下约 50 拍。
|
||||
pub motor_command_timeout_ms: u64,
|
||||
/// 最大前进 / 横移速度（m/s）。
|
||||
pub max_vx: f32,
|
||||
/// 最大横移速度（m/s）。
|
||||
pub max_vy: f32,
|
||||
/// 最大角速度（rad/s）。
|
||||
pub max_omega: f32,
|
||||
}
|
||||
|
||||
impl Default for ChassisConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
control_period_ms: 1,
|
||||
command_timeout_ms: 200,
|
||||
feedback_timeout_ms: 100,
|
||||
motor_command_timeout_ms: 50,
|
||||
max_vx: 3.0,
|
||||
max_vy: 3.0,
|
||||
max_omega: 6.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 麦轮几何（米）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields)]
|
||||
pub struct MecanumConfig {
|
||||
/// 前后轮轴线到中心的纵向半距。
|
||||
pub half_length: f32,
|
||||
/// 左右轮到中心的横向半距。
|
||||
pub half_width: f32,
|
||||
/// 车轮半径。
|
||||
pub wheel_radius: f32,
|
||||
}
|
||||
|
||||
/// 单电机配置。
|
||||
#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields)]
|
||||
pub struct MotorConfig {
|
||||
/// 便于日志辨认的名字（`front_left` 等）。
|
||||
pub name: String,
|
||||
/// 电调 ID（1–8）。
|
||||
pub esc_id: u8,
|
||||
/// 电调型号（`C620` / `C610`）。
|
||||
#[serde(default = "default_model")]
|
||||
pub model: EscModelSerde,
|
||||
/// 安装方向：`1` 或 `-1`。
|
||||
#[serde(default = "default_direction")]
|
||||
pub direction: f32,
|
||||
/// 减速比（GM3508 ≈ 19.203）。
|
||||
pub gear_ratio: f32,
|
||||
/// 电流限幅（raw）。
|
||||
#[serde(default = "default_current_limit")]
|
||||
pub current_limit: i16,
|
||||
/// 速度环 kp。
|
||||
#[serde(default)]
|
||||
pub kp: f32,
|
||||
/// 速度环 ki。
|
||||
#[serde(default)]
|
||||
pub ki: f32,
|
||||
/// 速度环 kd。
|
||||
#[serde(default)]
|
||||
pub kd: f32,
|
||||
/// 速度前馈（raw/rpm）。
|
||||
#[serde(default)]
|
||||
pub feedforward: f32,
|
||||
}
|
||||
|
||||
fn default_model() -> EscModelSerde {
|
||||
EscModelSerde::C620
|
||||
}
|
||||
|
||||
fn default_direction() -> f32 {
|
||||
1.0
|
||||
}
|
||||
|
||||
fn default_current_limit() -> i16 {
|
||||
12_000
|
||||
}
|
||||
|
||||
/// 配置文件中的电调型号。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields, rename_all = "UPPERCASE")]
|
||||
pub enum EscModelSerde {
|
||||
/// C620（GM3508）。
|
||||
C620,
|
||||
/// C610（M2006）。
|
||||
C610,
|
||||
}
|
||||
|
||||
impl From<EscModelSerde> for EscModel {
|
||||
fn from(value: EscModelSerde) -> Self {
|
||||
match value {
|
||||
EscModelSerde::C620 => Self::C620,
|
||||
EscModelSerde::C610 => Self::C610,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorConfig {
|
||||
/// 速度环增益组（可整定参数）。
|
||||
#[must_use]
|
||||
pub fn velocity_gains(&self) -> VelocityGains {
|
||||
VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward)
|
||||
}
|
||||
|
||||
/// 覆写速度环增益组，其余字段（限流、方向、减速比）保持不变。
|
||||
pub fn set_velocity_gains(&mut self, gains: VelocityGains) {
|
||||
self.kp = gains.kp;
|
||||
self.ki = gains.ki;
|
||||
self.kd = gains.kd;
|
||||
self.feedforward = gains.feedforward;
|
||||
}
|
||||
}
|
||||
|
||||
impl Config {
|
||||
/// 从 TOML 文本解析并校验。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 解析失败或语义校验失败（电机数量、几何、方向等）。
|
||||
pub fn from_toml(text: &str) -> Result<Self, ConfigError> {
|
||||
let config: Self = toml::from_str(text).map_err(|e| ConfigError::Parse(e.to_string()))?;
|
||||
config.validate()?;
|
||||
Ok(config)
|
||||
}
|
||||
|
||||
/// 从文件读取并解析。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 读取失败、解析失败或校验失败。
|
||||
pub fn load(path: &std::path::Path) -> Result<Self, ConfigError> {
|
||||
let text = std::fs::read_to_string(path)
|
||||
.map_err(|e| ConfigError::Parse(format!("{}: {e}", path.display())))?;
|
||||
Self::from_toml(&text)
|
||||
}
|
||||
|
||||
/// 序列化回 TOML 文本。
|
||||
///
|
||||
/// 与 [`Config::from_toml`] 往返等价（`from_toml(to_toml_string(c)) == c`），
|
||||
/// 因此整定工具改写增益后可以直接落盘成守护进程能读的配置。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 序列化失败（类型定义与 TOML 数据模型冲突时才会发生，属实现缺陷）。
|
||||
pub fn to_toml_string(&self) -> Result<String, ConfigError> {
|
||||
toml::to_string(self).map_err(|e| ConfigError::Parse(e.to_string()))
|
||||
}
|
||||
|
||||
/// 把配置写到文件。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 序列化失败或写入失败。
|
||||
pub fn save(&self, path: &std::path::Path) -> Result<(), ConfigError> {
|
||||
let text = self.to_toml_string()?;
|
||||
std::fs::write(path, text)
|
||||
.map_err(|e| ConfigError::Parse(format!("{}: {e}", path.display())))
|
||||
}
|
||||
|
||||
/// 按名字查找电机下标。
|
||||
#[must_use]
|
||||
pub fn motor_index(&self, name: &str) -> Option<usize> {
|
||||
self.motors.iter().position(|m| m.name == name)
|
||||
}
|
||||
|
||||
/// 覆写第 `index` 台电机的速度环增益。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 下标越界时返回 [`ConfigError::Invalid`]。
|
||||
pub fn set_motor_gains(
|
||||
&mut self,
|
||||
index: usize,
|
||||
gains: VelocityGains,
|
||||
) -> Result<(), ConfigError> {
|
||||
let count = self.motors.len();
|
||||
let motor = self.motors.get_mut(index).ok_or_else(|| {
|
||||
ConfigError::Invalid(format!("motor index {index} out of 0..{count}"))
|
||||
})?;
|
||||
motor.set_velocity_gains(gains);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 按轮名覆写速度环增益（整定工具按名字写回时用）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 名字不存在时返回 [`ConfigError::Invalid`]。
|
||||
pub fn set_motor_gains_by_name(
|
||||
&mut self,
|
||||
name: &str,
|
||||
gains: VelocityGains,
|
||||
) -> Result<(), ConfigError> {
|
||||
let index = self
|
||||
.motor_index(name)
|
||||
.ok_or_else(|| ConfigError::Invalid(format!("no motor named '{name}'")))?;
|
||||
self.set_motor_gains(index, gains)
|
||||
}
|
||||
|
||||
/// 语义校验。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 任一约束不满足时返回 [`ConfigError::Invalid`]。
|
||||
pub fn validate(&self) -> Result<(), ConfigError> {
|
||||
if self.motors.len() != 4 {
|
||||
return Err(ConfigError::Invalid(format!(
|
||||
"mecanum chassis needs exactly 4 motors (FL, FR, RL, RR), got {}",
|
||||
self.motors.len()
|
||||
)));
|
||||
}
|
||||
let m = &self.mecanum;
|
||||
if !(m.half_length > 0.0 && m.half_width > 0.0 && m.wheel_radius > 0.0) {
|
||||
return Err(ConfigError::Invalid(format!("mecanum geometry must be positive: {m:?}")));
|
||||
}
|
||||
let c = &self.chassis;
|
||||
if c.control_period_ms == 0
|
||||
|| c.command_timeout_ms == 0
|
||||
|| c.feedback_timeout_ms == 0
|
||||
|| c.motor_command_timeout_ms == 0
|
||||
{
|
||||
return Err(ConfigError::Invalid(
|
||||
"control/command/feedback/motor-command timeouts must be > 0 ms".into(),
|
||||
));
|
||||
}
|
||||
if c.motor_command_timeout_ms < c.control_period_ms {
|
||||
return Err(ConfigError::Invalid(format!(
|
||||
"motor_command_timeout_ms ({}) must be >= control_period_ms ({})",
|
||||
c.motor_command_timeout_ms, c.control_period_ms
|
||||
)));
|
||||
}
|
||||
if !(c.max_vx > 0.0 && c.max_vy > 0.0 && c.max_omega > 0.0) {
|
||||
return Err(ConfigError::Invalid("speed limits must be > 0".into()));
|
||||
}
|
||||
for motor in &self.motors {
|
||||
let gear_ratio_valid = motor.gear_ratio > 0.0;
|
||||
if !gear_ratio_valid {
|
||||
return Err(ConfigError::Invalid(format!(
|
||||
"motor {}: gear_ratio must be > 0",
|
||||
motor.name
|
||||
)));
|
||||
}
|
||||
}
|
||||
// 每总线单协议族：底盘四轮必须同属 ESC 族（C620/C610）。
|
||||
let chassis_families: std::collections::HashSet<ProtocolFamily> =
|
||||
self.motors.iter().map(|m| ProtocolFamily::of_esc_model(m.model)).collect();
|
||||
if chassis_families.len() > 1 {
|
||||
return Err(ConfigError::Invalid(
|
||||
"chassis bus mixes protocol families; keep one family per CAN bus".into(),
|
||||
));
|
||||
}
|
||||
// 功能总线接口名不得与底盘总线相同（双 CAN 物理隔离）。
|
||||
if let Some(function) = &self.can.function
|
||||
&& function.interface == self.can.interface
|
||||
{
|
||||
return Err(ConfigError::Invalid(format!(
|
||||
"function bus interface must differ from chassis interface (both '{}')",
|
||||
self.can.interface
|
||||
)));
|
||||
}
|
||||
// 电调 ID、方向、限流的细粒度校验由 EscBus::spawn 复核。
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// `--mock` 演示 / 测试默认配置（与 `config/chassis.toml.example` 同构，
|
||||
/// PID 与仿真器增益匹配）。
|
||||
#[must_use]
|
||||
pub fn mock_default() -> Self {
|
||||
let motor = |name: &str, esc_id: u8, direction: f32| MotorConfig {
|
||||
name: name.into(),
|
||||
esc_id,
|
||||
model: EscModelSerde::C620,
|
||||
direction,
|
||||
gear_ratio: 3591.0 / 187.0,
|
||||
current_limit: 12_000,
|
||||
kp: 8.0,
|
||||
ki: 2.0,
|
||||
kd: 0.0,
|
||||
feedforward: 16384.0 / 9254.0,
|
||||
};
|
||||
Self {
|
||||
can: CanConfig { interface: "vcan0".into(), function: None },
|
||||
chassis: ChassisConfig::default(),
|
||||
mecanum: MecanumConfig { half_length: 0.2, half_width: 0.25, wheel_radius: 0.0762 },
|
||||
motors: vec![
|
||||
motor("front_left", 1, 1.0),
|
||||
motor("front_right", 2, -1.0),
|
||||
motor("rear_left", 3, 1.0),
|
||||
motor("rear_right", 4, -1.0),
|
||||
],
|
||||
}
|
||||
}
|
||||
|
||||
/// 转换为 `EscBus` 配置（顺序 FL、FR、RL、RR）。
|
||||
#[must_use]
|
||||
pub fn to_esc_configs(&self) -> Vec<EscConfig> {
|
||||
self.motors
|
||||
.iter()
|
||||
.map(|m| EscConfig {
|
||||
name: m.name.clone(),
|
||||
esc_id: m.esc_id,
|
||||
model: m.model.into(),
|
||||
gear_ratio: m.gear_ratio,
|
||||
direction: m.direction,
|
||||
current_limit: m.current_limit,
|
||||
kp: m.kp,
|
||||
ki: m.ki,
|
||||
kd: m.kd,
|
||||
feedforward: m.feedforward,
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// 转换为 `EscBus` 运行参数。
|
||||
#[must_use]
|
||||
pub fn bus_options(&self) -> BusOptions {
|
||||
BusOptions {
|
||||
period: Duration::from_millis(self.chassis.control_period_ms),
|
||||
feedback_timeout: Duration::from_millis(self.chassis.feedback_timeout_ms),
|
||||
command_timeout: Duration::from_millis(self.chassis.motor_command_timeout_ms),
|
||||
}
|
||||
}
|
||||
|
||||
/// 麦轮运动学模型。
|
||||
#[must_use]
|
||||
pub fn kinematics(&self) -> Mecanum {
|
||||
Mecanum::new(self.mecanum.half_length, self.mecanum.half_width, self.mecanum.wheel_radius)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
const EXAMPLE: &str = r#"
|
||||
[can]
|
||||
interface = "can0"
|
||||
|
||||
[chassis]
|
||||
control_period_ms = 1
|
||||
command_timeout_ms = 200
|
||||
feedback_timeout_ms = 100
|
||||
|
||||
[mecanum]
|
||||
half_length = 0.18
|
||||
half_width = 0.20
|
||||
wheel_radius = 0.0762
|
||||
|
||||
[[motor]]
|
||||
name = "front_left"
|
||||
esc_id = 1
|
||||
direction = 1
|
||||
gear_ratio = 19.203
|
||||
current_limit = 12000
|
||||
kp = 18.0
|
||||
ki = 0.6
|
||||
kd = 0.0
|
||||
feedforward = 9.0
|
||||
|
||||
[[motor]]
|
||||
name = "front_right"
|
||||
esc_id = 2
|
||||
direction = -1
|
||||
gear_ratio = 19.203
|
||||
current_limit = 12000
|
||||
kp = 18.0
|
||||
ki = 0.6
|
||||
feedforward = 9.0
|
||||
|
||||
[[motor]]
|
||||
name = "rear_left"
|
||||
esc_id = 3
|
||||
direction = 1
|
||||
gear_ratio = 19.203
|
||||
current_limit = 12000
|
||||
kp = 18.0
|
||||
ki = 0.6
|
||||
feedforward = 9.0
|
||||
|
||||
[[motor]]
|
||||
name = "rear_right"
|
||||
esc_id = 4
|
||||
direction = -1
|
||||
gear_ratio = 19.203
|
||||
current_limit = 12000
|
||||
kp = 18.0
|
||||
ki = 0.6
|
||||
feedforward = 9.0
|
||||
"#;
|
||||
|
||||
#[test]
|
||||
fn parses_example() {
|
||||
let cfg = Config::from_toml(EXAMPLE).unwrap();
|
||||
assert_eq!(cfg.can.interface, "can0");
|
||||
assert_eq!(cfg.motors.len(), 4);
|
||||
assert_eq!(cfg.motors[0].name, "front_left");
|
||||
assert_eq!(cfg.motors[3].esc_id, 4);
|
||||
assert_eq!(cfg.motors[1].direction.to_bits(), (-1.0f32).to_bits());
|
||||
assert!(cfg.validate().is_ok());
|
||||
assert_eq!(cfg.bus_options().period, Duration::from_millis(1));
|
||||
assert_eq!(cfg.to_esc_configs().len(), 4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn minimal_config_uses_defaults() {
|
||||
let cfg = Config::from_toml(
|
||||
r#"
|
||||
[can]
|
||||
interface = "can1"
|
||||
[mecanum]
|
||||
half_length = 0.2
|
||||
half_width = 0.25
|
||||
wheel_radius = 0.0762
|
||||
[[motor]]
|
||||
name = "fl"
|
||||
esc_id = 1
|
||||
gear_ratio = 19.2
|
||||
[[motor]]
|
||||
name = "fr"
|
||||
esc_id = 2
|
||||
gear_ratio = 19.2
|
||||
[[motor]]
|
||||
name = "rl"
|
||||
esc_id = 3
|
||||
gear_ratio = 19.2
|
||||
[[motor]]
|
||||
name = "rr"
|
||||
esc_id = 4
|
||||
gear_ratio = 19.2
|
||||
"#,
|
||||
)
|
||||
.unwrap();
|
||||
assert_eq!(cfg.chassis, ChassisConfig::default());
|
||||
assert_eq!(cfg.motors[0].direction.to_bits(), 1.0f32.to_bits());
|
||||
assert_eq!(cfg.motors[0].current_limit, 12_000);
|
||||
assert_eq!(cfg.motors[0].model, EscModelSerde::C620);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_wrong_motor_count() {
|
||||
let mut cfg = Config::from_toml(EXAMPLE).unwrap();
|
||||
cfg.motors.pop();
|
||||
assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_bad_geometry() {
|
||||
let mut cfg = Config::from_toml(EXAMPLE).unwrap();
|
||||
cfg.mecanum.wheel_radius = 0.0;
|
||||
assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_unknown_fields() {
|
||||
let text = EXAMPLE.replace("[can]", "[can]\nstray = 1");
|
||||
assert!(matches!(Config::from_toml(&text), Err(ConfigError::Parse(_))));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mock_default_is_valid_and_wired_like_example() {
|
||||
let cfg = Config::mock_default();
|
||||
assert!(cfg.validate().is_ok());
|
||||
assert_eq!(cfg.motors.len(), 4);
|
||||
let esc: Vec<u8> = cfg.to_esc_configs().iter().map(|m| m.esc_id).collect();
|
||||
assert_eq!(esc, [1, 2, 3, 4]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn function_bus_must_differ_from_chassis_bus() {
|
||||
let mut cfg = Config::mock_default();
|
||||
cfg.can.function = Some(FunctionBusConfig { interface: "vcan0".into() });
|
||||
assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
|
||||
|
||||
cfg.can.function = Some(FunctionBusConfig { interface: "vcan1".into() });
|
||||
assert!(cfg.validate().is_ok());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_mixed_protocol_families_on_chassis_bus() {
|
||||
let mut cfg = Config::mock_default();
|
||||
// 当前 ESC 族包含 C620 与 C610；若引入 GM6020 到底盘总线必须拒绝。
|
||||
// 这里验证家族集合逻辑：全部 C620/C610 仍为单族。
|
||||
cfg.motors[0].model = EscModelSerde::C610;
|
||||
assert!(cfg.validate().is_ok());
|
||||
|
||||
// 人为构造跨族：通过 of_esc_model 映射覆盖后仍应单族（C610→Esc）。
|
||||
assert_eq!(ProtocolFamily::of_esc_model(EscModelSerde::C620), ProtocolFamily::Esc);
|
||||
assert_eq!(ProtocolFamily::of_esc_model(EscModelSerde::C610), ProtocolFamily::Esc);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn parses_function_bus_seam() {
|
||||
let text = EXAMPLE.replace(
|
||||
"[can]\ninterface = \"can0\"",
|
||||
"[can]\ninterface = \"can0\"\n\n[can.function]\ninterface = \"can1\"",
|
||||
);
|
||||
let cfg = Config::from_toml(&text).unwrap();
|
||||
let function = cfg.can.function.as_ref().unwrap();
|
||||
assert_eq!(function.interface, "can1");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn toml_round_trip_is_lossless() {
|
||||
let original = Config::from_toml(EXAMPLE).unwrap();
|
||||
let text = original.to_toml_string().unwrap();
|
||||
let reparsed = Config::from_toml(&text).unwrap();
|
||||
assert_eq!(reparsed, original);
|
||||
// 落盘文本必须仍然是合法配置（守护进程直接读它）。
|
||||
assert!(text.contains("[[motor]]"), "{text}");
|
||||
assert!(text.contains("interface = \"can0\""), "{text}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn round_trip_preserves_function_bus_and_timeouts() {
|
||||
let mut original = Config::mock_default();
|
||||
original.can.function = Some(FunctionBusConfig { interface: "can1".into() });
|
||||
original.chassis.motor_command_timeout_ms = 37;
|
||||
let reparsed = Config::from_toml(&original.to_toml_string().unwrap()).unwrap();
|
||||
assert_eq!(reparsed, original);
|
||||
assert_eq!(reparsed.chassis.motor_command_timeout_ms, 37);
|
||||
assert_eq!(reparsed.can.function.unwrap().interface, "can1");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn motor_gains_can_be_read_and_written_by_name() {
|
||||
let mut cfg = Config::from_toml(EXAMPLE).unwrap();
|
||||
assert_eq!(cfg.motor_index("rear_left"), Some(2));
|
||||
assert_eq!(cfg.motor_index("nope"), None);
|
||||
assert_eq!(cfg.motors[2].velocity_gains(), VelocityGains::new(18.0, 0.6, 0.0, 9.0));
|
||||
|
||||
let tuned = VelocityGains::new(2.0, 10.0, 0.1, 0.0);
|
||||
cfg.set_motor_gains_by_name("rear_left", tuned).unwrap();
|
||||
assert_eq!(cfg.motors[2].velocity_gains(), tuned);
|
||||
// 其余电机不受影响。
|
||||
assert_eq!(cfg.motors[0].velocity_gains(), VelocityGains::new(18.0, 0.6, 0.0, 9.0));
|
||||
// 机械 / 安全字段保持不变。
|
||||
assert_eq!(cfg.motors[2].current_limit, 12_000);
|
||||
assert!((cfg.motors[2].gear_ratio - 19.203).abs() < 1e-6);
|
||||
assert!(cfg.validate().is_ok());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn motor_gains_write_back_survives_round_trip() {
|
||||
let mut cfg = Config::mock_default();
|
||||
let tuned = VelocityGains::new(3.5, 22.0, 0.25, 0.75);
|
||||
cfg.set_motor_gains(0, tuned).unwrap();
|
||||
let reparsed = Config::from_toml(&cfg.to_toml_string().unwrap()).unwrap();
|
||||
assert_eq!(reparsed.motors[0].velocity_gains(), tuned);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn motor_gains_out_of_range_is_rejected() {
|
||||
let mut cfg = Config::mock_default();
|
||||
assert!(matches!(
|
||||
cfg.set_motor_gains(9, VelocityGains::ZERO),
|
||||
Err(ConfigError::Invalid(_))
|
||||
));
|
||||
assert!(matches!(
|
||||
cfg.set_motor_gains_by_name("ghost", VelocityGains::ZERO),
|
||||
Err(ConfigError::Invalid(_))
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn save_and_load_file_round_trip() {
|
||||
let dir = std::env::temp_dir().join("chassis-config-roundtrip");
|
||||
std::fs::create_dir_all(&dir).unwrap();
|
||||
let path = dir.join("chassis.toml");
|
||||
let original = Config::mock_default();
|
||||
original.save(&path).unwrap();
|
||||
let loaded = Config::load(&path).unwrap();
|
||||
assert_eq!(loaded, original);
|
||||
std::fs::remove_file(&path).unwrap();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user