Integrate hand gateway and spatial force rendering

src/serial_core/codecs/hand_gateway.rs

@@ -0,0 +1,404 @@
+use crate::serial_core::codec::Codec;
+use crate::serial_core::error::CodecError;
+use crate::serial_core::utils::{calc_crc8_itu, elapsed_millis};
+use std::time::Instant;
+
+const FRAME_HEADER: [u8; 2] = [0x55, 0xAA];
+const RESPONSE_DATA_CMD: u8 = 0x81;
+const RESPONSE_DATA_LEGACY_FIXED_LENGTH: usize = 16;
+const RESPONSE_DATA_LEGACY_PREFIX_LENGTH: usize = 19;
+const RESPONSE_DATA_TEMP_FIXED_LENGTH: usize = 20;
+const RESPONSE_DATA_TEMP_PREFIX_LENGTH: usize = 23;
+const MIN_FRAME_LENGTH: usize = 7;
+const DEFAULT_MAX_FRAME_LENGTH: usize = 64 * 1024;
+
+#[derive(Debug, Clone, Copy)]
+struct HandGatewayResponseLayout {
+    fixed_length: usize,
+    prefix_length: usize,
+    timestamp_len: usize,
+    config_offset: usize,
+    valid_config_offset: usize,
+    block_count_offset: usize,
+}
+
+const TEMP_RESPONSE_LAYOUT: HandGatewayResponseLayout = HandGatewayResponseLayout {
+    fixed_length: RESPONSE_DATA_TEMP_FIXED_LENGTH,
+    prefix_length: RESPONSE_DATA_TEMP_PREFIX_LENGTH,
+    timestamp_len: 8,
+    config_offset: 14,
+    valid_config_offset: 18,
+    block_count_offset: 22,
+};
+
+const LEGACY_RESPONSE_LAYOUT: HandGatewayResponseLayout = HandGatewayResponseLayout {
+    fixed_length: RESPONSE_DATA_LEGACY_FIXED_LENGTH,
+    prefix_length: RESPONSE_DATA_LEGACY_PREFIX_LENGTH,
+    timestamp_len: 4,
+    config_offset: 10,
+    valid_config_offset: 14,
+    block_count_offset: 18,
+};
+
+#[derive(Debug, Clone, PartialEq, Eq)]
+pub struct HandGatewayNodeConfig {
+    pub config_mask: u32,
+    pub sample_count: usize,
+}
+
+impl HandGatewayNodeConfig {
+    pub const fn new(config_mask: u32, sample_count: usize) -> Self {
+        Self {
+            config_mask,
+            sample_count,
+        }
+    }
+
+    fn payload_len(&self, bytes_per_sample: usize) -> Option<usize> {
+        self.sample_count.checked_mul(bytes_per_sample)
+    }
+}
+
+#[derive(Debug, Clone, PartialEq, Eq)]
+pub struct HandGatewayConfig {
+    pub protocol_version: u8,
+    pub bytes_per_sample: usize,
+    pub max_frame_length: usize,
+    pub nodes: Vec<HandGatewayNodeConfig>,
+}
+
+impl HandGatewayConfig {
+    pub fn new(nodes: Vec<HandGatewayNodeConfig>) -> Self {
+        Self {
+            protocol_version: 0x01,
+            bytes_per_sample: 2,
+            max_frame_length: DEFAULT_MAX_FRAME_LENGTH,
+            nodes,
+        }
+    }
+
+    fn validate(&self) -> Result<(), CodecError> {
+        if self.bytes_per_sample == 0 || self.max_frame_length < MIN_FRAME_LENGTH {
+            return Err(CodecError::InvalidLength);
+        }
+
+        let mut used_masks = 0u32;
+        for node in &self.nodes {
+            if node.config_mask == 0
+                || node.config_mask.count_ones() != 1
+                || used_masks & node.config_mask != 0
+                || node.payload_len(self.bytes_per_sample).is_none()
+            {
+                return Err(CodecError::InvalidLength);
+            }
+            used_masks |= node.config_mask;
+        }
+
+        Ok(())
+    }
+}
+
+#[derive(Debug, Clone, PartialEq, Eq)]
+pub struct HandGatewayFrameNode {
+    pub config_mask: u32,
+    pub valid: bool,
+    pub payload: Vec<u8>,
+}
+
+#[derive(Debug, Clone, PartialEq, Eq)]
+pub struct HandGatewayDataRepFrame {
+    pub timestamp: u64,
+    pub config: u32,
+    pub valid_config: u32,
+    pub block_count: u8,
+    pub raw: Vec<u8>,
+    pub nodes: Vec<HandGatewayFrameNode>,
+    pub dts_ms: u64,
+}
+
+#[derive(Debug, Clone, PartialEq, Eq)]
+pub enum HandGatewayFrame {
+    DataRep(HandGatewayDataRepFrame),
+}
+
+pub struct HandGatewayCodec {
+    buffer: Vec<u8>,
+    config: HandGatewayConfig,
+}
+
+impl HandGatewayCodec {
+    pub fn new(node_sample_counts: &[u16]) -> Self {
+        let nodes = node_sample_counts
+            .iter()
+            .enumerate()
+            .map(|(index, &sample_count)| {
+                HandGatewayNodeConfig::new(
+                    1u32.checked_shl(index as u32).unwrap_or(0),
+                    sample_count as usize,
+                )
+            })
+            .collect();
+
+        Self {
+            buffer: Vec::new(),
+            config: HandGatewayConfig::new(nodes),
+        }
+    }
+
+    pub fn parse_node_payload(data: &[u8]) -> Result<Vec<u16>, CodecError> {
+        if data.len() % 2 != 0 {
+            return Err(CodecError::InvalidLength);
+        }
+
+        Ok(data
+            .chunks_exact(2)
+            .map(|chunk| u16::from_le_bytes([chunk[0], chunk[1]]))
+            .collect())
+    }
+
+    fn retain_possible_header_prefix(&mut self) {
+        if self.buffer.last() == Some(&FRAME_HEADER[0]) {
+            self.buffer.drain(..self.buffer.len() - 1);
+        } else {
+            self.buffer.clear();
+        }
+    }
+
+    fn parse_data_response_with_layout(
+        &self,
+        frame: &[u8],
+        length: u16,
+        session_started_at: Instant,
+        layout: HandGatewayResponseLayout,
+    ) -> Result<HandGatewayFrame, CodecError> {
+        let payload_len = usize::from(length)
+            .checked_sub(layout.fixed_length)
+            .ok_or(CodecError::InvalidLength)?;
+        let payload_end = layout
+            .prefix_length
+            .checked_add(payload_len)
+            .ok_or(CodecError::PayloadTooLarge)?;
+
+        if payload_end + 1 != frame.len() {
+            return Err(CodecError::InvalidLength);
+        }
+
+        let version = frame[5];
+        if version != self.config.protocol_version {
+            return Err(CodecError::InvalidFrameType);
+        }
+
+        let timestamp = match layout.timestamp_len {
+            4 => u32::from_le_bytes(frame[6..10].try_into().unwrap()) as u64,
+            8 => u64::from_le_bytes(frame[6..14].try_into().unwrap()),
+            _ => return Err(CodecError::InvalidLength),
+        };
+        let config = u32::from_le_bytes(
+            frame[layout.config_offset..layout.config_offset + 4]
+                .try_into()
+                .unwrap(),
+        );
+        let valid_config = u32::from_le_bytes(
+            frame[layout.valid_config_offset..layout.valid_config_offset + 4]
+                .try_into()
+                .unwrap(),
+        );
+        let block_count = frame[layout.block_count_offset];
+
+        let active_nodes = self
+            .config
+            .nodes
+            .iter()
+            .filter(|node| config & node.config_mask != 0)
+            .collect::<Vec<_>>();
+
+        if active_nodes.len() != usize::from(block_count) {
+            return Err(CodecError::InvalidLength);
+        }
+
+        let expected_payload_len = active_nodes.iter().try_fold(0usize, |total, node| {
+            let node_len = node
+                .payload_len(self.config.bytes_per_sample)
+                .ok_or(CodecError::PayloadTooLarge)?;
+            total
+                .checked_add(node_len)
+                .ok_or(CodecError::PayloadTooLarge)
+        })?;
+
+        if expected_payload_len != payload_len {
+            return Err(CodecError::InvalidLength);
+        }
+
+        let mut cursor = layout.prefix_length;
+        let mut nodes = Vec::with_capacity(active_nodes.len());
+        for node in active_nodes {
+            let node_len = node
+                .payload_len(self.config.bytes_per_sample)
+                .ok_or(CodecError::PayloadTooLarge)?;
+            let next = cursor + node_len;
+            nodes.push(HandGatewayFrameNode {
+                config_mask: node.config_mask,
+                valid: valid_config & node.config_mask != 0,
+                payload: frame[cursor..next].to_vec(),
+            });
+            cursor = next;
+        }
+
+        Ok(HandGatewayFrame::DataRep(HandGatewayDataRepFrame {
+            timestamp,
+            config,
+            valid_config,
+            block_count,
+            raw: frame.to_vec(),
+            nodes,
+            dts_ms: elapsed_millis(session_started_at),
+        }))
+    }
+
+    fn parse_data_response(
+        &self,
+        frame: &[u8],
+        length: u16,
+        session_started_at: Instant,
+    ) -> Result<HandGatewayFrame, CodecError> {
+        self.parse_data_response_with_layout(
+            frame,
+            length,
+            session_started_at,
+            TEMP_RESPONSE_LAYOUT,
+        )
+        .or_else(|_| {
+            self.parse_data_response_with_layout(
+                frame,
+                length,
+                session_started_at,
+                LEGACY_RESPONSE_LAYOUT,
+            )
+        })
+    }
+}
+
+impl Codec<HandGatewayFrame> for HandGatewayCodec {
+    fn decode(
+        &mut self,
+        input: &[u8],
+        session_started_at: Instant,
+    ) -> Result<Vec<HandGatewayFrame>, CodecError> {
+        self.config.validate()?;
+        self.buffer.extend_from_slice(input);
+        let mut frames = Vec::new();
+
+        loop {
+            let Some(header_pos) = self
+                .buffer
+                .windows(FRAME_HEADER.len())
+                .position(|window| window == FRAME_HEADER)
+            else {
+                self.retain_possible_header_prefix();
+                break;
+            };
+
+            if header_pos > 0 {
+                self.buffer.drain(..header_pos);
+            }
+
+            if self.buffer.len() < 4 {
+                break;
+            }
+
+            let length = u16::from_le_bytes([self.buffer[2], self.buffer[3]]);
+            let frame_len = usize::from(length)
+                .checked_add(4)
+                .ok_or(CodecError::PayloadTooLarge)?;
+
+            if frame_len < MIN_FRAME_LENGTH || frame_len > self.config.max_frame_length {
+                log::debug!("invalid hand gateway frame length: {frame_len}");
+                self.buffer.drain(..1);
+                continue;
+            }
+
+            if self.buffer.len() < frame_len {
+                break;
+            }
+
+            let expected_checksum = calc_crc8_itu(&self.buffer[..frame_len - 1]);
+            let received_checksum = self.buffer[frame_len - 1];
+            if expected_checksum != received_checksum {
+                log::debug!(
+                    "hand gateway checksum mismatch: expected {expected_checksum:02X}, got {received_checksum:02X}"
+                );
+                self.buffer.drain(..1);
+                continue;
+            }
+
+            if self.buffer[4] == RESPONSE_DATA_CMD {
+                match self.parse_data_response(
+                    &self.buffer[..frame_len],
+                    length,
+                    session_started_at,
+                ) {
+                    Ok(frame) => frames.push(frame),
+                    Err(error) => log::debug!("invalid hand gateway data response: {error}"),
+                }
+            }
+
+            self.buffer.drain(..frame_len);
+        }
+
+        Ok(frames)
+    }
+
+    fn encode(&self, frame: &HandGatewayFrame) -> Result<Vec<u8>, CodecError> {
+        let _ = frame;
+        Err(CodecError::InvalidFrameType)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn data_response(config: u32, valid_config: u32, payload: &[u8]) -> Vec<u8> {
+        let block_count = config.count_ones() as u8;
+        let length = (RESPONSE_DATA_LEGACY_FIXED_LENGTH + payload.len()) as u16;
+        let mut frame = Vec::new();
+        frame.extend_from_slice(&FRAME_HEADER);
+        frame.extend_from_slice(&length.to_le_bytes());
+        frame.push(RESPONSE_DATA_CMD);
+        frame.push(0x01);
+        frame.extend_from_slice(&0x0102_0304u32.to_le_bytes());
+        frame.extend_from_slice(&config.to_le_bytes());
+        frame.extend_from_slice(&valid_config.to_le_bytes());
+        frame.push(block_count);
+        frame.extend_from_slice(payload);
+        let checksum = calc_crc8_itu(&frame);
+        frame.push(checksum);
+        frame
+    }
+
+    #[test]
+    fn decodes_data_response_frame() {
+        let payload = [0x10, 0x00, 0x34, 0x12, 0x08, 0x00];
+        let bytes = data_response(0b11, 0b01, &payload);
+        let frames = HandGatewayCodec::new(&[2, 1])
+            .decode(&bytes, Instant::now())
+            .unwrap();
+
+        assert_eq!(frames.len(), 1);
+        let HandGatewayFrame::DataRep(frame) = &frames[0];
+        assert_eq!(frame.timestamp, 0x0102_0304);
+        assert_eq!(frame.config, 0b11);
+        assert_eq!(frame.valid_config, 0b01);
+        assert_eq!(frame.block_count, 2);
+        assert_eq!(frame.raw, bytes);
+        assert_eq!(frame.nodes.len(), 2);
+        assert_eq!(
+            HandGatewayCodec::parse_node_payload(&frame.nodes[0].payload).unwrap(),
+            vec![16, 0x1234]
+        );
+        assert_eq!(
+            HandGatewayCodec::parse_node_payload(&frame.nodes[1].payload).unwrap(),
+            vec![8]
+        );
+    }
+}

src/serial_core/codecs/mod.rs

@@ -1 +1,2 @@
+pub mod hand_gateway;
 pub mod tactile_a;

src/serial_core/mod.rs

@@ -2,5 +2,6 @@ pub mod codec;
 pub mod codecs;
 pub mod error;
 pub mod frame;
+pub mod multi_dim_force;
 pub mod serial;
 pub mod utils;

src/serial_core/multi_dim_force.rs

@@ -0,0 +1,234 @@
+const SENSOR_ROWS: usize = 12;
+const SENSOR_COLS: usize = 7;
+const SENSOR_COUNT: usize = SENSOR_ROWS * SENSOR_COLS;
+
+const TOTAL_PRESSURE_LOW_THRESHOLD: f32 = 500.0;
+const COP_STABILITY_FRAMES_REQUIRED: usize = 15;
+
+const POST_INIT_WINDOW_CNT: usize = 100;
+const POST_INIT_STABLE_CNT: usize = 50;
+const POST_INIT_STABLE_THRESH: f32 = 0.1;
+
+#[derive(Debug, Clone, Copy)]
+pub struct PztSpatialAnalysis {
+    pub angle_deg: f32,
+    pub magnitude: f32,
+    pub planar_x: f32,
+    pub planar_y: f32,
+}
+
+pub struct PztProcessor {
+    first_frame: Option<Vec<f32>>,
+
+    first_contact_cop_x: Option<f32>,
+    first_contact_cop_y: Option<f32>,
+    contact_initialized: bool,
+
+    total_pressure_low_counter: usize,
+
+    cop_init_x_buf: Vec<f32>,
+    cop_init_y_buf: Vec<f32>,
+
+    post_init_frame_cnt: usize,
+    post_stable_cnt: usize,
+    post_refined_flag: bool,
+    post_cand_x: Option<f32>,
+    post_cand_y: Option<f32>,
+}
+
+impl PztProcessor {
+    pub fn new() -> Self {
+        Self {
+            first_frame: None,
+            first_contact_cop_x: None,
+            first_contact_cop_y: None,
+            contact_initialized: false,
+            total_pressure_low_counter: 0,
+            cop_init_x_buf: Vec::with_capacity(COP_STABILITY_FRAMES_REQUIRED),
+            cop_init_y_buf: Vec::with_capacity(COP_STABILITY_FRAMES_REQUIRED),
+            post_init_frame_cnt: 0,
+            post_stable_cnt: 0,
+            post_refined_flag: false,
+            post_cand_x: None,
+            post_cand_y: None,
+        }
+    }
+
+    fn subtract_baseline(&mut self, current_frame: &[f32]) -> Vec<f32> {
+        if self.first_frame.is_none() {
+            self.first_frame = Some(current_frame.to_vec());
+        }
+
+        let baseline = self.first_frame.as_ref().unwrap();
+        current_frame
+            .iter()
+            .zip(baseline.iter())
+            .map(|(current, baseline)| (current - baseline).max(0.0))
+            .collect()
+    }
+
+    fn reset_cop_state(&mut self) {
+        self.first_contact_cop_x = None;
+        self.first_contact_cop_y = None;
+        self.contact_initialized = false;
+        self.total_pressure_low_counter = 0;
+        self.cop_init_x_buf.clear();
+        self.cop_init_y_buf.clear();
+        self.post_init_frame_cnt = 0;
+        self.post_stable_cnt = 0;
+        self.post_refined_flag = false;
+        self.post_cand_x = None;
+        self.post_cand_y = None;
+    }
+
+    fn compute_median(sorted: &[f32]) -> f32 {
+        let n = sorted.len();
+        if n == 0 {
+            return 0.0;
+        }
+
+        if n % 2 == 0 {
+            (sorted[n / 2 - 1] + sorted[n / 2]) / 2.0
+        } else {
+            sorted[n / 2]
+        }
+    }
+
+    fn compute_pressure_direction(&mut self, frame: &[f32]) -> (f32, f32) {
+        let total_pressure = frame.iter().sum::<f32>();
+        if total_pressure < TOTAL_PRESSURE_LOW_THRESHOLD {
+            self.total_pressure_low_counter += 1;
+        } else {
+            self.total_pressure_low_counter = 0;
+        }
+
+        if self.total_pressure_low_counter >= COP_STABILITY_FRAMES_REQUIRED {
+            self.reset_cop_state();
+            return (0.0, 0.0);
+        }
+
+        if total_pressure == 0.0 {
+            return (0.0, 0.0);
+        }
+
+        let mut sum_x = 0.0;
+        let mut sum_y = 0.0;
+        for row in 0..SENSOR_ROWS {
+            for col in 0..SENSOR_COLS {
+                let value = frame[row * SENSOR_COLS + col];
+                sum_x += value * col as f32;
+                sum_y += value * row as f32;
+            }
+        }
+        let cop_x = sum_x / total_pressure;
+        let cop_y = sum_y / total_pressure;
+
+        if !self.contact_initialized {
+            self.cop_init_x_buf.push(cop_x);
+            self.cop_init_y_buf.push(cop_y);
+
+            if self.cop_init_x_buf.len() >= COP_STABILITY_FRAMES_REQUIRED {
+                let mut xs = self.cop_init_x_buf.clone();
+                xs.sort_by(|a, b| a.partial_cmp(b).unwrap());
+                let mut ys = self.cop_init_y_buf.clone();
+                ys.sort_by(|a, b| a.partial_cmp(b).unwrap());
+
+                self.first_contact_cop_x = Some(Self::compute_median(&xs));
+                self.first_contact_cop_y = Some(Self::compute_median(&ys));
+                self.contact_initialized = true;
+                self.cop_init_x_buf.clear();
+                self.cop_init_y_buf.clear();
+            }
+
+            return (0.0, 0.0);
+        }
+
+        self.post_init_frame_cnt += 1;
+        if !self.post_refined_flag && self.post_init_frame_cnt <= POST_INIT_WINDOW_CNT {
+            if let (Some(cx), Some(cy)) = (self.post_cand_x, self.post_cand_y) {
+                let dist = ((cop_x - cx).powi(2) + (cop_y - cy).powi(2)).sqrt();
+                if dist <= POST_INIT_STABLE_THRESH {
+                    self.post_stable_cnt += 1;
+                } else {
+                    self.post_cand_x = Some(cop_x);
+                    self.post_cand_y = Some(cop_y);
+                    self.post_stable_cnt = 1;
+                }
+            } else {
+                self.post_cand_x = Some(cop_x);
+                self.post_cand_y = Some(cop_y);
+                self.post_stable_cnt = 1;
+            }
+
+            if self.post_stable_cnt >= POST_INIT_STABLE_CNT {
+                self.first_contact_cop_x = self.post_cand_x;
+                self.first_contact_cop_y = self.post_cand_y;
+                self.post_refined_flag = true;
+            }
+        } else {
+            self.post_refined_flag = true;
+        }
+
+        let base_x = self.first_contact_cop_x.unwrap_or(cop_x);
+        let base_y = self.first_contact_cop_y.unwrap_or(cop_y);
+        let delta_x = cop_x - base_x;
+        let delta_y = base_y - cop_y;
+        (delta_x, delta_y)
+    }
+
+    fn compute_vector_angle(x: f32, y: f32) -> (f32, f32) {
+        let epsilon = 1e-8f32;
+        let magnitude = (x * x + y * y).sqrt();
+        let mut angle = y.atan2(x + epsilon).to_degrees();
+        if angle < 0.0 {
+            angle += 360.0;
+        }
+        (angle, magnitude)
+    }
+
+    pub fn get_pzt_analysis(
+        &mut self,
+        adc_data: &[f32],
+    ) -> Result<PztSpatialAnalysis, &'static str> {
+        if adc_data.len() != SENSOR_COUNT {
+            return Err("ADC data length must be 84");
+        }
+
+        let baseline = self.subtract_baseline(adc_data);
+        let (dx, dy) = self.compute_pressure_direction(&baseline);
+
+        let planar_x = dx;
+        let planar_y = -dy;
+        let (angle_deg, magnitude) = Self::compute_vector_angle(planar_x, planar_y);
+
+        Ok(PztSpatialAnalysis {
+            angle_deg,
+            magnitude,
+            planar_x,
+            planar_y,
+        })
+    }
+
+    pub fn get_pzt_angle(&mut self, adc_data: &[f32]) -> Result<f32, &'static str> {
+        Ok(self.get_pzt_analysis(adc_data)?.angle_deg)
+    }
+
+    pub fn reset_baseline(&mut self) {
+        self.first_frame = None;
+        self.reset_cop_state();
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn idle_frame_returns_zero() {
+        let mut processor = PztProcessor::new();
+        let frame = [0.0f32; SENSOR_COUNT];
+        let analysis = processor.get_pzt_analysis(&frame).unwrap();
+        assert_eq!(analysis.magnitude, 0.0);
+        assert_eq!(analysis.angle_deg, 0.0);
+    }
+}

src/serial_core/serial.rs

@@ -1,4 +1,5 @@
 use crate::serial_core::codec::Codec;
+use crate::serial_core::codecs::hand_gateway::{HandGatewayCodec, HandGatewayFrame};
 use crate::serial_core::codecs::tactile_a::TactileACodec;
 use crate::serial_core::frame::TactileAFrame;
 use crossbeam_channel::{Receiver, Sender};
@@ -6,6 +7,7 @@ use std::io::{Read, Write};
 use std::time::{Duration, Instant};
 
 const POLL_INTERVAL_MS: u64 = 10;
+const DEFAULT_HAND_GATEWAY_NODE_SAMPLE_COUNTS: &[u16] = &[84, 84, 84, 84, 84, 70, 44];
 
 #[derive(Debug, Default, Clone, Copy, PartialEq, Eq)]
 pub struct SerialIoStats {
@@ -13,9 +15,32 @@ pub struct SerialIoStats {
     pub tx_bytes: u64,
 }
 
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum SerialProtocol {
+    TactileA,
+    HandGateway,
+}
+
 /// Runs the serial polling loop on the calling (background) thread.
 /// Sends decoded pressure matrix data (Vec<i32>) to the output channel.
 pub fn run_serial_loop(
+    port: &mut dyn ReadWrite,
+    rows: usize,
+    cols: usize,
+    protocol: SerialProtocol,
+    cancel_rx: &Receiver<()>,
+    sample_tx: &Sender<Vec<i32>>,
+    stats_tx: Option<&Sender<SerialIoStats>>,
+) {
+    match protocol {
+        SerialProtocol::TactileA => {
+            run_tactile_a_loop(port, rows, cols, cancel_rx, sample_tx, stats_tx)
+        }
+        SerialProtocol::HandGateway => run_hand_gateway_loop(port, cancel_rx, sample_tx, stats_tx),
+    }
+}
+
+fn run_tactile_a_loop(
     port: &mut dyn ReadWrite,
     rows: usize,
     cols: usize,
@@ -86,6 +111,97 @@ pub fn run_serial_loop(
     }
 }
 
+fn run_hand_gateway_loop(
+    port: &mut dyn ReadWrite,
+    cancel_rx: &Receiver<()>,
+    sample_tx: &Sender<Vec<i32>>,
+    stats_tx: Option<&Sender<SerialIoStats>>,
+) {
+    let session_started_at = Instant::now();
+    let mut codec = HandGatewayCodec::new(DEFAULT_HAND_GATEWAY_NODE_SAMPLE_COUNTS);
+    let mut buffer = [0u8; 1024];
+    let poll_interval = Duration::from_millis(POLL_INTERVAL_MS);
+    let mut io_stats = SerialIoStats::default();
+
+    loop {
+        if cancel_rx.try_recv().is_ok() {
+            break;
+        }
+
+        let deadline = Instant::now() + poll_interval;
+        loop {
+            if Instant::now() >= deadline {
+                break;
+            }
+
+            match port.read(&mut buffer) {
+                Ok(n) if n > 0 => {
+                    io_stats.rx_bytes += n as u64;
+                    publish_stats(stats_tx, io_stats);
+                    if let Ok(frames) = codec.decode(&buffer[..n], session_started_at) {
+                        for frame in frames {
+                            let HandGatewayFrame::DataRep(rep) = frame;
+                            // println!(
+                            //     "[hand-packet-raw] bytes={} timestamp_us={} config=0x{:08X} valid_config=0x{:08X} block_count={} payload_bytes={} raw={}",
+                            //     rep.raw.len(),
+                            //     rep.timestamp,
+                            //     rep.config,
+                            //     rep.valid_config,
+                            //     rep.block_count,
+                            //     rep.nodes
+                            //         .iter()
+                            //         .map(|node| node.payload.len())
+                            //         .sum::<usize>(),
+                            //     format_hex_bytes(&rep.raw)
+                            // );
+                            let mut vals = Vec::new();
+                            let mut parse_ok = true;
+                            for node in &rep.nodes {
+                                match HandGatewayCodec::parse_node_payload(&node.payload) {
+                                    Ok(node_values) => {
+                                        vals.extend(node_values.into_iter().map(|raw| {
+                                            let raw = raw as i32;
+                                            if raw < 15 { 0 } else { raw }
+                                        }));
+                                    }
+                                    Err(err) => {
+                                        parse_ok = false;
+                                        eprintln!("[hand-packet-values] parse error: {err}");
+                                        break;
+                                    }
+                                }
+                            }
+
+                            if parse_ok {
+                                // println!("[hand-packet-values] samples={}", vals.len());
+                                let _ = sample_tx.try_send(vals);
+                            }
+                        }
+                    }
+                }
+                Ok(_) => {
+                    std::thread::sleep(Duration::from_millis(1));
+                }
+                Err(ref e) if e.kind() == std::io::ErrorKind::TimedOut => {
+                    continue;
+                }
+                Err(e) => {
+                    eprintln!("[serial] hand gateway read error: {e}");
+                    return;
+                }
+            }
+        }
+    }
+}
+
+fn format_hex_bytes(bytes: &[u8]) -> String {
+    bytes
+        .iter()
+        .map(|byte| format!("{byte:02X}"))
+        .collect::<Vec<_>>()
+        .join(" ")
+}
+
 fn publish_stats(stats_tx: Option<&Sender<SerialIoStats>>, stats: SerialIoStats) {
     if let Some(tx) = stats_tx {
         let _ = tx.try_send(stats);

src/serial_core/utils.rs

@@ -26,13 +26,17 @@ pub fn calc_crc8_itu(c: &[u8]) -> u8 {
     crc8_itu_alg.checksum(c)
 }
 
+pub fn calc_crc8_itu_xor55(c: &[u8]) -> u8 {
+    calc_crc8_itu(c) ^ 0x55
+}
+
 pub fn elapsed_millis(start_at: Instant) -> u64 {
     start_at.elapsed().as_millis() as u64
 }
 
 #[cfg(test)]
 mod test {
-    use crate::serial_core::utils::{calc_crc8_itu, calc_crc8_smbus};
+    use crate::serial_core::utils::{calc_crc8_itu, calc_crc8_itu_xor55, calc_crc8_smbus};
 
     #[test]
     fn test_crc8_itu() {
@@ -51,4 +55,10 @@ mod test {
         let checksum = calc_crc8_smbus(req_vec.as_slice());
         assert_eq!(checksum, 0x2F);
     }
+
+    #[test]
+    fn test_crc8_itu_xor55() {
+        let req_vec = vec![0x55, 0xAA, 0x07, 0x00, 0x01, 0x01, 0xFF, 0xFF, 0xFF, 0xFF];
+        assert_eq!(calc_crc8_itu_xor55(req_vec.as_slice()), 0xFD);
+    }
 }