Merge pull request de-vri-es#7 from omelia-wetaworkshop/alert_bit_res…

…ponse

changed functions to return a response struct,

CHANGELOG.md

@@ -1,3 +1,9 @@
+main:
+  * Fix amount of data read in `sync_read_u16` and `sync_read_u16_cb`.
+  * Do not return `Err()` when the `alert` bit is set in a status packet from a motor.
+  * Report the `alert` bit in the returned values from commands in a new `Response` struct.
+  * Pass original `BulkReadData` command to user callback in `bulk_read_cb()`.
+
 v0.5.1 - 2023-12-07:
   * Parse all status messages when more than on has been read in a single `read()`.
 

dynamixel2-cli/src/bin/dynamixel2/main.rs

@@ -25,7 +25,7 @@ fn do_main(options: Options) -> Result<(), ()> {
 				MotorId::Broadcast => {
 					log::debug!("Scanning bus for connected motors");
 					bus.scan_cb(|response| match response {
-						Ok(response) => log_ping_response(&response),
+						Ok(response) => log::info!("{:?}", response),
 						Err(e) => log::warn!("Communication error: {}", e),
 					})
 					.map_err(|e| log::error!("Command failed: {}", e))?;
@@ -41,26 +41,31 @@ fn do_main(options: Options) -> Result<(), ()> {
 		Command::Read8 { motor_id, address } => {
 			let mut bus = open_bus(&options)?;
 			log::debug!("Reading an 8-bit value from motor {} at address {}", motor_id.raw(), address);
-			let value = bus
+			let response = bus
 				.read_u8(motor_id.assume_unicast()?, *address)
 				.map_err(|e| log::error!("Command failed: {}", e))?;
-			log::info!("Ok: {} (0x{:02X})", value, value);
+			log::info!("{:?} (0x{:02X})", response, response.data);
 		},
 		Command::Read16 { motor_id, address } => {
 			let mut bus = open_bus(&options)?;
 			log::debug!("Reading a 16-bit value from motor {} at address {}", motor_id.raw(), address);
-			let value = bus
+			let response = bus
 				.read_u16(motor_id.assume_unicast()?, *address)
 				.map_err(|e| log::error!("Command failed: {}", e))?;
-			log::info!("Ok: {} (0x{:04X})", value, value);
+			log::info!("{:?} (0x{:04X})", response, response.data);
 		},
 		Command::Read32 { motor_id, address } => {
 			let mut bus = open_bus(&options)?;
 			log::debug!("Reading a 32-bit value from motor {} at address {}", motor_id.raw(), address);
-			let value = bus
+			let response = bus
 				.read_u32(motor_id.assume_unicast()?, *address)
 				.map_err(|e| log::error!("Command failed: {}", e))?;
-			log::info!("Ok: {} (0x{:04X} {:04X})", value, (value >> 16) & 0xFFFF, value & 0xFFFF);
+			log::info!(
+				"{:?} (0x{:04X} {:04X})",
+				response,
+				(response.data >> 16) & 0xFFFF,
+				response.data & 0xFFFF
+			);
 		},
 		Command::Write8 { motor_id, address, value } => {
 			let mut bus = open_bus(&options)?;
@@ -71,9 +76,10 @@ fn do_main(options: Options) -> Result<(), ()> {
 				motor_id.raw(),
 				address
 			);
-			bus.write_u8(motor_id.raw(), *address, *value)
+			let response = bus
+				.write_u8(motor_id.raw(), *address, *value)
 				.map_err(|e| log::error!("Write failed: {}", e))?;
-			log::info!("Ok");
+			log::info!("Ok (Hardware error: {})", response.alert);
 		},
 		Command::Write16 { motor_id, address, value } => {
 			let mut bus = open_bus(&options)?;
@@ -84,9 +90,10 @@ fn do_main(options: Options) -> Result<(), ()> {
 				motor_id.raw(),
 				address
 			);
-			bus.write_u16(motor_id.raw(), *address, *value)
+			let response = bus
+				.write_u16(motor_id.raw(), *address, *value)
 				.map_err(|e| log::error!("Command failed: {}", e))?;
-			log::info!("Ok");
+			log::info!("Ok (Hardware error: {})", response.alert);
 		},
 		Command::Write32 { motor_id, address, value } => {
 			let mut bus = open_bus(&options)?;
@@ -98,9 +105,10 @@ fn do_main(options: Options) -> Result<(), ()> {
 				motor_id.raw(),
 				address
 			);
-			bus.write_u32(motor_id.raw(), *address, *value)
+			let response = bus
+				.write_u32(motor_id.raw(), *address, *value)
 				.map_err(|e| log::error!("Command failed: {}", e))?;
-			log::info!("Ok");
+			log::info!("Ok (Hardware error: {})", response.alert);
 		},
 		Command::ShellCompletion { shell, output } => {
 			write_shell_completion(*shell, output.as_deref())?;

src/bus.rs

@@ -4,7 +4,7 @@ use std::time::{Duration, Instant};
 
 use crate::bytestuff;
 use crate::checksum::calculate_checksum;
-use crate::endian::{read_u16_le, write_u16_le};
+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];
@@ -95,7 +95,7 @@ where
 		instruction_id: u8,
 		parameter_count: usize,
 		encode_parameters: F,
-	) -> Result<Response<ReadBuffer, WriteBuffer>, TransferError>
+	) -> Result<StatusPacket<ReadBuffer, WriteBuffer>, TransferError>
 	where
 		F: FnOnce(&mut [u8]),
 	{
@@ -150,15 +150,13 @@ where
 		// Send message.
 		let stuffed_message = &buffer[..checksum_index + 2];
 		trace!("sending instruction: {:02X?}", stuffed_message);
-		self.serial_port.discard_input_buffer()
-			.map_err(WriteError::DiscardBuffer)?;
-		self.serial_port.write_all(stuffed_message)
-			.map_err(WriteError::Write)?;
+		self.serial_port.discard_input_buffer().map_err(WriteError::DiscardBuffer)?;
+		self.serial_port.write_all(stuffed_message).map_err(WriteError::Write)?;
 		Ok(())
 	}
 
 	/// Read a raw status response from the bus.
-	pub fn read_status_response(&mut self) -> Result<Response<ReadBuffer, WriteBuffer>, ReadError> {
+	pub fn read_status_response(&mut self) -> Result<StatusPacket<ReadBuffer, WriteBuffer>, ReadError> {
 		let deadline = Instant::now() + self.read_timeout;
 		let stuffed_message_len = loop {
 			self.remove_garbage();
@@ -176,7 +174,10 @@ where
 			}
 
 			if Instant::now() > deadline {
-				trace!("timeout reading status response, data in buffer: {:02X?}", &self.read_buffer.as_ref()[..self.read_len]);
+				trace!(
+					"timeout reading status response, data in buffer: {:02X?}",
+					&self.read_buffer.as_ref()[..self.read_len]
+				);
 				return Err(std::io::ErrorKind::TimedOut.into());
 			}
 
@@ -207,8 +208,8 @@ where
 		// Remove byte-stuffing from the parameters.
 		let parameter_count = bytestuff::unstuff_inplace(&mut buffer[STATUS_HEADER_SIZE..parameters_end]);
 
-		// Creating the response struct here means that the data gets purged from the buffer even if we return early using the try operator.
-		let response = Response {
+		// Creating the status packet struct here means that the data gets purged from the buffer even if we return early using the try operator.
+		let response = StatusPacket {
 			bus: self,
 			stuffed_message_len,
 			parameter_count,
@@ -246,7 +247,7 @@ where
 /// A status response that is currently in the read buffer of a bus.
 ///
 /// When dropped, the response data is removed from the read buffer.
-pub struct Response<'a, ReadBuffer, WriteBuffer>
+pub struct StatusPacket<'a, ReadBuffer, WriteBuffer>
 where
 	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
 	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
@@ -261,7 +262,7 @@ where
 	parameter_count: usize,
 }
 
-impl<'a, ReadBuffer, WriteBuffer> Response<'a, ReadBuffer, WriteBuffer>
+impl<'a, ReadBuffer, WriteBuffer> StatusPacket<'a, ReadBuffer, WriteBuffer>
 where
 	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
 	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
@@ -289,13 +290,18 @@ where
 		self.as_bytes()[8]
 	}
 
+	// The alert bit from the error field of the response.
+	pub fn alert(&self) -> bool {
+		self.error() & 0x80 != 0
+	}
+
 	/// The parameters of the response.
 	pub fn parameters(&self) -> &[u8] {
 		&self.as_bytes()[STATUS_HEADER_SIZE..][..self.parameter_count]
 	}
 }
 
-impl<'a, ReadBuffer, WriteBuffer> Drop for Response<'a, ReadBuffer, WriteBuffer>
+impl<'a, ReadBuffer, WriteBuffer> Drop for StatusPacket<'a, ReadBuffer, WriteBuffer>
 where
 	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
 	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
@@ -321,6 +327,104 @@ fn find_header(buffer: &[u8]) -> usize {
 	buffer.len()
 }
 
+/// A response from a motor.
+#[derive(Debug)]
+pub struct Response<T> {
+	/// The motor that sent the response.
+	pub motor_id: u8,
+
+	/// The alert bit from the response message.
+	///
+	/// If this is set, you can normally check the "Hardware Error" register for more details.
+	/// Consult your motor manual for more details.
+	pub alert: bool,
+
+	/// The data from the motor.
+	pub data: T,
+}
+
+impl<'a, ReadBuffer, WriteBuffer> TryFrom<StatusPacket<'a, ReadBuffer, WriteBuffer>> for Response<()>
+where
+	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
+	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
+{
+	type Error = crate::InvalidParameterCount;
+
+	fn try_from(status_packet: StatusPacket<'a, ReadBuffer, WriteBuffer>) -> Result<Self, Self::Error> {
+		crate::InvalidParameterCount::check(status_packet.parameter_count, 0)?;
+		Ok(Self {
+			data: (),
+			motor_id: status_packet.packet_id(),
+			alert: status_packet.alert(),
+		})
+	}
+}
+
+impl<'a, ReadBuffer, WriteBuffer> From<StatusPacket<'a, ReadBuffer, WriteBuffer>> for Response<Vec<u8>>
+where
+	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
+	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
+{
+	fn from(status_packet: StatusPacket<'a, ReadBuffer, WriteBuffer>) -> Self {
+		Self {
+			data: status_packet.parameters().to_owned(),
+			motor_id: status_packet.packet_id(),
+			alert: status_packet.alert(),
+		}
+	}
+}
+
+impl<'a, ReadBuffer, WriteBuffer> TryFrom<StatusPacket<'a, ReadBuffer, WriteBuffer>> for Response<u8>
+where
+	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
+	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
+{
+	type Error = crate::InvalidParameterCount;
+
+	fn try_from(status_packet: StatusPacket<'a, ReadBuffer, WriteBuffer>) -> Result<Self, Self::Error> {
+		crate::InvalidParameterCount::check(status_packet.parameter_count, 1)?;
+		Ok(Self {
+			data: read_u8_le(status_packet.parameters()),
+			motor_id: status_packet.packet_id(),
+			alert: status_packet.alert(),
+		})
+	}
+}
+
+impl<'a, ReadBuffer, WriteBuffer> TryFrom<StatusPacket<'a, ReadBuffer, WriteBuffer>> for Response<u16>
+where
+	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
+	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
+{
+	type Error = crate::InvalidParameterCount;
+
+	fn try_from(status_packet: StatusPacket<'a, ReadBuffer, WriteBuffer>) -> Result<Self, Self::Error> {
+		crate::InvalidParameterCount::check(status_packet.parameter_count, 2)?;
+		Ok(Self {
+			data: read_u16_le(status_packet.parameters()),
+			motor_id: status_packet.packet_id(),
+			alert: status_packet.alert(),
+		})
+	}
+}
+
+impl<'a, ReadBuffer, WriteBuffer> TryFrom<StatusPacket<'a, ReadBuffer, WriteBuffer>> for Response<u32>
+where
+	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
+	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
+{
+	type Error = crate::InvalidParameterCount;
+
+	fn try_from(status_packet: StatusPacket<'a, ReadBuffer, WriteBuffer>) -> Result<Self, Self::Error> {
+		crate::InvalidParameterCount::check(status_packet.parameter_count, 4)?;
+		Ok(Self {
+			data: read_u32_le(status_packet.parameters()),
+			motor_id: status_packet.packet_id(),
+			alert: status_packet.alert(),
+		})
+	}
+}
+
 #[cfg(test)]
 mod test {
 	use super::*;

src/error.rs

@@ -82,7 +82,9 @@ pub struct InvalidParameterCount {
 
 impl MotorError {
 	pub fn check(raw: u8) -> Result<(), Self> {
-		if raw == 0 {
+		// Ignore the alert bit for this check.
+		// If the alert bit is set, the motor encountered an error, but the instruction was still executed.
+		if raw & !0x80 == 0 {
 			Ok(())
 		} else {
 			Err(Self { raw })
@@ -181,6 +183,42 @@ impl From<ReadError> for TransferError {
 	}
 }
 
+impl From<InvalidMessage> for TransferError {
+	fn from(other: InvalidMessage) -> Self {
+		Self::ReadError(other.into())
+	}
+}
+
+impl From<InvalidHeaderPrefix> for TransferError {
+	fn from(other: InvalidHeaderPrefix) -> Self {
+		Self::ReadError(other.into())
+	}
+}
+
+impl From<InvalidChecksum> for TransferError {
+	fn from(other: InvalidChecksum) -> Self {
+		Self::ReadError(other.into())
+	}
+}
+
+impl From<InvalidPacketId> for TransferError {
+	fn from(other: InvalidPacketId) -> Self {
+		Self::ReadError(other.into())
+	}
+}
+
+impl From<InvalidInstruction> for TransferError {
+	fn from(other: InvalidInstruction) -> Self {
+		Self::ReadError(other.into())
+	}
+}
+
+impl From<InvalidParameterCount> for TransferError {
+	fn from(other: InvalidParameterCount) -> Self {
+		Self::ReadError(other.into())
+	}
+}
+
 impl From<std::io::Error> for ReadError {
 	fn from(other: std::io::Error) -> Self {
 		Self::Io(other)

src/instructions/action.rs

@@ -1,5 +1,5 @@
 use super::{instruction_id, packet_id};
-use crate::{Bus, TransferError, WriteError};
+use crate::{Bus, Response, TransferError, WriteError};
 
 impl<ReadBuffer, WriteBuffer> Bus<ReadBuffer, WriteBuffer>
 where
@@ -8,16 +8,11 @@ where
 {
 	/// Send an action command to trigger a previously registered instruction.
 	///
-	/// The `motor_id` parameter may be set to [`packet_id::BROADCAST`],
-	/// although the [`Self::broadcast_action`] is generally easier to use.
-	pub fn action(&mut self, motor_id: u8) -> Result<(), TransferError> {
-		if motor_id == packet_id::BROADCAST {
-			self.broadcast_action()?;
-		} else {
-			let response = self.transfer_single(motor_id, instruction_id::ACTION, 0, |_| ())?;
-			crate::InvalidParameterCount::check(response.parameters().len(), 0).map_err(crate::ReadError::from)?;
-		}
-		Ok(())
+	/// The `motor_id` parameter must not be set to [`packet_id::BROADCAST`],
+	/// Instead use [`Self::broadcast_action`].
+	pub fn action(&mut self, motor_id: u8) -> Result<Response<()>, TransferError> {
+		let response = self.transfer_single(motor_id, instruction_id::ACTION, 0, |_| ())?;
+		Ok(response.try_into()?)
 	}
 
 	/// Broadcast an action command to all connected motors to trigger a previously registered instruction.