use core::fmt;
use crate::{EncodedFrame, FrameId, FramePayload, FrameRef};
const COMMAND_MASK: u16 = 0x1f;
const MAX_STANDARD_ID: u16 = 0x07ff;
const MAX_NODE_ID: u32 = 63;
const HEARTBEAT: u8 = 0x01;
const ESTOP: u8 = 0x02;
const MOTOR_ERROR: u8 = 0x03;
const ENCODER_ERROR: u8 = 0x04;
const SENSORLESS_ERROR: u8 = 0x05;
const SET_AXIS_NODE_ID: u8 = 0x06;
const SET_AXIS_REQUESTED_STATE: u8 = 0x07;
const SET_AXIS_STARTUP_CONFIG: u8 = 0x08;
const ENCODER_ESTIMATES: u8 = 0x09;
const ENCODER_COUNT: u8 = 0x0a;
const SET_CONTROLLER_MODES: u8 = 0x0b;
const SET_INPUT_POS: u8 = 0x0c;
const SET_INPUT_VEL: u8 = 0x0d;
const SET_INPUT_TORQUE: u8 = 0x0e;
const SET_VELOCITY_LIMIT: u8 = 0x0f;
const START_ANTICOGGING: u8 = 0x10;
const SET_TRAJ_VEL_LIMIT: u8 = 0x11;
const SET_TRAJ_ACCEL_LIMITS: u8 = 0x12;
const SET_TRAJ_INERTIA: u8 = 0x13;
const IQ: u8 = 0x14;
const SENSORLESS_ESTIMATES: u8 = 0x15;
const REBOOT: u8 = 0x16;
const VBUS_VOLTAGE: u8 = 0x17;
const CLEAR_ERRORS: u8 = 0x18;
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct NodeId(u8);
impl NodeId {
pub const fn new(value: u32) -> Result<Self, EncodeError> {
if value <= MAX_NODE_ID {
Ok(Self(value as u8))
} else {
Err(EncodeError::InvalidNodeId(value))
}
}
pub const fn get(self) -> u8 {
self.0
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct AxisState(pub u32);
impl AxisState {
pub const UNDEFINED: Self = Self(0);
pub const IDLE: Self = Self(1);
pub const STARTUP_SEQUENCE: Self = Self(2);
pub const FULL_CALIBRATION_SEQUENCE: Self = Self(3);
pub const MOTOR_CALIBRATION: Self = Self(4);
pub const SENSORLESS_CONTROL: Self = Self(5);
pub const ENCODER_INDEX_SEARCH: Self = Self(6);
pub const ENCODER_OFFSET_CALIBRATION: Self = Self(7);
pub const CLOSED_LOOP_CONTROL: Self = Self(8);
pub const LOCKIN_SPIN: Self = Self(9);
pub const ENCODER_DIR_FIND: Self = Self(10);
pub const HOMING: Self = Self(11);
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum Command {
Estop,
SetAxisNodeId {
node_id: NodeId,
},
SetAxisRequestedState {
state: AxisState,
},
SetControllerModes {
control_mode: i32,
input_mode: i32,
},
SetInputPos {
position: f32,
velocity_ff: i16,
torque_ff: i16,
},
SetInputVel {
velocity: f32,
torque_ff: f32,
},
SetInputTorque {
torque: f32,
},
SetVelocityLimit {
velocity: f32,
},
StartAnticogging,
SetTrajVelLimit {
velocity: f32,
},
SetTrajAccelLimits {
acceleration: f32,
deceleration: f32,
},
SetTrajInertia {
inertia: f32,
},
Reboot,
ClearErrors,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Query {
MotorError,
EncoderError,
SensorlessError,
EncoderEstimates,
EncoderCount,
Iq,
SensorlessEstimates,
VbusVoltage,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum Response {
Heartbeat {
axis_error: u32,
axis_state: AxisState,
},
MotorError(u32),
EncoderError(u32),
SensorlessError(u32),
EncoderEstimates {
position: f32,
velocity: f32,
},
EncoderCount {
shadow_count: i32,
count_in_cpr: i32,
},
Iq {
setpoint: f32,
measured: f32,
},
SensorlessEstimates {
position: f32,
velocity: f32,
},
VbusVoltage(f32),
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum Message {
Command(Command),
Request(Query),
Response(Response),
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum EncodeError {
InvalidNodeId(u32),
NonFinite {
field: &'static str,
},
}
impl fmt::Display for EncodeError {
fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::InvalidNodeId(value) => write!(formatter, "invalid ODrive CAN node id: {value}"),
Self::NonFinite { field } => write!(formatter, "non-finite CAN command field: {field}"),
}
}
}
impl core::error::Error for EncodeError {}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum DecodeError {
InvalidStandardId(u16),
UnsupportedExtendedId,
UnsupportedCanFd,
UnsupportedCommand {
command_id: u8,
},
InvalidFrameKind {
command_id: u8,
},
InvalidLength {
command_id: u8,
expected: u8,
actual: usize,
},
InvalidRemoteDlc {
dlc: u8,
},
InvalidNodeId(u32),
}
impl fmt::Display for DecodeError {
fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::InvalidStandardId(id) => {
write!(formatter, "standard CAN id exceeds 11 bits: {id}")
}
Self::UnsupportedExtendedId => formatter.write_str("extended CAN id is unsupported"),
Self::UnsupportedCanFd => formatter.write_str("CAN FD is unsupported"),
Self::UnsupportedCommand { command_id } => {
write!(formatter, "unsupported CAN command: {command_id:#04x}")
}
Self::InvalidFrameKind { command_id } => write!(
formatter,
"invalid frame kind for CAN command: {command_id:#04x}"
),
Self::InvalidLength {
command_id,
expected,
actual,
} => write!(
formatter,
"invalid length for CAN command {command_id:#04x}: expected {expected}, got {actual}"
),
Self::InvalidRemoteDlc { dlc } => {
write!(formatter, "invalid classic CAN RTR DLC: {dlc}")
}
Self::InvalidNodeId(node_id) => {
write!(formatter, "invalid ODrive CAN node id in frame: {node_id}")
}
}
}
}
impl core::error::Error for DecodeError {}
pub fn encode(node: NodeId, message: Message) -> Result<EncodedFrame, EncodeError> {
let (command_id, data, len, rtr) = match message {
Message::Command(command) => encode_command(command)?,
Message::Request(query) => (query_id(query), [0; 8], 8, true),
Message::Response(response) => encode_response(response),
};
let id = (u16::from(node.get()) << 5) | u16::from(command_id);
Ok(EncodedFrame::new(id, data, len, rtr))
}
pub fn decode(node: NodeId, frame: FrameRef<'_>) -> Result<Option<Message>, DecodeError> {
let id = match frame.id {
FrameId::Standard(id) if id <= MAX_STANDARD_ID => id,
FrameId::Standard(id) => return Err(DecodeError::InvalidStandardId(id)),
FrameId::Extended(_) => return Err(DecodeError::UnsupportedExtendedId),
};
if u32::from(id >> 5) != u32::from(node.get()) {
return Ok(None);
}
let command_id = (id & COMMAND_MASK) as u8;
if command_id == SET_AXIS_STARTUP_CONFIG {
return Err(DecodeError::UnsupportedCommand { command_id });
}
if !is_known(command_id) {
return Ok(None);
}
match frame.payload {
FramePayload::Fd(_) => Err(DecodeError::UnsupportedCanFd),
FramePayload::Remote { dlc } => decode_remote(command_id, dlc),
FramePayload::Data(data) => decode_data(command_id, data),
}
}
fn encode_command(command: Command) -> Result<(u8, [u8; 8], u8, bool), EncodeError> {
let mut data = [0; 8];
let (command_id, len) = match command {
Command::Estop => (ESTOP, 0),
Command::SetAxisNodeId { node_id } => {
put_u32(&mut data, 0, u32::from(node_id.get()));
(SET_AXIS_NODE_ID, 4)
}
Command::SetAxisRequestedState { state } => {
put_u32(&mut data, 0, state.0);
(SET_AXIS_REQUESTED_STATE, 4)
}
Command::SetControllerModes {
control_mode,
input_mode,
} => {
put_u32(&mut data, 0, control_mode as u32);
put_u32(&mut data, 4, input_mode as u32);
(SET_CONTROLLER_MODES, 8)
}
Command::SetInputPos {
position,
velocity_ff,
torque_ff,
} => {
finite(position, "position")?;
put_f32(&mut data, 0, position);
put_i16(&mut data, 4, velocity_ff);
put_i16(&mut data, 6, torque_ff);
(SET_INPUT_POS, 8)
}
Command::SetInputVel {
velocity,
torque_ff,
} => {
finite(velocity, "velocity")?;
finite(torque_ff, "torque_ff")?;
put_f32(&mut data, 0, velocity);
put_f32(&mut data, 4, torque_ff);
(SET_INPUT_VEL, 8)
}
Command::SetInputTorque { torque } => {
finite(torque, "torque")?;
put_f32(&mut data, 0, torque);
(SET_INPUT_TORQUE, 4)
}
Command::SetVelocityLimit { velocity } => {
finite(velocity, "velocity")?;
put_f32(&mut data, 0, velocity);
(SET_VELOCITY_LIMIT, 4)
}
Command::StartAnticogging => (START_ANTICOGGING, 0),
Command::SetTrajVelLimit { velocity } => {
finite(velocity, "velocity")?;
put_f32(&mut data, 0, velocity);
(SET_TRAJ_VEL_LIMIT, 4)
}
Command::SetTrajAccelLimits {
acceleration,
deceleration,
} => {
finite(acceleration, "acceleration")?;
finite(deceleration, "deceleration")?;
put_f32(&mut data, 0, acceleration);
put_f32(&mut data, 4, deceleration);
(SET_TRAJ_ACCEL_LIMITS, 8)
}
Command::SetTrajInertia { inertia } => {
finite(inertia, "inertia")?;
put_f32(&mut data, 0, inertia);
(SET_TRAJ_INERTIA, 4)
}
Command::Reboot => (REBOOT, 0),
Command::ClearErrors => (CLEAR_ERRORS, 0),
};
Ok((command_id, data, len, false))
}
fn encode_response(response: Response) -> (u8, [u8; 8], u8, bool) {
let mut data = [0; 8];
let command_id = match response {
Response::Heartbeat {
axis_error,
axis_state,
} => {
put_u32(&mut data, 0, axis_error);
put_u32(&mut data, 4, axis_state.0);
HEARTBEAT
}
Response::MotorError(error) => {
put_u32(&mut data, 0, error);
MOTOR_ERROR
}
Response::EncoderError(error) => {
put_u32(&mut data, 0, error);
ENCODER_ERROR
}
Response::SensorlessError(error) => {
put_u32(&mut data, 0, error);
SENSORLESS_ERROR
}
Response::EncoderEstimates { position, velocity } => {
put_f32(&mut data, 0, position);
put_f32(&mut data, 4, velocity);
ENCODER_ESTIMATES
}
Response::EncoderCount {
shadow_count,
count_in_cpr,
} => {
put_u32(&mut data, 0, shadow_count as u32);
put_u32(&mut data, 4, count_in_cpr as u32);
ENCODER_COUNT
}
Response::Iq { setpoint, measured } => {
put_f32(&mut data, 0, setpoint);
put_f32(&mut data, 4, measured);
IQ
}
Response::SensorlessEstimates { position, velocity } => {
put_f32(&mut data, 0, position);
put_f32(&mut data, 4, velocity);
SENSORLESS_ESTIMATES
}
Response::VbusVoltage(voltage) => {
put_f32(&mut data, 0, voltage);
VBUS_VOLTAGE
}
};
(command_id, data, 8, false)
}
fn decode_remote(command_id: u8, dlc: u8) -> Result<Option<Message>, DecodeError> {
if dlc > 8 {
return Err(DecodeError::InvalidRemoteDlc { dlc });
}
match query_from_id(command_id) {
Some(query) => Ok(Some(Message::Request(query))),
None => Err(DecodeError::InvalidFrameKind { command_id }),
}
}
fn decode_data(command_id: u8, data: &[u8]) -> Result<Option<Message>, DecodeError> {
let message = match command_id {
HEARTBEAT => {
require_len(command_id, data, 8)?;
Message::Response(Response::Heartbeat {
axis_error: u32_at(data, 0),
axis_state: AxisState(u32_at(data, 4)),
})
}
ESTOP => {
require_len(command_id, data, 0)?;
Message::Command(Command::Estop)
}
MOTOR_ERROR => Message::Response(Response::MotorError(decode_error_response(
command_id, data,
)?)),
ENCODER_ERROR => Message::Response(Response::EncoderError(decode_error_response(
command_id, data,
)?)),
SENSORLESS_ERROR => Message::Response(Response::SensorlessError(decode_error_response(
command_id, data,
)?)),
SET_AXIS_NODE_ID => {
require_len(command_id, data, 4)?;
let node_id = u32_at(data, 0);
let node_id = NodeId::new(node_id).map_err(|_| DecodeError::InvalidNodeId(node_id))?;
Message::Command(Command::SetAxisNodeId { node_id })
}
SET_AXIS_REQUESTED_STATE => {
require_len(command_id, data, 4)?;
Message::Command(Command::SetAxisRequestedState {
state: AxisState(u32_at(data, 0)),
})
}
ENCODER_ESTIMATES => {
require_len(command_id, data, 8)?;
Message::Response(Response::EncoderEstimates {
position: f32_at(data, 0),
velocity: f32_at(data, 4),
})
}
ENCODER_COUNT => {
require_len(command_id, data, 8)?;
Message::Response(Response::EncoderCount {
shadow_count: u32_at(data, 0) as i32,
count_in_cpr: u32_at(data, 4) as i32,
})
}
SET_CONTROLLER_MODES => {
require_len(command_id, data, 8)?;
Message::Command(Command::SetControllerModes {
control_mode: u32_at(data, 0) as i32,
input_mode: u32_at(data, 4) as i32,
})
}
SET_INPUT_POS => {
require_len(command_id, data, 8)?;
Message::Command(Command::SetInputPos {
position: f32_at(data, 0),
velocity_ff: i16_at(data, 4),
torque_ff: i16_at(data, 6),
})
}
SET_INPUT_VEL => {
require_len(command_id, data, 8)?;
Message::Command(Command::SetInputVel {
velocity: f32_at(data, 0),
torque_ff: f32_at(data, 4),
})
}
SET_INPUT_TORQUE => {
require_len(command_id, data, 4)?;
Message::Command(Command::SetInputTorque {
torque: f32_at(data, 0),
})
}
SET_VELOCITY_LIMIT => {
require_len(command_id, data, 4)?;
Message::Command(Command::SetVelocityLimit {
velocity: f32_at(data, 0),
})
}
START_ANTICOGGING => {
require_len(command_id, data, 0)?;
Message::Command(Command::StartAnticogging)
}
SET_TRAJ_VEL_LIMIT => {
require_len(command_id, data, 4)?;
Message::Command(Command::SetTrajVelLimit {
velocity: f32_at(data, 0),
})
}
SET_TRAJ_ACCEL_LIMITS => {
require_len(command_id, data, 8)?;
Message::Command(Command::SetTrajAccelLimits {
acceleration: f32_at(data, 0),
deceleration: f32_at(data, 4),
})
}
SET_TRAJ_INERTIA => {
require_len(command_id, data, 4)?;
Message::Command(Command::SetTrajInertia {
inertia: f32_at(data, 0),
})
}
IQ => {
require_len(command_id, data, 8)?;
Message::Response(Response::Iq {
setpoint: f32_at(data, 0),
measured: f32_at(data, 4),
})
}
SENSORLESS_ESTIMATES => {
require_len(command_id, data, 8)?;
Message::Response(Response::SensorlessEstimates {
position: f32_at(data, 0),
velocity: f32_at(data, 4),
})
}
REBOOT => {
require_len(command_id, data, 0)?;
Message::Command(Command::Reboot)
}
VBUS_VOLTAGE => Message::Response(Response::VbusVoltage(decode_vbus_voltage(data)?)),
CLEAR_ERRORS => {
require_len(command_id, data, 0)?;
Message::Command(Command::ClearErrors)
}
_ => return Ok(None),
};
Ok(Some(message))
}
fn decode_error_response(command_id: u8, data: &[u8]) -> Result<u32, DecodeError> {
require_len(command_id, data, 8)?;
Ok(u32_at(data, 0))
}
fn decode_vbus_voltage(data: &[u8]) -> Result<f32, DecodeError> {
require_len(VBUS_VOLTAGE, data, 8)?;
Ok(f32_at(data, 0))
}
fn is_known(command_id: u8) -> bool {
matches!(
command_id,
HEARTBEAT
| ESTOP
| MOTOR_ERROR
| ENCODER_ERROR
| SENSORLESS_ERROR
| SET_AXIS_NODE_ID
| SET_AXIS_REQUESTED_STATE
| ENCODER_ESTIMATES
| ENCODER_COUNT
| SET_CONTROLLER_MODES
| SET_INPUT_POS
| SET_INPUT_VEL
| SET_INPUT_TORQUE
| SET_VELOCITY_LIMIT
| START_ANTICOGGING
| SET_TRAJ_VEL_LIMIT
| SET_TRAJ_ACCEL_LIMITS
| SET_TRAJ_INERTIA
| IQ
| SENSORLESS_ESTIMATES
| REBOOT
| VBUS_VOLTAGE
| CLEAR_ERRORS
)
}
fn query_id(query: Query) -> u8 {
match query {
Query::MotorError => MOTOR_ERROR,
Query::EncoderError => ENCODER_ERROR,
Query::SensorlessError => SENSORLESS_ERROR,
Query::EncoderEstimates => ENCODER_ESTIMATES,
Query::EncoderCount => ENCODER_COUNT,
Query::Iq => IQ,
Query::SensorlessEstimates => SENSORLESS_ESTIMATES,
Query::VbusVoltage => VBUS_VOLTAGE,
}
}
fn query_from_id(command_id: u8) -> Option<Query> {
match command_id {
MOTOR_ERROR => Some(Query::MotorError),
ENCODER_ERROR => Some(Query::EncoderError),
SENSORLESS_ERROR => Some(Query::SensorlessError),
ENCODER_ESTIMATES => Some(Query::EncoderEstimates),
ENCODER_COUNT => Some(Query::EncoderCount),
IQ => Some(Query::Iq),
SENSORLESS_ESTIMATES => Some(Query::SensorlessEstimates),
VBUS_VOLTAGE => Some(Query::VbusVoltage),
_ => None,
}
}
fn finite(value: f32, field: &'static str) -> Result<(), EncodeError> {
if value.is_finite() {
Ok(())
} else {
Err(EncodeError::NonFinite { field })
}
}
fn require_len(command_id: u8, data: &[u8], expected: u8) -> Result<(), DecodeError> {
let actual = data.len();
if actual == usize::from(expected) {
Ok(())
} else {
Err(DecodeError::InvalidLength {
command_id,
expected,
actual,
})
}
}
fn put_u32(data: &mut [u8; 8], offset: usize, value: u32) {
data[offset..offset + 4].copy_from_slice(&value.to_le_bytes());
}
fn put_i16(data: &mut [u8; 8], offset: usize, value: i16) {
data[offset..offset + 2].copy_from_slice(&value.to_le_bytes());
}
fn put_f32(data: &mut [u8; 8], offset: usize, value: f32) {
put_u32(data, offset, value.to_bits());
}
fn u32_at(data: &[u8], offset: usize) -> u32 {
u32::from_le_bytes([
data[offset],
data[offset + 1],
data[offset + 2],
data[offset + 3],
])
}
fn i16_at(data: &[u8], offset: usize) -> i16 {
i16::from_le_bytes([data[offset], data[offset + 1]])
}
fn f32_at(data: &[u8], offset: usize) -> f32 {
f32::from_bits(u32_at(data, offset))
}