use std::collections::VecDeque;
use std::time::{Duration, Instant};
use crate::bus::{CanBus, CanFrame, CanId};
use crate::error::{Error, Result};
use crate::protocol::{ControlMode, DEFAULT_MOTOR_ID};
use crate::protocols::robstride::{
ROBSTRIDE_CONFIG_BASE_ID, ROBSTRIDE_MIT_BASE_ID, ROBSTRIDE_POSITION_BASE_ID,
ROBSTRIDE_SPEED_BASE_ID, ROBSTRIDE_STATUS_BASE_ID,
};
pub const CAN_TRACE_CSV_HEADER: &str = "sequence,elapsed_us,frame_format,id,len,data_hex";
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum CanFrameClass {
CubeMarsMit { motor_id: u8 },
CubeMarsMitHelper { motor_id: u8 },
CubeMarsDirect { mode: ControlMode, motor_id: u8 },
RobStrideMit { motor_id: u8 },
RobStridePosition { motor_id: u8 },
RobStrideSpeed { motor_id: u8 },
RobStrideStatus { motor_id: u8 },
RobStrideConfig { motor_id: u8 },
Standard { id: u16 },
Extended { id: u32 },
}
impl CanFrameClass {
pub fn classify(frame: &CanFrame) -> Self {
match frame.id() {
CanId::Standard(id) => Self::classify_standard(id),
CanId::Extended(id) => Self::classify_extended(id, frame.len()),
}
}
pub fn label(self) -> String {
match self {
Self::CubeMarsMit { motor_id } => format!("CubeMars MIT motor=0x{motor_id:02X}"),
Self::CubeMarsMitHelper { motor_id } => {
format!("CubeMars helper motor=0x{motor_id:02X}")
}
Self::CubeMarsDirect { mode, motor_id } => {
format!("CubeMars {:?} motor=0x{motor_id:02X}", mode)
}
Self::RobStrideMit { motor_id } => format!("RobStride MIT motor=0x{motor_id:02X}"),
Self::RobStridePosition { motor_id } => {
format!("RobStride position motor=0x{motor_id:02X}")
}
Self::RobStrideSpeed { motor_id } => {
format!("RobStride speed motor=0x{motor_id:02X}")
}
Self::RobStrideStatus { motor_id } => {
format!("RobStride status motor=0x{motor_id:02X}")
}
Self::RobStrideConfig { motor_id } => {
format!("RobStride config motor=0x{motor_id:02X}")
}
Self::Standard { id } => format!("standard 0x{id:03X}"),
Self::Extended { id } => format!("extended 0x{id:08X}"),
}
}
const fn classify_standard(id: u16) -> Self {
if id <= u8::MAX as u16 {
Self::CubeMarsMit { motor_id: id as u8 }
} else {
Self::Standard { id }
}
}
const fn classify_extended(id: u32, len: usize) -> Self {
let high = (id >> 8) & 0xFF;
let low = id & 0xFF;
if len == 8 {
if id >= ROBSTRIDE_MIT_BASE_ID && id <= ROBSTRIDE_MIT_BASE_ID + u8::MAX as u32 {
return Self::RobStrideMit {
motor_id: (id - ROBSTRIDE_MIT_BASE_ID) as u8,
};
}
if id >= ROBSTRIDE_POSITION_BASE_ID && id <= ROBSTRIDE_POSITION_BASE_ID + u8::MAX as u32
{
return Self::RobStridePosition {
motor_id: (id - ROBSTRIDE_POSITION_BASE_ID) as u8,
};
}
if id >= ROBSTRIDE_SPEED_BASE_ID && id <= ROBSTRIDE_SPEED_BASE_ID + u8::MAX as u32 {
return Self::RobStrideSpeed {
motor_id: (id - ROBSTRIDE_SPEED_BASE_ID) as u8,
};
}
if id >= ROBSTRIDE_STATUS_BASE_ID && id <= ROBSTRIDE_STATUS_BASE_ID + u8::MAX as u32 {
return Self::RobStrideStatus {
motor_id: (id - ROBSTRIDE_STATUS_BASE_ID) as u8,
};
}
if id >= ROBSTRIDE_CONFIG_BASE_ID && id <= ROBSTRIDE_CONFIG_BASE_ID + u8::MAX as u32 {
return Self::RobStrideConfig {
motor_id: (id - ROBSTRIDE_CONFIG_BASE_ID) as u8,
};
}
}
if high == ControlMode::DutyCycle as u32 {
return Self::CubeMarsDirect {
mode: ControlMode::DutyCycle,
motor_id: low as u8,
};
}
if high == ControlMode::CurrentLoop as u32 {
return Self::CubeMarsDirect {
mode: ControlMode::CurrentLoop,
motor_id: low as u8,
};
}
if high == ControlMode::CurrentBrake as u32 {
return Self::CubeMarsDirect {
mode: ControlMode::CurrentBrake,
motor_id: low as u8,
};
}
if high == ControlMode::VelocityLoop as u32 {
return Self::CubeMarsDirect {
mode: ControlMode::VelocityLoop,
motor_id: low as u8,
};
}
if high == ControlMode::PositionLoop as u32 {
return Self::CubeMarsDirect {
mode: ControlMode::PositionLoop,
motor_id: low as u8,
};
}
if high == ControlMode::SetOrigin as u32 {
return Self::CubeMarsDirect {
mode: ControlMode::SetOrigin,
motor_id: low as u8,
};
}
if high == ControlMode::PositionVelocity as u32 {
return Self::CubeMarsDirect {
mode: ControlMode::PositionVelocity,
motor_id: low as u8,
};
}
if high == ControlMode::MitMode as u32 {
return Self::CubeMarsMit {
motor_id: low as u8,
};
}
if id <= u8::MAX as u32 {
return Self::CubeMarsMitHelper { motor_id: id as u8 };
}
Self::Extended { id }
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct FormattedCanFrame {
pub id: String,
pub data: String,
pub ascii: String,
pub len: usize,
pub class: String,
}
impl FormattedCanFrame {
pub fn from_frame(frame: &CanFrame) -> Self {
let id = match frame.id() {
CanId::Standard(id) => format!("{:03X}", id),
CanId::Extended(id) => format!("{:08X}", id),
};
let data = frame
.data()
.iter()
.map(|byte| format!("{byte:02X}"))
.collect::<Vec<_>>()
.join(" ");
let ascii = frame
.data()
.iter()
.map(|byte| {
if byte.is_ascii_graphic() || *byte == b' ' {
char::from(*byte)
} else {
'.'
}
})
.collect();
Self {
id,
data,
ascii,
len: frame.len(),
class: CanFrameClass::classify(frame).label(),
}
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct CanFrameRecord {
pub sequence: u64,
pub elapsed: Duration,
pub frame: CanFrame,
pub class: CanFrameClass,
}
impl CanFrameRecord {
pub fn formatted(&self) -> FormattedCanFrame {
FormattedCanFrame::from_frame(&self.frame)
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct CanTraceRecord {
pub sequence: u64,
pub elapsed: Duration,
pub frame: CanFrame,
}
impl CanTraceRecord {
pub fn from_record(record: &CanFrameRecord) -> Self {
Self {
sequence: record.sequence,
elapsed: record.elapsed,
frame: record.frame.clone(),
}
}
pub fn formatted(&self) -> FormattedCanFrame {
FormattedCanFrame::from_frame(&self.frame)
}
pub fn to_csv_row(&self) -> String {
let (frame_format, id) = match self.frame.id() {
CanId::Standard(id) => ("S", format!("{id:03X}")),
CanId::Extended(id) => ("E", format!("{id:08X}")),
};
format!(
"{},{},{},{},{},{}",
self.sequence,
duration_micros(self.elapsed),
frame_format,
id,
self.frame.len(),
data_hex(self.frame.data())
)
}
}
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
pub struct CanMonitorStats {
pub frames_seen: u64,
pub frames_retained: usize,
pub frames_dropped: u64,
pub standard_frames: u64,
pub extended_frames: u64,
pub bytes_seen: u64,
}
#[derive(Debug, Clone)]
pub struct CanMonitor {
capacity: usize,
started_at: Instant,
next_sequence: u64,
records: VecDeque<CanFrameRecord>,
stats: CanMonitorStats,
}
impl CanMonitor {
pub fn new(capacity: usize) -> Self {
Self {
capacity: capacity.max(1),
started_at: Instant::now(),
next_sequence: 0,
records: VecDeque::with_capacity(capacity.max(1)),
stats: CanMonitorStats::default(),
}
}
pub const fn capacity(&self) -> usize {
self.capacity
}
pub fn records(&self) -> &VecDeque<CanFrameRecord> {
&self.records
}
pub const fn stats(&self) -> CanMonitorStats {
self.stats
}
pub fn clear(&mut self) {
self.records.clear();
self.stats = CanMonitorStats::default();
self.next_sequence = 0;
self.started_at = Instant::now();
}
pub fn observe(&mut self, frame: CanFrame) -> &CanFrameRecord {
let elapsed = self.started_at.elapsed();
let class = CanFrameClass::classify(&frame);
if self.records.len() == self.capacity {
self.records.pop_front();
self.stats.frames_dropped = self.stats.frames_dropped.saturating_add(1);
}
self.stats.frames_seen = self.stats.frames_seen.saturating_add(1);
self.stats.frames_retained = self.records.len() + 1;
self.stats.bytes_seen = self.stats.bytes_seen.saturating_add(frame.len() as u64);
match frame.id() {
CanId::Standard(_) => {
self.stats.standard_frames = self.stats.standard_frames.saturating_add(1);
}
CanId::Extended(_) => {
self.stats.extended_frames = self.stats.extended_frames.saturating_add(1);
}
}
self.records.push_back(CanFrameRecord {
sequence: self.next_sequence,
elapsed,
frame,
class,
});
self.next_sequence = self.next_sequence.wrapping_add(1);
self.records.back().expect("record was just pushed")
}
pub fn poll_once<B>(
&mut self,
bus: &mut B,
timeout: Duration,
) -> Result<Option<&CanFrameRecord>>
where
B: CanBus,
{
match bus.receive(timeout)? {
Some(frame) => Ok(Some(self.observe(frame))),
None => Ok(None),
}
}
pub fn poll_available<B>(
&mut self,
bus: &mut B,
timeout: Duration,
max_frames: usize,
) -> Result<usize>
where
B: CanBus,
{
let mut received = 0;
for _ in 0..max_frames {
let timeout = if received == 0 {
timeout
} else {
Duration::ZERO
};
if self.poll_once(bus, timeout)?.is_some() {
received += 1;
} else {
break;
}
}
Ok(received)
}
pub fn rows(&self) -> Vec<String> {
self.records
.iter()
.rev()
.map(|record| {
let formatted = record.formatted();
format!(
"#{:<5} {:>8.3}s {:<8} len={} {:<23} {}",
record.sequence,
record.elapsed.as_secs_f64(),
formatted.id,
formatted.len,
formatted.data,
record.class.label()
)
})
.collect()
}
pub fn trace_records(&self) -> Vec<CanTraceRecord> {
self.records
.iter()
.map(CanTraceRecord::from_record)
.collect()
}
pub fn to_trace_csv(&self) -> String {
let mut csv = String::from(CAN_TRACE_CSV_HEADER);
csv.push('\n');
for record in &self.records {
csv.push_str(&CanTraceRecord::from_record(record).to_csv_row());
csv.push('\n');
}
csv
}
}
impl Default for CanMonitor {
fn default() -> Self {
Self::new(256)
}
}
pub fn sample_can_monitor() -> Result<CanMonitor> {
let mut monitor = CanMonitor::new(32);
monitor.observe(CanFrame::new(
ControlMode::MitMode.extended_id(DEFAULT_MOTOR_ID)?,
&[0x80, 0x00, 0x7F, 0xF0, 0x00, 0x00, 0x00, 0x00],
)?);
monitor.observe(CanFrame::new_padded(
CanId::extended(ROBSTRIDE_STATUS_BASE_ID + 1)?,
&[0xE2, 0x04, 0x00, 0x00, 0x10, 0x00, 0x2A, 0x00],
)?);
monitor.observe(CanFrame::new(CanId::standard(0x123)?, b"kcan")?);
Ok(monitor)
}
pub fn parse_can_trace_csv(input: &str) -> Result<Vec<CanTraceRecord>> {
let mut records = Vec::new();
for (index, raw_line) in input.lines().enumerate() {
let line_number = index + 1;
let line = raw_line.trim();
if line.is_empty() || line == CAN_TRACE_CSV_HEADER {
continue;
}
let fields = line.split(',').map(str::trim).collect::<Vec<_>>();
if fields.len() != 6 {
return Err(trace_error(
line_number,
format!("expected 6 columns, got {}", fields.len()),
));
}
let sequence = parse_u64_field(fields[0], line_number, "sequence")?;
let elapsed_us = parse_u64_field(fields[1], line_number, "elapsed_us")?;
let id = parse_can_trace_id(fields[2], fields[3], line_number)?;
let len = parse_usize_field(fields[4], line_number, "len")?;
let data = parse_trace_data(fields[5], len, line_number)?;
let frame = CanFrame::new(id, &data)?;
records.push(CanTraceRecord {
sequence,
elapsed: Duration::from_micros(elapsed_us),
frame,
});
}
Ok(records)
}
pub fn replay_can_trace<B>(bus: &mut B, trace: &[CanTraceRecord]) -> Result<usize>
where
B: CanBus,
{
for record in trace {
bus.send(&record.frame)?;
}
Ok(trace.len())
}
fn duration_micros(duration: Duration) -> u64 {
let micros = duration.as_micros();
if micros > u64::MAX as u128 {
u64::MAX
} else {
micros as u64
}
}
fn data_hex(data: &[u8]) -> String {
data.iter()
.map(|byte| format!("{byte:02X}"))
.collect::<Vec<_>>()
.join("")
}
fn parse_can_trace_id(frame_format: &str, id: &str, line_number: usize) -> Result<CanId> {
let raw_id = parse_hex_u32_field(id, line_number, "id")?;
match frame_format {
"S" | "s" => {
if raw_id > u16::MAX as u32 {
return Err(trace_error(
line_number,
format!("standard id 0x{raw_id:X} exceeds 11-bit range"),
));
}
CanId::standard(raw_id as u16).map_err(|err| match err {
Error::InvalidCanId(_) => trace_error(
line_number,
format!("standard id 0x{raw_id:X} exceeds 11-bit range"),
),
other => other,
})
}
"E" | "e" => CanId::extended(raw_id).map_err(|err| match err {
Error::InvalidCanId(_) => trace_error(
line_number,
format!("extended id 0x{raw_id:X} exceeds 29-bit range"),
),
other => other,
}),
other => Err(trace_error(
line_number,
format!("frame_format must be S or E, got {other:?}"),
)),
}
}
fn parse_trace_data(data_hex: &str, len: usize, line_number: usize) -> Result<Vec<u8>> {
if len > crate::bus::MAX_CAN_DATA_LEN {
return Err(trace_error(
line_number,
format!("len {len} exceeds {} bytes", crate::bus::MAX_CAN_DATA_LEN),
));
}
if data_hex.len() != len * 2 {
return Err(trace_error(
line_number,
format!(
"data_hex length {} does not match len {len}",
data_hex.len()
),
));
}
let mut data = Vec::with_capacity(len);
for index in 0..len {
let start = index * 2;
let end = start + 2;
let byte = u8::from_str_radix(&data_hex[start..end], 16).map_err(|err| {
trace_error(
line_number,
format!("invalid data byte {:?}: {err}", &data_hex[start..end]),
)
})?;
data.push(byte);
}
Ok(data)
}
fn parse_u64_field(value: &str, line_number: usize, field: &str) -> Result<u64> {
value.parse::<u64>().map_err(|err| {
trace_error(
line_number,
format!("invalid {field} value {value:?}: {err}"),
)
})
}
fn parse_usize_field(value: &str, line_number: usize, field: &str) -> Result<usize> {
value.parse::<usize>().map_err(|err| {
trace_error(
line_number,
format!("invalid {field} value {value:?}: {err}"),
)
})
}
fn parse_hex_u32_field(value: &str, line_number: usize, field: &str) -> Result<u32> {
let value = value
.strip_prefix("0x")
.or_else(|| value.strip_prefix("0X"))
.unwrap_or(value);
u32::from_str_radix(value, 16).map_err(|err| {
trace_error(
line_number,
format!("invalid {field} hex value {value:?}: {err}"),
)
})
}
fn trace_error(line: usize, reason: String) -> Error {
Error::InvalidTraceLine { line, reason }
}