1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133

use crate::endian::{read_u16_le, write_u16_le};
use crate::instructions::{instruction_id, Instruction, Ping, PingResponse};
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;

use crate::checksum::calculate_checksum;

/// Write an instruction to a stream.
pub fn write_instruction<W, I>(stream: &mut W, instruction: &I) -> Result<(), WriteError>
where
	W: std::io::Write + ?Sized,
	I: Instruction,
{
	// Encode the body.
	let raw_body_len: usize = instruction.request_parameters_len().into();

	// Make buffer with enough capacity for fully stuffed message.
	let max_padded_body = crate::bytestuff::maximum_stuffed_len(raw_body_len);
	let mut buffer = vec![0u8; HEADER_SIZE + max_padded_body + 2];

	// Add the header, with a placeholder for the length field.
	buffer[0..4].copy_from_slice(&HEADER_PREFIX);
	buffer[4] = instruction.request_packet_id();
	buffer[7] = instruction.request_instruction_id();
	instruction.encode_request_parameters(&mut buffer[HEADER_SIZE..][..raw_body_len]);

	// Perform bitstuffing on the body.
	// The header never needs stuffing.
	let stuffed_body_len = crate::bytestuff::stuff_inplace(&mut buffer[HEADER_SIZE..], raw_body_len).unwrap();

	write_u16_le(&mut buffer[5..], stuffed_body_len as u16 + 3);

	// Add checksum.
	let checksum_index = HEADER_SIZE + stuffed_body_len;
	let checksum = calculate_checksum(0, &buffer[..checksum_index]);
	write_u16_le(&mut buffer[checksum_index..], checksum);
	buffer.resize(checksum_index + 2, 0);

	// Send message.
	trace!("sending instruction: {:02X?}", buffer);
	stream.write_all(&buffer)?;
	Ok(())
}

/// Read a response from a stream.
pub fn read_response<R, I>(stream: &mut R, instruction: &mut I) -> Result<I::Response, ReadError>
where
	R: std::io::Read + ?Sized,
	I: Instruction,
{
	// TODO: Scan for header prefix rather than assuming it is at start.
	// This matters in case of noise on the serial line.
	let mut raw_header = [0u8; STATUS_HEADER_SIZE];
	stream.read_exact(&mut raw_header[..])?;
	trace!("read status header: {:02X?}", raw_header);

	crate::InvalidHeaderPrefix::check(&raw_header[0..4], HEADER_PREFIX)?;
	crate::InvalidInstruction::check(raw_header[7], instruction_id::STATUS)?;

	let parameters = usize::from(read_u16_le(&raw_header[5..]) - 4);
	let packet_id = raw_header[4];

	let mut body = vec![0u8; parameters + 2];
	stream.read_exact(&mut body)?;
	trace!("read status parameters: {:02X?}", body);
	let checksum_from_msg = read_u16_le(&body[parameters..]);
	let body = &mut body[..parameters];

	let checksum = calculate_checksum(0, &raw_header);
	let checksum = calculate_checksum(checksum, &body);
	crate::InvalidChecksum::check(checksum, checksum_from_msg)?;

	// Remove bit-stuffing on the body.
	let unstuffed_size = crate::bytestuff::unstuff_inplace(body);

	Ok(instruction.decode_response_parameters(packet_id, &body[..unstuffed_size])?)
}

/// Write an instruction to a stream, and read a single response.
///
/// This is not suitable for broadcast instructions where each motor sends an individual response.
pub fn transfer_single<S, I>(stream: &mut S, instruction: &mut I) -> Result<I::Response, TransferError>
where
	S: std::io::Read + std::io::Write + ?Sized,
	I: Instruction,
{
	write_instruction(stream, instruction)?;
	Ok(read_response(stream, instruction)?)
}

/// Scan a bus for motors with a broadcast ping, calling an [`FnMut`] for each response.
///
/// Only timeouts are filtered out since they indicate a lack of response.
/// All other responses (including errors) are passed to the handler.
pub fn scan<S, F>(stream: &mut S, mut on_response: F) -> Result<(), WriteError>
where
	S: std::io::Read + std::io::Write + ?Sized,
	F: FnMut(Result<PingResponse, ReadError>),
{
	let mut ping = Ping::broadcast();
	write_instruction(stream, &ping)?;

	// TODO: See if we can terminate quicker.
	// Peek at the official SDK to see what they do.

	for _ in 0..253 {
		let response = read_response(stream, &mut ping);
		if let Err(ReadError::Io(e)) = &response {
			if e.kind() == std::io::ErrorKind::TimedOut {
				continue;
			}
		}
		on_response(response)
	}

	Ok(())
}

/// Scan a bus for motors with a broadcast ping, returning the responses in a [`Vec`].
///
/// Only timeouts are filtered out since they indicate a lack of response.
/// All other responses (including errors) are collected.
pub fn scan_to_vec<S>(stream: &mut S) -> Result<Vec<Result<PingResponse, ReadError>>, WriteError>
where
	S: std::io::Read + std::io::Write + ?Sized,
{
	let mut result = Vec::with_capacity(253);
	scan(stream, |x| result.push(x))?;
	Ok(result)
}