Add better debugging for header errors

Cargo.toml

@@ -2,7 +2,7 @@
 authors = ["David Weis <dweis7@gmail.com>"]
 edition = "2021"
 name = "dynamixel-driver"
-version = "0.2.4"
+version = "0.2.5"
 
 description = "Driver for Dynamixel v1 servos"
 documentation = "https://davidweis.dev/dynamixel-driver/dynamixel_driver/index.html"

src/instructions.rs

@@ -11,8 +11,10 @@ pub enum DynamixelDriverError {
     StatusError(StatusError),
     #[error("checksum error on arriving packet")]
     ChecksumError,
-    #[error("invalid header")]
-    HeaderError,
+    #[error("invalid header {0:X?}")]
+    HeaderError(Vec<u8>),
+    #[error("header length less than 2")]
+    HeaderLenTooSmall,
     #[error("reading error")]
     ReadingError,
     #[error("failed reading {0:?}")]
@@ -34,7 +36,7 @@ impl DynamixelDriverError {
             DynamixelDriverError::Timeout
                 | DynamixelDriverError::StatusError(_)
                 | DynamixelDriverError::ChecksumError
-                | DynamixelDriverError::HeaderError
+                | DynamixelDriverError::HeaderError(_)
                 | DynamixelDriverError::ReadingError
                 | DynamixelDriverError::DecodingError(_)
                 | DynamixelDriverError::IdMismatchError(_, _)

src/serial_driver.rs

@@ -79,15 +79,26 @@ impl Decoder for DynamixelProtocol {
 
         let id = src[2];
         let len = src[3] as usize;
-        if !src.starts_with(&[0xFF, 0xFF]) || len < 2 {
+        if len < 2 {
             // discard 1 byte in case we are starting with FF, FF
             let _ = src.split_to(1);
             if let Some(start) = src.windows(2).position(|pos| pos == [0xFF, 0xFF]) {
                 let _ = src.split_to(start);
             } else {
                 src.clear();
             }
-            return Err(DynamixelDriverError::HeaderError);
+            return Err(DynamixelDriverError::HeaderLenTooSmall);
+        }
+        if !src.starts_with(&[0xFF, 0xFF]) {
+            // discard 1 byte in case we are starting with FF, FF
+            let buffer_copy = src.clone().into();
+            let _ = src.split_to(1);
+            if let Some(start) = src.windows(2).position(|pos| pos == [0xFF, 0xFF]) {
+                let _ = src.split_to(start);
+            } else {
+                src.clear();
+            }
+            return Err(DynamixelDriverError::HeaderError(buffer_copy));
         }
         if src.len() < 4 + len {
             return Ok(None);
@@ -216,11 +227,11 @@ mod tests {
         let mut codec = DynamixelProtocol {};
         assert!(std::matches!(
             codec.decode(&mut payload).unwrap_err(),
-            DynamixelDriverError::HeaderError
+            DynamixelDriverError::HeaderError(_)
         ));
         assert!(std::matches!(
             codec.decode(&mut payload).unwrap_err(),
-            DynamixelDriverError::HeaderError
+            DynamixelDriverError::HeaderLenTooSmall
         ));
         let res = codec.decode(&mut payload).unwrap().unwrap();
         assert_eq!(res, Status::new(1, vec![0x20]));