Decode motion and telemetry samples

serial-test/src/main.rs

@@ -143,6 +143,7 @@ fn command_data(frame: &McuFrame) -> String {
     match &frame.command {
         McuCommand::HostPeriodicQuery(data) => format!("data={data}"),
         McuCommand::HostMotionControl(data) => format!("data={data}"),
+        McuCommand::HostMotionProgram(data) => format!("data={data}"),
         McuCommand::HostControlResponse(data) => format!("data={data}"),
         McuCommand::HostAction(data) => format!("data={data}"),
         McuCommand::DeviceQueryResponse(data) => format!("data={data}"),
@@ -151,6 +152,7 @@ fn command_data(frame: &McuFrame) -> String {
         McuCommand::DevicePeriodicStatus(data) => format!("data={data}"),
         McuCommand::DeviceActionResponse(data) => format!("data={data}"),
         McuCommand::DeviceSensorStatus(data) => format!("data={data}"),
+        McuCommand::TransportAck(data) => format!("crc={}", hex_bytes(&data.crc)),
         McuCommand::Unknown(_) => format!(
             "payload={} raw={}",
             hex_bytes(&frame.body),

serial-test/src/protocol/device_periodic_status.rs

@@ -4,18 +4,46 @@ use super::hex_bytes;
 
 #[derive(Debug, Clone, PartialEq, Eq)]
 pub enum DevicePeriodicStatusData {
+    PeriodicStatus {
+        group: u8,
+        fields: Vec<u8>,
+        status_tail: Vec<u8>,
+    },
     UnknownPayload(Vec<u8>),
 }
 
 impl DevicePeriodicStatusData {
     pub fn from_body(body: &[u8]) -> Self {
-        Self::UnknownPayload(body.to_vec())
+        let Some((&group, fields)) = body.split_first() else {
+            return Self::UnknownPayload(body.to_vec());
+        };
+        let status_tail = if fields.len() >= 2 {
+            fields[fields.len() - 2..].to_vec()
+        } else {
+            fields.to_vec()
+        };
+
+        Self::PeriodicStatus {
+            group,
+            fields: fields.to_vec(),
+            status_tail,
+        }
     }
 }
 
 impl fmt::Display for DevicePeriodicStatusData {
     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
         match self {
+            Self::PeriodicStatus {
+                group,
+                fields,
+                status_tail,
+            } => write!(
+                f,
+                "PeriodicStatus(group=0x{group:02x}, status_tail={}, fields={})",
+                hex_bytes(status_tail),
+                hex_bytes(fields)
+            ),
             Self::UnknownPayload(payload) => {
                 write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
             }

serial-test/src/protocol/device_sensor_status.rs

@@ -4,18 +4,71 @@ use super::hex_bytes;
 
 #[derive(Debug, Clone, PartialEq, Eq)]
 pub enum DeviceSensorStatusData {
+    SensorStatus {
+        sensor_id: u8,
+        extruder_group: Option<ExtruderTelemetryGroup>,
+        fields: Vec<u8>,
+    },
     UnknownPayload(Vec<u8>),
 }
 
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum ExtruderTelemetryGroup {
+    ExtruderDown,
+    ExtruderUp,
+}
+
+impl fmt::Display for ExtruderTelemetryGroup {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        match self {
+            Self::ExtruderDown => f.write_str("ExtruderDown(0x96)"),
+            Self::ExtruderUp => f.write_str("ExtruderUp(0x97)"),
+        }
+    }
+}
+
 impl DeviceSensorStatusData {
     pub fn from_body(body: &[u8]) -> Self {
-        Self::UnknownPayload(body.to_vec())
+        let Some((&sensor_id, fields)) = body.split_first() else {
+            return Self::UnknownPayload(body.to_vec());
+        };
+
+        let extruder_group = fields.iter().find_map(|byte| match byte {
+            0x96 => Some(ExtruderTelemetryGroup::ExtruderDown),
+            0x97 => Some(ExtruderTelemetryGroup::ExtruderUp),
+            _ => None,
+        });
+
+        Self::SensorStatus {
+            sensor_id,
+            extruder_group,
+            fields: fields.to_vec(),
+        }
     }
 }
 
 impl fmt::Display for DeviceSensorStatusData {
     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
         match self {
+            Self::SensorStatus {
+                sensor_id,
+                extruder_group,
+                fields,
+            } => {
+                if let Some(extruder_group) = extruder_group {
+                    write!(
+                        f,
+                        "SensorStatus(sensor_id=0x{sensor_id:02x}, extruder_group={extruder_group}, fields={})",
+                        hex_bytes(fields)
+                    )
+                } else {
+                    write!(
+                        f,
+                        "SensorStatus(sensor_id=0x{sensor_id:02x}, fields={})",
+                        hex_bytes(fields)
+                    )
+                }
+            }
             Self::UnknownPayload(payload) => {
                 write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
             }

serial-test/src/protocol/host_action.rs

@@ -4,11 +4,27 @@ use super::hex_bytes;
 
 #[derive(Debug, Clone, PartialEq, Eq)]
 pub enum HostActionData {
+    ShortAction { group: u8, value: u8, arg: u16 },
+    ExtendedAction { fields: Vec<u8> },
     UnknownPayload(Vec<u8>),
 }
 
 impl HostActionData {
     pub fn from_body(body: &[u8]) -> Self {
+        if body.len() == 8 && body[..3] == [0x0c, 0x05, 0xea] && body[7] == 0x0a {
+            return Self::ShortAction {
+                group: body[3],
+                value: body[4],
+                arg: u16::from_be_bytes([body[5], body[6]]),
+            };
+        }
+
+        if body.len() == 13 && body[..3] == [0x0c, 0x0a, 0xea] {
+            return Self::ExtendedAction {
+                fields: body[3..].to_vec(),
+            };
+        }
+
         Self::UnknownPayload(body.to_vec())
     }
 }
@@ -16,6 +32,13 @@ impl HostActionData {
 impl fmt::Display for HostActionData {
     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
         match self {
+            Self::ShortAction { group, value, arg } => write!(
+                f,
+                "ShortAction(group=0x{group:02x}, value=0x{value:02x}, arg=0x{arg:04x})"
+            ),
+            Self::ExtendedAction { fields } => {
+                write!(f, "ExtendedAction(fields={})", hex_bytes(fields))
+            }
             Self::UnknownPayload(payload) => {
                 write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
             }

serial-test/src/protocol/host_motion_control.rs

@@ -1,21 +1,30 @@
 use std::fmt;
 
 use super::hex_bytes;
+use super::motion::{MotionChunk, format_chunks, parse_motion_chunks};
 
 #[derive(Debug, Clone, PartialEq, Eq)]
 pub enum HostMotionControlData {
+    MotionScript { chunks: Vec<MotionChunk> },
     UnknownPayload(Vec<u8>),
 }
 
 impl HostMotionControlData {
     pub fn from_body(body: &[u8]) -> Self {
-        Self::UnknownPayload(body.to_vec())
+        if let Some(chunks) = parse_motion_chunks(body) {
+            Self::MotionScript { chunks }
+        } else {
+            Self::UnknownPayload(body.to_vec())
+        }
     }
 }
 
 impl fmt::Display for HostMotionControlData {
     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
         match self {
+            Self::MotionScript { chunks } => {
+                write!(f, "MotionScript(chunks=[{}])", format_chunks(chunks))
+            }
             Self::UnknownPayload(payload) => {
                 write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
             }

serial-test/src/protocol/host_motion_program.rs

@@ -0,0 +1,60 @@
+use std::fmt;
+
+use super::hex_bytes;
+use super::motion::{MotionChannel, MotionChunk, format_chunks, parse_motion_chunks};
+
+#[derive(Debug, Clone, PartialEq, Eq)]
+pub enum HostMotionProgramData {
+    StartMotionProgram {
+        channel: MotionChannel,
+        flags: u8,
+        chunks: Vec<MotionChunk>,
+        raw_tail: Vec<u8>,
+    },
+    UnknownPayload(Vec<u8>),
+}
+
+impl HostMotionProgramData {
+    pub fn from_body(body: &[u8]) -> Self {
+        let Some((&channel, rest)) = body.split_first() else {
+            return Self::UnknownPayload(body.to_vec());
+        };
+        let Some((&flags, rest)) = rest.split_first() else {
+            return Self::UnknownPayload(body.to_vec());
+        };
+
+        let chunks = if rest.first().copied() == Some(0x17) {
+            parse_motion_chunks(&rest[1..]).unwrap_or_default()
+        } else {
+            parse_motion_chunks(rest).unwrap_or_default()
+        };
+
+        Self::StartMotionProgram {
+            channel: MotionChannel::from_byte(channel),
+            flags,
+            chunks,
+            raw_tail: Vec::new(),
+        }
+    }
+}
+
+impl fmt::Display for HostMotionProgramData {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        match self {
+            Self::StartMotionProgram {
+                channel,
+                flags,
+                chunks,
+                raw_tail,
+            } => write!(
+                f,
+                "StartMotionProgram(channel={channel}, flags=0x{flags:02x}, chunks=[{}], raw_tail={})",
+                format_chunks(chunks),
+                hex_bytes(raw_tail)
+            ),
+            Self::UnknownPayload(payload) => {
+                write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
+            }
+        }
+    }
+}

serial-test/src/protocol/mod.rs

@@ -7,7 +7,9 @@ mod device_sensor_status;
 mod host_action;
 mod host_control_response;
 mod host_motion_control;
+mod host_motion_program;
 mod host_periodic_query;
+pub mod motion;
 
 use std::fmt;
 
@@ -20,6 +22,7 @@ pub use device_sensor_status::DeviceSensorStatusData;
 pub use host_action::HostActionData;
 pub use host_control_response::HostControlResponseData;
 pub use host_motion_control::HostMotionControlData;
+pub use host_motion_program::HostMotionProgramData;
 pub use host_periodic_query::HostPeriodicQueryData;
 
 #[derive(Debug, Clone, Copy, PartialEq, Eq)]
@@ -43,6 +46,7 @@ impl fmt::Display for Direction {
 pub enum McuCommand {
     HostPeriodicQuery(HostPeriodicQueryData),
     HostMotionControl(HostMotionControlData),
+    HostMotionProgram(HostMotionProgramData),
     HostControlResponse(HostControlResponseData),
     HostAction(HostActionData),
     DeviceQueryResponse(DeviceQueryResponseData),
@@ -51,10 +55,16 @@ pub enum McuCommand {
     DevicePeriodicStatus(DevicePeriodicStatusData),
     DeviceActionResponse(DeviceActionResponseData),
     DeviceSensorStatus(DeviceSensorStatusData),
+    TransportAck(TransportAckData),
     Unknown(u8),
     MalformedFrame,
 }
 
+#[derive(Debug, Clone, PartialEq, Eq)]
+pub struct TransportAckData {
+    pub crc: Vec<u8>,
+}
+
 impl McuCommand {
     fn from_parts(direction: Direction, opcode: u8, body: &[u8]) -> Self {
         match (direction, opcode) {
@@ -64,6 +74,9 @@ impl McuCommand {
             (Direction::HostWrite, 0x17) => {
                 Self::HostMotionControl(HostMotionControlData::from_body(body))
             }
+            (Direction::HostWrite, 0x18) => {
+                Self::HostMotionProgram(HostMotionProgramData::from_body(body))
+            }
             (Direction::HostWrite, 0x21) => {
                 Self::HostControlResponse(HostControlResponseData::from_body(body))
             }
@@ -96,6 +109,7 @@ impl fmt::Display for McuCommand {
         match self {
             Self::HostPeriodicQuery(_) => f.write_str("HostPeriodicQuery"),
             Self::HostMotionControl(_) => f.write_str("HostMotionControl"),
+            Self::HostMotionProgram(_) => f.write_str("HostMotionProgram"),
             Self::HostControlResponse(_) => f.write_str("HostControlResponse"),
             Self::HostAction(_) => f.write_str("HostAction"),
             Self::DeviceQueryResponse(_) => f.write_str("DeviceQueryResponse"),
@@ -104,6 +118,7 @@ impl fmt::Display for McuCommand {
             Self::DevicePeriodicStatus(_) => f.write_str("DevicePeriodicStatus"),
             Self::DeviceActionResponse(_) => f.write_str("DeviceActionResponse"),
             Self::DeviceSensorStatus(_) => f.write_str("DeviceSensorStatus"),
+            Self::TransportAck(_) => f.write_str("TransportAck"),
             Self::Unknown(opcode) => write!(f, "Unknown(0x{opcode:02x})"),
             Self::MalformedFrame => f.write_str("MalformedFrame"),
         }
@@ -142,12 +157,22 @@ impl McuFrame {
 
         let len = bytes[0];
         let seq = bytes[1];
-        let opcode = bytes[2];
-        let trailer_len = if bytes.len() >= 6 { 2 } else { 1 };
+        let trailer_len = 2;
         let body_end = bytes.len() - trailer_len - 1;
-        let body = bytes[3..body_end].to_vec();
+        let opcode = if body_end > 2 { bytes[2] } else { 0 };
+        let body = if body_end > 3 {
+            bytes[3..body_end].to_vec()
+        } else {
+            Vec::new()
+        };
         let trailer = bytes[body_end..bytes.len() - 1].to_vec();
-        let command = McuCommand::from_parts(direction, opcode, &body);
+        let command = if body_end == 2 {
+            McuCommand::TransportAck(TransportAckData {
+                crc: trailer.clone(),
+            })
+        } else {
+            McuCommand::from_parts(direction, opcode, &body)
+        };
 
         Self {
             len,
@@ -221,10 +246,18 @@ mod tests {
         assert_eq!(frames.len(), 2);
         assert_eq!(frames[0].len(), 11);
         assert_eq!(frames[1].len(), 5);
+        let ack = McuFrame::parse(Direction::DeviceRead, frames[1]);
+        assert_eq!(
+            ack.command,
+            McuCommand::TransportAck(TransportAckData {
+                crc: vec![0xc9, 0x2c]
+            })
+        );
+        assert_eq!(ack.opcode, 0);
     }
 
     #[test]
-    fn unknown_command_keeps_only_unknown_payload() {
+    fn host_motion_program_decodes_opcode_18() {
         let bytes = [
             0x3a, 0x10, 0x18, 0x13, 0x00, 0x17, 0x13, 0x80, 0xf0, 0xe0, 0x82, 0x65, 0x01, 0x00,
             0x17, 0x13, 0x83, 0xb0, 0x5c, 0x02, 0xff, 0x8f, 0x3a, 0x17, 0x13, 0x82, 0x81, 0x71,
@@ -234,7 +267,7 @@ mod tests {
         ];
         let frame = McuFrame::parse(Direction::HostWrite, &bytes);
 
-        assert_eq!(frame.command, McuCommand::Unknown(0x18));
+        assert!(matches!(frame.command, McuCommand::HostMotionProgram(_)));
         assert_eq!(frame.body[0], 0x13);
         assert_eq!(frame.trailer, [0x53, 0x10]);
     }

serial-test/src/protocol/motion.rs

@@ -0,0 +1,96 @@
+use std::fmt;
+
+use super::hex_bytes;
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum MotionChannel {
+    Channel0d,
+    Channel10,
+    Channel13,
+    Unknown(u8),
+}
+
+impl MotionChannel {
+    pub fn from_byte(byte: u8) -> Self {
+        match byte {
+            0x0d => Self::Channel0d,
+            0x10 => Self::Channel10,
+            0x13 => Self::Channel13,
+            _ => Self::Unknown(byte),
+        }
+    }
+}
+
+impl fmt::Display for MotionChannel {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        match self {
+            Self::Channel0d => f.write_str("Channel0d"),
+            Self::Channel10 => f.write_str("Channel10"),
+            Self::Channel13 => f.write_str("Channel13"),
+            Self::Unknown(byte) => write!(f, "Unknown(0x{byte:02x})"),
+        }
+    }
+}
+
+#[derive(Debug, Clone, PartialEq, Eq)]
+pub struct MotionChunk {
+    pub channel: MotionChannel,
+    pub encoded_point: Vec<u8>,
+}
+
+impl fmt::Display for MotionChunk {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(
+            f,
+            "{{channel={}, encoded_point={}}}",
+            self.channel,
+            hex_bytes(&self.encoded_point)
+        )
+    }
+}
+
+pub fn parse_motion_chunks(body: &[u8]) -> Option<Vec<MotionChunk>> {
+    if body.is_empty() {
+        return None;
+    }
+
+    let mut chunks = Vec::new();
+    let mut offset = 0;
+
+    loop {
+        let channel = *body.get(offset)?;
+        let point_start = offset + 1;
+        let mut next = body.len();
+
+        for i in point_start..body.len().saturating_sub(1) {
+            if body[i] == 0x17 {
+                next = i;
+                break;
+            }
+        }
+
+        chunks.push(MotionChunk {
+            channel: MotionChannel::from_byte(channel),
+            encoded_point: body[point_start..next].to_vec(),
+        });
+
+        if next == body.len() {
+            break;
+        }
+
+        offset = next + 1;
+        if offset >= body.len() {
+            return None;
+        }
+    }
+
+    Some(chunks)
+}
+
+pub fn format_chunks(chunks: &[MotionChunk]) -> String {
+    chunks
+        .iter()
+        .map(ToString::to_string)
+        .collect::<Vec<_>>()
+        .join(", ")
+}