use kcan::{
CanFrame, CanId, Error, KNOTT_DYNAMICS_DEFAULT_BITRATE, KNOTT_DYNAMICS_MAX_NODE_ID,
KNOTT_DYNAMICS_MIN_NODE_ID, KNOTT_DYNAMICS_PARAMETERS, KnottDynamicsFault, KnottDynamicsFaults,
KnottDynamicsFeedbackKind, KnottDynamicsFlashCommand, KnottDynamicsFunction, KnottDynamicsMode,
KnottDynamicsModel, KnottDynamicsParameter, KnottDynamicsParameterAccess,
KnottDynamicsParameterType, KnottDynamicsParameterValue, KnottDynamicsWriteRequirement,
is_knott_dynamics_frame_shape, knott_dynamics_disable_frame, knott_dynamics_flash_frame,
knott_dynamics_flash_frame_in_mode, knott_dynamics_heartbeat_frame, knott_dynamics_nmt_frame,
knott_dynamics_nmt_frame_from_mode, knott_dynamics_opaque_watchdog_frame,
knott_dynamics_parameter_read_frame, knott_dynamics_parameter_write_frame,
knott_dynamics_parameter_write_frame_in_mode, knott_dynamics_position_torque_frame,
knott_dynamics_position_velocity_frame, knott_dynamics_probe_frame,
parse_knott_dynamics_feedback, parse_knott_dynamics_opaque_watchdog_response,
parse_knott_dynamics_parameter_response, parse_knott_dynamics_probe_response, validate_node_id,
};
#[test]
fn models_and_protocol_constants_are_stable() {
assert_eq!(KNOTT_DYNAMICS_DEFAULT_BITRATE, 1_000_000);
assert_eq!(KNOTT_DYNAMICS_MIN_NODE_ID, 1);
assert_eq!(KNOTT_DYNAMICS_MAX_NODE_ID, 63);
assert_eq!(KnottDynamicsModel::all().len(), 2);
assert_eq!(
"k00".parse::<KnottDynamicsModel>().unwrap(),
KnottDynamicsModel::K00
);
assert_eq!(
"K-01".parse::<KnottDynamicsModel>().unwrap(),
KnottDynamicsModel::K01
);
assert_eq!(KnottDynamicsModel::K00.size_class(), "5010-class");
assert_eq!(KnottDynamicsModel::K01.size_class(), "6512-class");
}
#[test]
fn function_ids_use_standard_can_and_node_offsets() -> kcan::Result<()> {
assert_eq!(KnottDynamicsFunction::Probe.can_id(1)?.raw(), 0x081);
assert_eq!(KnottDynamicsFunction::TransmitPdo2.can_id(2)?.raw(), 0x282);
assert_eq!(KnottDynamicsFunction::ReceivePdo2.can_id(1)?.raw(), 0x301);
assert_eq!(KnottDynamicsFunction::Heartbeat.can_id(63)?.raw(), 0x73F);
assert_eq!(
KnottDynamicsFunction::from_standard_id(0x382),
Some((KnottDynamicsFunction::TransmitPdo3, 2))
);
assert_eq!(KnottDynamicsFunction::from_standard_id(0x080), None);
assert_eq!(KnottDynamicsFunction::from_standard_id(0x740), None);
Ok(())
}
#[test]
fn mode_transition_graph_matches_firmware() {
assert!(KnottDynamicsMode::Disabled.can_transition_to(KnottDynamicsMode::Idle));
assert!(!KnottDynamicsMode::Disabled.can_transition_to(KnottDynamicsMode::Current));
assert!(KnottDynamicsMode::Idle.can_transition_to(KnottDynamicsMode::Current));
assert!(KnottDynamicsMode::Current.can_transition_to(KnottDynamicsMode::Torque));
assert!(KnottDynamicsMode::Current.can_transition_to(KnottDynamicsMode::Debug));
assert!(!KnottDynamicsMode::Debug.can_transition_to(KnottDynamicsMode::Position));
assert!(!KnottDynamicsMode::Idle.can_transition_to(KnottDynamicsMode::Calibration));
assert!(KnottDynamicsMode::Calibration.can_transition_to(KnottDynamicsMode::Disabled));
}
#[test]
fn node_validation_rejects_broadcast_and_out_of_range_device_ids() {
assert_eq!(validate_node_id(0), Err(Error::InvalidNodeId(0)));
assert_eq!(validate_node_id(64), Err(Error::InvalidNodeId(64)));
assert!(validate_node_id(1).is_ok());
assert!(validate_node_id(63).is_ok());
}
#[test]
fn nmt_probe_heartbeat_flash_and_watchdog_frames_match_contract() -> kcan::Result<()> {
let broadcast = knott_dynamics_nmt_frame(None, KnottDynamicsMode::Idle)?;
assert_eq!(broadcast.id(), CanId::Standard(0));
assert_eq!(broadcast.data(), &[0x01, 0x00]);
let addressed = knott_dynamics_nmt_frame(Some(7), KnottDynamicsMode::Position)?;
assert_eq!(addressed.id(), CanId::Standard(0));
assert_eq!(addressed.data(), &[0x13, 0x07]);
let disable = knott_dynamics_disable_frame(Some(7))?;
assert_eq!(disable.id(), CanId::Standard(0));
assert_eq!(disable.data(), &[0x00, 0x07]);
assert_eq!(
knott_dynamics_nmt_frame(None, KnottDynamicsMode::Calibration),
Err(Error::UnsupportedMode(0x05))
);
let probe = knott_dynamics_probe_frame(1)?;
assert_eq!(probe.id(), CanId::Standard(0x081));
assert!(probe.is_empty());
let heartbeat = knott_dynamics_heartbeat_frame(1)?;
assert_eq!(heartbeat.id(), CanId::Standard(0x701));
assert!(heartbeat.is_empty());
let flash = knott_dynamics_flash_frame(1, KnottDynamicsFlashCommand::Store)?;
assert_eq!(flash.id(), CanId::Standard(0x681));
assert_eq!(flash.data(), &[1]);
let payload = [1, 2, 3, 4, 5, 6, 7, 8];
let watchdog = knott_dynamics_opaque_watchdog_frame(1, payload)?;
assert_eq!(watchdog.id(), CanId::Standard(0x201));
assert_eq!(watchdog.data(), payload);
Ok(())
}
#[test]
fn checked_nmt_and_flash_builders_reject_unsafe_modes() -> kcan::Result<()> {
assert_eq!(
knott_dynamics_nmt_frame_from_mode(
Some(1),
KnottDynamicsMode::Disabled,
KnottDynamicsMode::Torque,
),
Err(Error::InvalidModeTransition {
from: KnottDynamicsMode::Disabled as u8,
to: KnottDynamicsMode::Torque as u8,
})
);
let active = knott_dynamics_nmt_frame_from_mode(
Some(1),
KnottDynamicsMode::Idle,
KnottDynamicsMode::Torque,
)?;
assert_eq!(active.data(), &[KnottDynamicsMode::Torque as u8, 1]);
assert_eq!(
knott_dynamics_flash_frame_in_mode(
1,
KnottDynamicsMode::Position,
KnottDynamicsFlashCommand::Store,
),
Err(Error::UnsafeOperation {
operation: "flash operation",
required: "disabled or idle mode with PWM disabled",
actual_mode: KnottDynamicsMode::Position as u8,
})
);
let flash = knott_dynamics_flash_frame_in_mode(
1,
KnottDynamicsMode::Idle,
KnottDynamicsFlashCommand::Load,
)?;
assert_eq!(flash.data(), &[2]);
Ok(())
}
#[test]
fn position_command_frames_are_finite_little_endian_f32() -> kcan::Result<()> {
let position_velocity = knott_dynamics_position_velocity_frame(1, 1.0, -2.0)?;
assert_eq!(position_velocity.id(), CanId::Standard(0x301));
assert_eq!(
position_velocity.data(),
&[0x00, 0x00, 0x80, 0x3F, 0x00, 0x00, 0x00, 0xC0]
);
let position_torque = knott_dynamics_position_torque_frame(2, -0.5, 3.25)?;
assert_eq!(position_torque.id(), CanId::Standard(0x402));
assert_eq!(&position_torque.data()[..4], &(-0.5_f32).to_le_bytes());
assert_eq!(&position_torque.data()[4..], &3.25_f32.to_le_bytes());
assert_eq!(
knott_dynamics_position_velocity_frame(1, f32::NAN, 0.0),
Err(Error::NonFiniteValue("position_rad"))
);
assert_eq!(
knott_dynamics_position_torque_frame(1, 0.0, f32::INFINITY),
Err(Error::NonFiniteValue("torque_nm"))
);
Ok(())
}
#[test]
fn typed_parameter_registry_and_sdo_frames_match_firmware() -> kcan::Result<()> {
assert_eq!(KNOTT_DYNAMICS_PARAMETERS.len(), 80);
for (index, descriptor) in KNOTT_DYNAMICS_PARAMETERS.iter().enumerate() {
assert_eq!(
KnottDynamicsParameter::try_from(descriptor.parameter as u16)?,
descriptor.parameter
);
assert_eq!(descriptor.parameter.descriptor(), descriptor);
if index > 0 {
assert!(
descriptor.parameter as u16 > KNOTT_DYNAMICS_PARAMETERS[index - 1].parameter as u16
);
}
}
for reserved in [0x114, 0x118, 0x148] {
assert!(matches!(
KnottDynamicsParameter::try_from(reserved),
Err(Error::UnknownParameter(_))
));
}
assert_eq!(
KnottDynamicsParameter::FirmwareVersion
.descriptor()
.parameter_type,
KnottDynamicsParameterType::U32
);
assert_eq!(
KnottDynamicsParameter::FirmwareVersion.descriptor().access,
KnottDynamicsParameterAccess::ReadOnly
);
assert_eq!(
"torque-limit".parse::<KnottDynamicsParameter>()?,
KnottDynamicsParameter::TorqueLimit
);
assert_eq!(
"0x144".parse::<KnottDynamicsParameter>()?,
KnottDynamicsParameter::EncoderFluxTableValue
);
let read = knott_dynamics_parameter_read_frame(1, KnottDynamicsParameter::FirmwareVersion)?;
assert_eq!(read.id(), CanId::Standard(0x601));
assert_eq!(read.data(), &[0x40, 0x04, 0x00, 0x00]);
let write = knott_dynamics_parameter_write_frame(
1,
KnottDynamicsParameter::TorqueLimit,
KnottDynamicsParameterValue::F32(2.5),
)?;
assert_eq!(write.id(), CanId::Standard(0x601));
assert_eq!(&write.data()[..4], &[0x20, 0x30, 0x00, 0x00]);
assert_eq!(&write.data()[4..], &2.5_f32.to_le_bytes());
Ok(())
}
#[test]
fn parameter_writes_reject_read_only_wrong_type_and_unsafe_values() {
assert_eq!(
knott_dynamics_parameter_write_frame(
1,
KnottDynamicsParameter::FirmwareVersion,
KnottDynamicsParameterValue::U32(1),
),
Err(Error::ReadOnlyParameter("firmware_version"))
);
assert_eq!(
knott_dynamics_parameter_write_frame(
1,
KnottDynamicsParameter::TorqueLimit,
KnottDynamicsParameterValue::U32(1),
),
Err(Error::ParameterTypeMismatch {
parameter: "torque_limit",
expected: "f32",
actual: "u32",
})
);
assert_eq!(
knott_dynamics_parameter_write_frame(
1,
KnottDynamicsParameter::TorqueLimit,
KnottDynamicsParameterValue::F32(f32::NAN),
),
Err(Error::NonFiniteValue("torque_limit"))
);
assert_eq!(
knott_dynamics_parameter_write_frame(
1,
KnottDynamicsParameter::Mode,
KnottDynamicsParameterValue::U32(KnottDynamicsMode::Calibration as u32),
),
Err(Error::UnsupportedMode(0x05))
);
assert_eq!(
knott_dynamics_parameter_write_frame(
1,
KnottDynamicsParameter::Mode,
KnottDynamicsParameterValue::U32(0x99),
),
Err(Error::UnsupportedMode(0x99))
);
}
#[test]
fn parameter_writes_mirror_firmware_stateless_range_checks() {
let invalid = [
(
KnottDynamicsParameter::DeviceId,
KnottDynamicsParameterValue::U32(0),
"1..63",
),
(
KnottDynamicsParameter::FastFrameFrequencyHz,
KnottDynamicsParameterValue::U32(10_001),
"0..10000 Hz",
),
(
KnottDynamicsParameter::WatchdogTimeoutMs,
KnottDynamicsParameterValue::U32(0),
"a nonzero timeout in milliseconds; zero disables the command watchdog",
),
(
KnottDynamicsParameter::GearRatio,
KnottDynamicsParameterValue::F32(0.0),
"a finite nonzero gear ratio",
),
(
KnottDynamicsParameter::TorqueLimit,
KnottDynamicsParameterValue::F32(0.0),
"a finite value greater than zero",
),
(
KnottDynamicsParameter::CurrentKp,
KnottDynamicsParameterValue::F32(-0.1),
"a finite nonnegative value",
),
(
KnottDynamicsParameter::TorqueFilterAlpha,
KnottDynamicsParameterValue::F32(1.1),
"a finite value in 0..=1",
),
(
KnottDynamicsParameter::MotorPolePairs,
KnottDynamicsParameterValue::U32(33),
"1..32 pole pairs",
),
(
KnottDynamicsParameter::MotorPhaseOrder,
KnottDynamicsParameterValue::I32(0),
"-1 or +1",
),
(
KnottDynamicsParameter::EncoderCpr,
KnottDynamicsParameterValue::I32(2048),
"-4096 or +4096",
),
(
KnottDynamicsParameter::EncoderFluxTableIndex,
KnottDynamicsParameterValue::U32(128),
"0..127",
),
];
for (parameter, value, constraint) in invalid {
assert_eq!(
knott_dynamics_parameter_write_frame(1, parameter, value),
Err(Error::InvalidParameterValue {
parameter: parameter.descriptor().name,
constraint,
})
);
}
}
#[test]
fn checked_parameter_writes_enforce_mode_requirements_and_transitions() -> kcan::Result<()> {
assert_eq!(
KnottDynamicsParameter::MotorPolePairs.write_requirement(),
KnottDynamicsWriteRequirement::DisabledOrIdle
);
assert_eq!(
KnottDynamicsParameter::QVoltageTarget.write_requirement(),
KnottDynamicsWriteRequirement::VqdOverride
);
assert_eq!(
knott_dynamics_parameter_write_frame_in_mode(
1,
KnottDynamicsMode::Position,
KnottDynamicsParameter::MotorPolePairs,
KnottDynamicsParameterValue::U32(14),
),
Err(Error::UnsafeOperation {
operation: "motor_pole_pairs",
required: "disabled or idle mode with PWM disabled",
actual_mode: KnottDynamicsMode::Position as u8,
})
);
assert_eq!(
knott_dynamics_parameter_write_frame_in_mode(
1,
KnottDynamicsMode::Idle,
KnottDynamicsParameter::QVoltageTarget,
KnottDynamicsParameterValue::F32(1.0),
),
Err(Error::UnsafeOperation {
operation: "q_voltage_target",
required: "V-q/d override mode",
actual_mode: KnottDynamicsMode::Idle as u8,
})
);
assert_eq!(
knott_dynamics_parameter_write_frame_in_mode(
1,
KnottDynamicsMode::Disabled,
KnottDynamicsParameter::Mode,
KnottDynamicsParameterValue::U32(KnottDynamicsMode::Position as u32),
),
Err(Error::InvalidModeTransition {
from: KnottDynamicsMode::Disabled as u8,
to: KnottDynamicsMode::Position as u8,
})
);
let motor_config = knott_dynamics_parameter_write_frame_in_mode(
1,
KnottDynamicsMode::Idle,
KnottDynamicsParameter::MotorPolePairs,
KnottDynamicsParameterValue::U32(14),
)?;
assert_eq!(motor_config.id(), CanId::Standard(0x601));
Ok(())
}
#[test]
fn probe_echo_and_feedback_parsers_validate_id_and_exact_dlc() -> kcan::Result<()> {
let probe = CanFrame::new(CanId::standard(3)?, &[3])?;
assert_eq!(parse_knott_dynamics_probe_response(&probe)?, Some(3));
let wrong_probe_payload = CanFrame::new(CanId::standard(3)?, &[2])?;
assert_eq!(
parse_knott_dynamics_probe_response(&wrong_probe_payload)?,
None
);
let short_probe = CanFrame::new(CanId::standard(3)?, &[])?;
assert_eq!(
parse_knott_dynamics_probe_response(&short_probe),
Err(Error::InvalidPayloadLength {
expected: 1,
actual: 0,
})
);
let echo_data = [8, 7, 6, 5, 4, 3, 2, 1];
let echo = CanFrame::new(KnottDynamicsFunction::TransmitPdo1.can_id(3)?, &echo_data)?;
assert_eq!(
parse_knott_dynamics_opaque_watchdog_response(3, &echo)?,
Some(echo_data)
);
assert_eq!(
parse_knott_dynamics_opaque_watchdog_response(2, &echo)?,
None
);
let mut feedback_data = [0_u8; 8];
feedback_data[..4].copy_from_slice(&1.25_f32.to_le_bytes());
feedback_data[4..].copy_from_slice(&(-2.5_f32).to_le_bytes());
let feedback_frame = CanFrame::new(
KnottDynamicsFunction::TransmitPdo2.can_id(3)?,
&feedback_data,
)?;
let feedback = parse_knott_dynamics_feedback(&feedback_frame)?.unwrap();
assert_eq!(feedback.node_id, 3);
assert_eq!(feedback.kind, KnottDynamicsFeedbackKind::PositionVelocity);
assert_eq!(feedback.position_rad, 1.25);
assert_eq!(feedback.velocity_rad_s, Some(-2.5));
assert_eq!(feedback.torque_nm, None);
let torque_frame = CanFrame::new(
KnottDynamicsFunction::TransmitPdo3.can_id(3)?,
&feedback_data,
)?;
let torque = parse_knott_dynamics_feedback(&torque_frame)?.unwrap();
assert_eq!(torque.kind, KnottDynamicsFeedbackKind::PositionTorque);
assert_eq!(torque.velocity_rad_s, None);
assert_eq!(torque.torque_nm, Some(-2.5));
let periodic_frame = CanFrame::new(
KnottDynamicsFunction::TransmitPdo4.can_id(3)?,
&feedback_data,
)?;
assert_eq!(
parse_knott_dynamics_feedback(&periodic_frame)?
.unwrap()
.kind,
KnottDynamicsFeedbackKind::PeriodicPositionVelocity
);
Ok(())
}
#[test]
fn feedback_parser_rejects_nonfinite_and_wrong_dlc_telemetry() -> kcan::Result<()> {
let short = CanFrame::new(KnottDynamicsFunction::TransmitPdo2.can_id(1)?, &[0; 4])?;
assert_eq!(
parse_knott_dynamics_feedback(&short),
Err(Error::InvalidPayloadLength {
expected: 8,
actual: 4,
})
);
let mut nan_data = [0_u8; 8];
nan_data[..4].copy_from_slice(&f32::NAN.to_le_bytes());
let nan = CanFrame::new(KnottDynamicsFunction::TransmitPdo2.can_id(1)?, &nan_data)?;
assert_eq!(
parse_knott_dynamics_feedback(&nan),
Err(Error::NonFiniteValue("position feedback"))
);
let unrelated = CanFrame::new(CanId::standard(0x123)?, &[0; 8])?;
assert_eq!(parse_knott_dynamics_feedback(&unrelated)?, None);
Ok(())
}
#[test]
fn parameter_response_decodes_requested_type_and_preserves_raw_float_values() -> kcan::Result<()> {
let version = CanFrame::new(
KnottDynamicsFunction::TransmitSdo.can_id(1)?,
&0x2025_0226_u32.to_le_bytes(),
)?;
assert_eq!(
parse_knott_dynamics_parameter_response(
1,
KnottDynamicsParameter::FirmwareVersion,
&version,
)?,
KnottDynamicsParameterValue::U32(0x2025_0226)
);
let infinity = CanFrame::new(
KnottDynamicsFunction::TransmitSdo.can_id(1)?,
&f32::INFINITY.to_le_bytes(),
)?;
assert_eq!(
parse_knott_dynamics_parameter_response(
1,
KnottDynamicsParameter::PositionLimitUpper,
&infinity,
)?,
KnottDynamicsParameterValue::F32(f32::INFINITY)
);
let wrong_node = CanFrame::new(KnottDynamicsFunction::TransmitSdo.can_id(2)?, &[0; 4])?;
assert_eq!(
parse_knott_dynamics_parameter_response(
1,
KnottDynamicsParameter::FirmwareVersion,
&wrong_node,
),
Err(Error::InvalidCanId(0x582))
);
Ok(())
}
#[test]
fn faults_preserve_unknown_bits_and_include_undervoltage() {
let faults = KnottDynamicsFaults::from_bits(0x8000 | 0x0040 | 0x1_0000);
assert!(faults.contains(KnottDynamicsFault::BusUndervoltage));
assert!(faults.contains(KnottDynamicsFault::WatchdogTimeout));
assert_eq!(faults.bits(), 0x1_8040);
assert_eq!(KnottDynamicsFault::all().len(), 16);
}
#[test]
fn frame_shape_detection_is_direction_and_dlc_aware() -> kcan::Result<()> {
assert!(is_knott_dynamics_frame_shape(&knott_dynamics_nmt_frame(
None,
KnottDynamicsMode::Idle,
)?));
assert!(is_knott_dynamics_frame_shape(&knott_dynamics_probe_frame(
1
)?));
assert!(is_knott_dynamics_frame_shape(
&knott_dynamics_position_velocity_frame(1, 0.0, 0.0)?,
));
assert!(is_knott_dynamics_frame_shape(
&knott_dynamics_parameter_read_frame(1, KnottDynamicsParameter::FirmwareVersion)?,
));
let wrong_dlc = CanFrame::new(KnottDynamicsFunction::ReceivePdo2.can_id(1)?, &[0; 4])?;
assert!(!is_knott_dynamics_frame_shape(&wrong_dlc));
let extended = CanFrame::new(CanId::extended(0x301)?, &[0; 8])?;
assert!(!is_knott_dynamics_frame_shape(&extended));
Ok(())
}