use serial2::SerialPort;
use std::path::Path;
use std::time::{Duration, Instant};
use crate::bytestuff;
use crate::checksum::calculate_checksum;
use crate::endian::{read_u16_le, read_u32_le, read_u8_le, write_u16_le};
use crate::{ReadError, TransferError, WriteError};
const HEADER_PREFIX: [u8; 4] = [0xFF, 0xFF, 0xFD, 0x00];
const HEADER_SIZE: usize = 8;
const STATUS_HEADER_SIZE: usize = 9;
pub struct Bus<ReadBuffer, WriteBuffer> {
serial_port: SerialPort,
baud_rate: u32,
read_buffer: ReadBuffer,
read_len: usize,
used_bytes: usize,
write_buffer: WriteBuffer,
}
impl<ReadBuffer, WriteBuffer> std::fmt::Debug for Bus<ReadBuffer, WriteBuffer> {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
#[derive(Debug)]
#[allow(dead_code)] enum Raw {
#[cfg(unix)]
Fd(std::os::unix::io::RawFd),
#[cfg(windows)]
Handle(std::os::windows::io::RawHandle),
}
#[cfg(unix)]
let raw = {
use std::os::unix::io::AsRawFd;
Raw::Fd(self.serial_port.as_raw_fd())
};
#[cfg(windows)]
let raw = {
use std::os::windows::io::AsRawHandle;
Raw::Handle(self.serial_port.as_raw_handle())
};
f.debug_struct("Bus")
.field("serial_port", &raw)
.field("baud_rate", &self.baud_rate)
.finish_non_exhaustive()
}
}
impl Bus<Vec<u8>, Vec<u8>> {
pub fn open(path: impl AsRef<Path>, baud_rate: u32) -> std::io::Result<Self> {
let port = SerialPort::open(path, baud_rate)?;
Ok(Self::with_buffers_and_baud_rate(port, vec![0; 128], vec![0; 128], baud_rate))
}
pub fn new(serial_port: SerialPort) -> Result<Self, crate::InitializeError> {
Self::with_buffers(serial_port, vec![0; 128], vec![0; 128])
}
}
impl<ReadBuffer, WriteBuffer> Bus<ReadBuffer, WriteBuffer>
where
ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
{
pub fn open_with_buffers(
path: impl AsRef<Path>,
baud_rate: u32,
read_buffer: ReadBuffer,
write_buffer: WriteBuffer,
) -> std::io::Result<Self> {
let port = SerialPort::open(path, baud_rate)?;
Ok(Self::with_buffers_and_baud_rate(port, read_buffer, write_buffer, baud_rate))
}
pub fn with_buffers(serial_port: SerialPort, read_buffer: ReadBuffer, write_buffer: WriteBuffer) -> Result<Self, crate::InitializeError> {
let baud_rate = serial_port.get_configuration()
.map_err(crate::InitializeError::GetConfiguration)?
.get_baud_rate()
.map_err(crate::InitializeError::GetBaudRate)?;
Ok(Self::with_buffers_and_baud_rate(serial_port, read_buffer, write_buffer, baud_rate))
}
fn with_buffers_and_baud_rate(serial_port: SerialPort, read_buffer: ReadBuffer, mut write_buffer: WriteBuffer, baud_rate: u32) -> Self {
assert!(write_buffer.as_mut().len() >= HEADER_SIZE + 2);
write_buffer.as_mut()[..4].copy_from_slice(&HEADER_PREFIX);
Self {
serial_port,
baud_rate,
read_buffer,
read_len: 0,
used_bytes: 0,
write_buffer,
}
}
pub fn serial_port(&self) -> &SerialPort {
&self.serial_port
}
pub fn into_serial_port(self) -> SerialPort {
self.serial_port
}
pub fn baud_rate(&self) -> u32 {
self.baud_rate
}
pub fn set_baud_rate(&mut self, baud_rate: u32) -> Result<(), std::io::Error> {
let mut settings = self.serial_port.get_configuration()?;
settings.set_baud_rate(baud_rate)?;
self.serial_port.set_configuration(&settings)?;
self.baud_rate = baud_rate;
Ok(())
}
pub fn transfer_single<F>(
&mut self,
packet_id: u8,
instruction_id: u8,
parameter_count: usize,
expected_response_parameters: u16,
encode_parameters: F,
) -> Result<StatusPacket<'_>, TransferError>
where
F: FnOnce(&mut [u8]),
{
self.write_instruction(packet_id, instruction_id, parameter_count, encode_parameters)?;
let response = self.read_status_response(expected_response_parameters)?;
crate::error::InvalidPacketId::check(response.packet_id(), packet_id).map_err(crate::ReadError::from)?;
Ok(response)
}
pub fn write_instruction<F>(
&mut self,
packet_id: u8,
instruction_id: u8,
parameter_count: usize,
encode_parameters: F,
) -> Result<(), WriteError>
where
F: FnOnce(&mut [u8]),
{
let buffer = self.write_buffer.as_mut();
crate::error::BufferTooSmallError::check(HEADER_SIZE + parameter_count + 2, buffer.len())?;
buffer[4] = packet_id;
buffer[5] = 0;
buffer[6] = 0;
buffer[7] = instruction_id;
encode_parameters(&mut buffer[HEADER_SIZE..][..parameter_count]);
let stuffed_body_len = bytestuff::stuff_inplace(&mut buffer[HEADER_SIZE..], parameter_count)?;
write_u16_le(&mut buffer[5..], stuffed_body_len as u16 + 3);
let checksum_index = HEADER_SIZE + stuffed_body_len;
let checksum = calculate_checksum(0, &buffer[..checksum_index]);
write_u16_le(&mut buffer[checksum_index..], checksum);
self.read_len = 0;
self.used_bytes = 0;
self.serial_port.discard_input_buffer().map_err(WriteError::DiscardBuffer)?;
let stuffed_message = &buffer[..checksum_index + 2];
trace!("sending instruction: {:02X?}", stuffed_message);
self.serial_port.write_all(stuffed_message).map_err(WriteError::Write)?;
Ok(())
}
pub fn read_status_response_deadline(&mut self, deadline: Instant) -> Result<StatusPacket, ReadError> {
crate::error::BufferTooSmallError::check(STATUS_HEADER_SIZE, self.read_buffer.as_mut().len())?;
let stuffed_message_len = loop {
self.remove_garbage();
if self.read_len > STATUS_HEADER_SIZE {
let read_buffer = &self.read_buffer.as_mut()[..self.read_len];
let body_len = read_buffer[5] as usize + read_buffer[6] as usize * 256;
let body_len = body_len - 2; crate::error::BufferTooSmallError::check(STATUS_HEADER_SIZE + body_len, self.read_buffer.as_mut().len())?;
if self.read_len >= STATUS_HEADER_SIZE + body_len {
break STATUS_HEADER_SIZE + body_len;
}
}
let timeout = match deadline.checked_duration_since(Instant::now()) {
Some(x) => x,
None => {
trace!(
"timeout reading status response, data in buffer: {:02X?}",
&self.read_buffer.as_ref()[..self.read_len]
);
return Err(std::io::ErrorKind::TimedOut.into());
},
};
self.serial_port.set_read_timeout(timeout).ok();
let new_data = self.serial_port.read(&mut self.read_buffer.as_mut()[self.read_len..])?;
if new_data == 0 {
continue;
}
self.read_len += new_data;
};
let buffer = self.read_buffer.as_mut();
let parameters_end = stuffed_message_len - 2;
trace!("read packet: {:02X?}", &buffer[..parameters_end]);
let checksum_message = read_u16_le(&buffer[parameters_end..]);
let checksum_computed = calculate_checksum(0, &buffer[..parameters_end]);
if checksum_message != checksum_computed {
self.consume_read_bytes(stuffed_message_len);
return Err(crate::InvalidChecksum {
message: checksum_message,
computed: checksum_computed,
}
.into());
}
self.used_bytes += stuffed_message_len;
let parameter_count = bytestuff::unstuff_inplace(&mut buffer[STATUS_HEADER_SIZE..parameters_end]);
let response = StatusPacket {
data: &self.read_buffer.as_ref()[..STATUS_HEADER_SIZE + parameter_count],
};
crate::InvalidInstruction::check(response.instruction_id(), crate::instructions::instruction_id::STATUS)?;
crate::MotorError::check(response.error())?;
Ok(response)
}
pub fn read_status_response(&mut self, expected_parameters: u16) -> Result<StatusPacket, ReadError> {
let message_size = STATUS_HEADER_SIZE as u32 + u32::from(expected_parameters) + 2;
let timeout = message_transfer_time(message_size, self.baud_rate) + Duration::from_millis(34);
self.read_status_response_deadline(Instant::now() + timeout)
}
}
pub(crate) fn message_transfer_time(message_size: u32, baud_rate: u32) -> Duration {
let baud_rate = u64::from(baud_rate);
let bits = u64::from(message_size) * 10; let secs = bits / baud_rate;
let subsec_bits = bits % baud_rate;
let nanos = (subsec_bits * 1_000_000_000).div_ceil(baud_rate);
Duration::new(secs, nanos as u32)
}
impl<ReadBuffer, WriteBuffer> Bus<ReadBuffer, WriteBuffer>
where
ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
{
fn remove_garbage(&mut self) {
let read_buffer = self.read_buffer.as_mut();
let garbage_len = find_header(&read_buffer[..self.read_len][self.used_bytes..]);
if garbage_len > 0 {
debug!("skipping {} bytes of leading garbage.", garbage_len);
trace!("skipped garbage: {:02X?}", &read_buffer[..garbage_len]);
}
self.consume_read_bytes(self.used_bytes + garbage_len);
debug_assert_eq!(self.used_bytes, 0);
}
fn consume_read_bytes(&mut self, len: usize) {
debug_assert!(len <= self.read_len);
self.read_buffer.as_mut().copy_within(len..self.read_len, 0);
self.used_bytes = self.used_bytes.saturating_sub(len);
self.read_len -= len;
}
}
#[derive(Debug)]
pub struct StatusPacket<'a> {
data: &'a [u8],
}
impl<'a> StatusPacket<'a> {
pub fn as_bytes(&self) -> &[u8] {
self.data
}
pub fn packet_id(&self) -> u8 {
self.as_bytes()[4]
}
pub fn instruction_id(&self) -> u8 {
self.as_bytes()[7]
}
pub fn error(&self) -> u8 {
self.as_bytes()[8]
}
pub fn error_number(&self) -> u8 {
self.error() & !0x80
}
pub fn alert(&self) -> bool {
self.error() & 0x80 != 0
}
pub fn parameters(&self) -> &[u8] {
&self.data[STATUS_HEADER_SIZE..]
}
}
fn find_header(buffer: &[u8]) -> usize {
for i in 0..buffer.len() {
let possible_prefix = HEADER_PREFIX.len().min(buffer.len() - i);
if buffer[i..].starts_with(&HEADER_PREFIX[..possible_prefix]) {
return i;
}
}
buffer.len()
}
#[derive(Debug, Clone, Eq, PartialEq)]
pub struct Response<T> {
pub motor_id: u8,
pub alert: bool,
pub data: T,
}
impl<'a> TryFrom<StatusPacket<'a>> for Response<()> {
type Error = crate::InvalidParameterCount;
fn try_from(status_packet: StatusPacket<'a>) -> Result<Self, Self::Error> {
crate::InvalidParameterCount::check(status_packet.parameters().len(), 0)?;
Ok(Self {
motor_id: status_packet.packet_id(),
alert: status_packet.alert(),
data: (),
})
}
}
impl<'a, 'b> From<&'b StatusPacket<'a>> for Response<&'b [u8]> {
fn from(status_packet: &'b StatusPacket<'a>) -> Self {
Self {
motor_id: status_packet.packet_id(),
alert: status_packet.alert(),
data: status_packet.parameters(),
}
}
}
impl<'a> From<StatusPacket<'a>> for Response<Vec<u8>> {
fn from(status_packet: StatusPacket<'a>) -> Self {
Self {
motor_id: status_packet.packet_id(),
alert: status_packet.alert(),
data: status_packet.parameters().to_owned(),
}
}
}
impl<'a> TryFrom<StatusPacket<'a>> for Response<u8> {
type Error = crate::InvalidParameterCount;
fn try_from(status_packet: StatusPacket<'a>) -> Result<Self, Self::Error> {
crate::InvalidParameterCount::check(status_packet.parameters().len(), 1)?;
Ok(Self {
motor_id: status_packet.packet_id(),
alert: status_packet.alert(),
data: read_u8_le(status_packet.parameters()),
})
}
}
impl<'a> TryFrom<StatusPacket<'a>> for Response<u16> {
type Error = crate::InvalidParameterCount;
fn try_from(status_packet: StatusPacket<'a>) -> Result<Self, Self::Error> {
crate::InvalidParameterCount::check(status_packet.parameters().len(), 2)?;
Ok(Self {
motor_id: status_packet.packet_id(),
alert: status_packet.alert(),
data: read_u16_le(status_packet.parameters()),
})
}
}
impl<'a> TryFrom<StatusPacket<'a>> for Response<u32> {
type Error = crate::InvalidParameterCount;
fn try_from(status_packet: StatusPacket<'a>) -> Result<Self, Self::Error> {
crate::InvalidParameterCount::check(status_packet.parameters().len(), 4)?;
Ok(Self {
motor_id: status_packet.packet_id(),
alert: status_packet.alert(),
data: read_u32_le(status_packet.parameters()),
})
}
}
#[cfg(test)]
mod test {
use super::*;
use assert2::assert;
#[test]
fn test_find_garbage_end() {
assert!(find_header(&[0xFF]) == 0);
assert!(find_header(&[0xFF, 0xFF]) == 0);
assert!(find_header(&[0xFF, 0xFF, 0xFD]) == 0);
assert!(find_header(&[0xFF, 0xFF, 0xFD, 0x00]) == 0);
assert!(find_header(&[0xFF, 0xFF, 0xFD, 0x00, 9]) == 0);
assert!(find_header(&[0, 1, 2, 3, 4, 0xFF]) == 5);
assert!(find_header(&[0, 1, 2, 3, 4, 0xFF, 0xFF]) == 5);
assert!(find_header(&[0, 1, 2, 3, 4, 0xFF, 0xFF, 0xFD]) == 5);
assert!(find_header(&[0, 1, 2, 3, 4, 0xFF, 0xFF, 0xFD, 0x00]) == 5);
assert!(find_header(&[0, 1, 2, 3, 4, 0xFF, 0xFF, 0xFD, 0x00, 9]) == 5);
assert!(find_header(&[0xFF, 1]) == 2);
assert!(find_header(&[0, 1, 2, 3, 4, 0xFF, 6]) == 7);
}
#[test]
fn test_message_transfer_time() {
assert!(message_transfer_time(100, 1_000) == Duration::from_secs(1));
assert!(message_transfer_time(1_000, 10_000) == Duration::from_secs(1));
assert!(message_transfer_time(1_000, 1_000_000) == Duration::from_millis(10));
assert!(message_transfer_time(1_000, 10_000_000) == Duration::from_millis(1));
assert!(message_transfer_time(1_000, 100_000_000) == Duration::from_micros(100));
assert!(message_transfer_time(1_000, 1_000_000_000) == Duration::from_micros(10));
assert!(message_transfer_time(1_000, 2_000_000_000) == Duration::from_micros(5));
assert!(message_transfer_time(1_000, 4_000_000_000) == Duration::from_nanos(2500));
assert!(message_transfer_time(10_000, 4_000_000_000) == Duration::from_micros(25));
assert!(message_transfer_time(1_000_000, 4_000_000_000) == Duration::from_micros(2500));
assert!(message_transfer_time(10_000_000, 4_000_000_000) == Duration::from_millis(25));
assert!(message_transfer_time(100_000_000, 4_000_000_000) == Duration::from_millis(250));
assert!(message_transfer_time(1_000_000_000, 4_000_000_000) == Duration::from_millis(2500));
assert!(message_transfer_time(2_000_000_000, 4_000_000_000) == Duration::from_secs(5));
assert!(message_transfer_time(4_000_000_000, 4_000_000_000) == Duration::from_secs(10));
assert!(message_transfer_time(4_000_000_000, 2_000_000_000) == Duration::from_secs(20));
assert!(message_transfer_time(4_000_000_000, 1_000_000_000) == Duration::from_secs(40));
assert!(message_transfer_time(4_000_000_000, 100_000_000) == Duration::from_secs(400));
assert!(message_transfer_time(4_000_000_000, 10_000_000) == Duration::from_secs(4_000));
assert!(message_transfer_time(4_000_000_000, 1_000_000) == Duration::from_secs(40_000));
assert!(message_transfer_time(4_000_000_000, 100_000) == Duration::from_secs(400_000));
assert!(message_transfer_time(4_000_000_000, 10_000) == Duration::from_secs(4_000_000));
assert!(message_transfer_time(4_000_000_000, 1_000) == Duration::from_secs(40_000_000));
assert!(message_transfer_time(4_000_000_000, 100) == Duration::from_secs(400_000_000));
assert!(message_transfer_time(4_000_000_000, 10) == Duration::from_secs(4_000_000_000));
assert!(message_transfer_time(4_000_000_000, 1) == Duration::from_secs(40_000_000_000));
assert!(message_transfer_time(43, 1) == Duration::from_secs(430));
assert!(message_transfer_time(43, 10) == Duration::from_secs(43));
assert!(message_transfer_time(43, 2) == Duration::from_secs(215));
assert!(message_transfer_time(43, 20) == Duration::from_millis(21_500));
assert!(message_transfer_time(43, 200) == Duration::from_millis(2_150));
assert!(message_transfer_time(43, 2_000_000) == Duration::from_micros(215));
assert!(message_transfer_time(43, 2_000_000_000) == Duration::from_nanos(215));
assert!(message_transfer_time(43, 4_000_000_000) == Duration::from_nanos(108)); assert!(message_transfer_time(3, 4_000_000_000) == Duration::from_nanos(8)); assert!(message_transfer_time(5, 4_000_000_000) == Duration::from_nanos(13)); let lots = u32::MAX - 1; assert!(message_transfer_time(lots, 1) == Duration::from_secs(u64::from(lots) * 10));
assert!(message_transfer_time(lots, lots) == Duration::from_secs(10));
assert!(message_transfer_time(lots / 2, lots) == Duration::from_secs(5));
assert!(message_transfer_time(lots, lots / 2) == Duration::from_secs(20));
}
}