From 6187976b6bca99b276d529dc9467846f92cbaa9e Mon Sep 17 00:00:00 2001 From: lenn Date: Wed, 20 May 2026 08:33:20 +0800 Subject: [PATCH] feat: integrate tangential force HUD --- src-tauri/Cargo.toml | 2 +- src-tauri/src/serial_core/model.rs | 25 +- src-tauri/src/serial_core/multi_dim_force.rs | 545 ++++++++++++++++--- src-tauri/src/serial_core/serial.rs | 33 +- src/lib/components/CenterStage.svelte | 16 + src/lib/components/SpatialForcePanel.svelte | 499 +++++++++++++++++ src/lib/types/hud.ts | 7 + src/routes/+page.svelte | 13 +- 8 files changed, 1058 insertions(+), 82 deletions(-) create mode 100644 src/lib/components/SpatialForcePanel.svelte diff --git a/src-tauri/Cargo.toml b/src-tauri/Cargo.toml index 8a19afb..c86f43b 100644 --- a/src-tauri/Cargo.toml +++ b/src-tauri/Cargo.toml @@ -15,7 +15,7 @@ name = "tauri_demo_lib" crate-type = ["staticlib", "cdylib", "rlib"] [features] -default = [] +default = ["multi-dim"] devkit = ["dep:tonic", "dep:prost", "dep:prost-types", "dep:async-stream", "dep:dirs"] multi-dim = ["dep:ndarray"] diff --git a/src-tauri/src/serial_core/model.rs b/src-tauri/src/serial_core/model.rs index ce5b9e9..3c07ca2 100644 --- a/src-tauri/src/serial_core/model.rs +++ b/src-tauri/src/serial_core/model.rs @@ -13,6 +13,7 @@ pub struct HudPacket { pub panels: Vec, pub summary: HudSummary, pub pressure_matrix: Option>, + pub spatial_force: Option, } #[derive(serde::Serialize, Clone)] @@ -74,6 +75,14 @@ pub struct HudSignalIcon { pub tone: HudTone, } +#[derive(serde::Serialize, Clone)] +#[serde(rename_all = "camelCase")] +pub struct HudSpatialForce { + pub angle_deg: f32, + pub magnitude: f32, + pub confidence: f32, +} + struct HudPanelUpdate { source_id: String, values: Vec, @@ -89,6 +98,7 @@ pub struct HudChartState { order: Vec, summary_points: Vec, pressure_matrix: Option>, + spatial_force: Option, last_frame_seen: Option, } @@ -99,6 +109,7 @@ impl HudChartState { order: Vec::new(), summary_points: Vec::new(), pressure_matrix: None, + spatial_force: None, last_frame_seen: None, } } @@ -115,6 +126,10 @@ impl HudChartState { self.pressure_matrix = Some(values.iter().map(|value| *value as f32).collect()); } + pub fn record_spatial_force(&mut self, spatial_force: Option) { + self.spatial_force = spatial_force; + } + pub fn apply_frame(&mut self, frame: &TestFrame, decoded_values: Option<&[i32]>) -> HudPacket { let now = Instant::now(); self.last_frame_seen = Some(now); @@ -130,9 +145,15 @@ impl HudChartState { pub fn prune_stale(&mut self) -> Option { let before = self.panels.len(); let summary_points_before = self.summary_points.len(); + let had_pressure_matrix = self.pressure_matrix.is_some(); + let had_spatial_force = self.spatial_force.is_some(); self.prune_stale_at(Instant::now()); - if before == self.panels.len() && summary_points_before == self.summary_points.len() { + if before == self.panels.len() + && summary_points_before == self.summary_points.len() + && had_pressure_matrix == self.pressure_matrix.is_some() + && had_spatial_force == self.spatial_force.is_some() + { return None; } @@ -187,6 +208,7 @@ impl HudChartState { if summary_stale { self.summary_points.clear(); self.pressure_matrix = None; + self.spatial_force = None; self.last_frame_seen = None; } } @@ -205,6 +227,7 @@ impl HudChartState { panels, summary: build_summary(&self.summary_points), pressure_matrix: self.pressure_matrix.clone(), + spatial_force: self.spatial_force.clone(), } } diff --git a/src-tauri/src/serial_core/multi_dim_force.rs b/src-tauri/src/serial_core/multi_dim_force.rs index 379af89..629e9a1 100644 --- a/src-tauri/src/serial_core/multi_dim_force.rs +++ b/src-tauri/src/serial_core/multi_dim_force.rs @@ -1,122 +1,527 @@ -use ndarray::Array2; - -const TOTAL_PRESSURE_LOW_THRESHOLD: usize = 500; -const COP_STABILITY_FRAMES_REQUIRED: usize = 5; const SENSOR_ROWS: usize = 12; const SENSOR_COLS: usize = 7; +const SENSOR_COUNT: usize = SENSOR_ROWS * SENSOR_COLS; + +const CONTACT_ENTER_TOTAL_THRESHOLD: f32 = 520.0; +const CONTACT_ENTER_PEAK_THRESHOLD: f32 = 50.0; +const CONTACT_EXIT_TOTAL_THRESHOLD: f32 = 260.0; +const CONTACT_EXIT_PEAK_THRESHOLD: f32 = 28.0; +const CONTACT_ENTER_FRAMES_REQUIRED: usize = 2; +const CONTACT_EXIT_FRAMES_REQUIRED: usize = 8; + +const BASELINE_IDLE_ALPHA: f32 = 0.035; +const BASELINE_BOOTSTRAP_ALPHA: f32 = 1.0; +const BASELINE_NOISE_FLOOR: f32 = 5.0; + +const ACTIVE_CELL_MIN_VALUE: f32 = 18.0; +const ACTIVE_CELL_PEAK_RATIO: f32 = 0.14; +const MIN_ACTIVE_CELLS: usize = 3; + +const ANCHOR_LERP_ALPHA: f32 = 0.018; +const VECTOR_SMOOTHING_ALPHA: f32 = 0.16; + +const REPORT_MAGNITUDE_ENTER: f32 = 0.12; +const REPORT_MAGNITUDE_EXIT: f32 = 0.045; +const REPORT_CONFIDENCE_ENTER: f32 = 0.14; +const REPORT_CONFIDENCE_EXIT: f32 = 0.06; +const REPORT_HOLD_FRAMES: usize = 10; + +const ASYMMETRY_WEIGHT: f32 = 1.1; +const DRIFT_WEIGHT: f32 = 0.65; +const MOTION_WEIGHT: f32 = 0.25; + +#[derive(Debug, Clone, Copy)] +pub struct PztSpatialAnalysis { + pub angle_deg: f32, + pub magnitude: f32, + pub planar_x: f32, + pub planar_y: f32, + pub confidence: f32, + pub contact_active: bool, + pub reportable: bool, +} pub struct PztProcessor { - first_frame: Option>, - first_contact_cop_x: Option, - first_contact_cop_y: Option, - contact_initialized: bool, - total_pressure_low_counter: usize, + baseline_frame: Option>, + contact_active: bool, + contact_enter_counter: usize, + contact_exit_counter: usize, + anchor_cop_x: Option, + anchor_cop_y: Option, + last_cop_x: Option, + last_cop_y: Option, + smoothed_x: f32, + smoothed_y: f32, + report_active: bool, + report_hold_counter: usize, + held_report: Option, +} + +#[derive(Clone, Copy)] +struct ContactStats { + total: f32, + peak: f32, + active_total: f32, + active_cells: usize, + min_row: usize, + max_row: usize, + min_col: usize, + max_col: usize, + cop_x: f32, + cop_y: f32, + asymmetry_x: f32, + asymmetry_y: 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, + baseline_frame: None, + contact_active: false, + contact_enter_counter: 0, + contact_exit_counter: 0, + anchor_cop_x: None, + anchor_cop_y: None, + last_cop_x: None, + last_cop_y: None, + smoothed_x: 0.0, + smoothed_y: 0.0, + report_active: false, + report_hold_counter: 0, + held_report: None, } } - fn subtract_baseline(&mut self, current_frame: &[f32]) -> Vec { - if self.first_frame.is_none() { - self.first_frame = Some(current_frame.to_vec()); + fn reset_tracking_state(&mut self) { + self.contact_active = false; + self.contact_enter_counter = 0; + self.contact_exit_counter = 0; + self.anchor_cop_x = None; + self.anchor_cop_y = None; + self.last_cop_x = None; + self.last_cop_y = None; + self.smoothed_x = 0.0; + self.smoothed_y = 0.0; + } + + fn reset_report_state(&mut self) { + self.report_active = false; + self.report_hold_counter = 0; + self.held_report = None; + } + + fn update_idle_baseline(&mut self, raw_frame: &[f32], alpha: f32) { + match self.baseline_frame.as_mut() { + Some(baseline) => { + for (base, current) in baseline.iter_mut().zip(raw_frame.iter().copied()) { + *base += (current - *base) * alpha; + } + } + None => { + self.baseline_frame = Some(raw_frame.to_vec()); + } + } + } + + fn subtract_baseline(&mut self, raw_frame: &[f32]) -> Vec { + if self.baseline_frame.is_none() { + self.update_idle_baseline(raw_frame, BASELINE_BOOTSTRAP_ALPHA); } - let baseline = self.first_frame.as_ref().unwrap(); - current_frame + let baseline = self + .baseline_frame + .as_ref() + .expect("baseline should exist after bootstrap"); + + raw_frame .iter() .zip(baseline.iter()) - .map(|(c, b)| (c - b).max(0.0)) + .map(|(raw, base)| (raw - base - BASELINE_NOISE_FLOOR).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; + fn pressure_metrics(frame: &[f32]) -> (f32, f32) { + let total = frame.iter().sum::(); + let peak = frame.iter().copied().fold(0.0, f32::max); + (total, peak) } - fn compute_pressure_direction(&mut self, frame: &[f32]) -> (f32, f32) { - let frame2d = Array2::from_shape_vec((SENSOR_ROWS, SENSOR_COLS), frame.to_vec()).unwrap(); - let total_pressure: f32 = frame2d.sum(); - if total_pressure < TOTAL_PRESSURE_LOW_THRESHOLD as f32 { - self.total_pressure_low_counter += 1; - } else { - self.total_pressure_low_counter = 0; + fn is_contact_enter_frame(frame: &[f32]) -> bool { + let (total, peak) = Self::pressure_metrics(frame); + total >= CONTACT_ENTER_TOTAL_THRESHOLD && peak >= CONTACT_ENTER_PEAK_THRESHOLD + } + + fn is_contact_exit_frame(frame: &[f32]) -> bool { + let (total, peak) = Self::pressure_metrics(frame); + total <= CONTACT_EXIT_TOTAL_THRESHOLD || peak <= CONTACT_EXIT_PEAK_THRESHOLD + } + + fn inactive_analysis() -> PztSpatialAnalysis { + PztSpatialAnalysis { + angle_deg: 0.0, + magnitude: 0.0, + planar_x: 0.0, + planar_y: 0.0, + confidence: 0.0, + contact_active: false, + reportable: false, + } + } + + fn weak_contact_analysis() -> PztSpatialAnalysis { + PztSpatialAnalysis { + contact_active: true, + ..Self::inactive_analysis() + } + } + + fn compute_contact_stats(frame: &[f32]) -> Option { + let total = frame.iter().sum::(); + if total <= 0.0 { + return None; } - if self.total_pressure_low_counter >= COP_STABILITY_FRAMES_REQUIRED { - self.reset_cop_state(); - return (0.0, 0.0); + let peak = frame.iter().copied().fold(0.0, f32::max); + if peak <= 0.0 { + return None; } - if total_pressure == 0.0 { - return (0.0, 0.0); - } + let active_threshold = (peak * ACTIVE_CELL_PEAK_RATIO).max(ACTIVE_CELL_MIN_VALUE); - let mut sum_x = 0.0; - let mut sum_y = 0.0; + let mut active_total = 0.0; + let mut active_cells = 0usize; + let mut weighted_col_sum = 0.0; + let mut weighted_row_sum = 0.0; + let mut min_row = SENSOR_ROWS; + let mut max_row = 0usize; + let mut min_col = SENSOR_COLS; + let mut max_col = 0usize; - for r in 0..SENSOR_ROWS { - for c in 0..SENSOR_COLS { - let val = frame2d[(r, c)]; - sum_x += val * c as f32; - sum_y += val * r as f32; + for row in 0..SENSOR_ROWS { + for col in 0..SENSOR_COLS { + let index = row * SENSOR_COLS + col; + let value = frame[index]; + if value < active_threshold { + continue; + } + + active_cells += 1; + active_total += value; + weighted_col_sum += value * col as f32; + weighted_row_sum += value * row as f32; + min_row = min_row.min(row); + max_row = max_row.max(row); + min_col = min_col.min(col); + max_col = max_col.max(col); } } - let cop_x = sum_x / total_pressure; - let cop_y = sum_y / total_pressure; - - if !self.contact_initialized { - self.first_contact_cop_x = Some(cop_x); - self.first_contact_cop_y = Some(cop_y); - self.contact_initialized = true; - return (0.0, 0.0); + if active_cells < MIN_ACTIVE_CELLS || active_total <= 0.0 { + return None; } - let dx = cop_x - self.first_contact_cop_x.unwrap(); - let dy = cop_y - self.first_contact_cop_y.unwrap(); + let cop_x = weighted_col_sum / active_total; + let cop_y = weighted_row_sum / active_total; + let bbox_center_x = (min_col + max_col) as f32 * 0.5; + let bbox_center_y = (min_row + max_row) as f32 * 0.5; + let half_width = ((max_col - min_col).max(1) as f32) * 0.5; + let half_height = ((max_row - min_row).max(1) as f32) * 0.5; - (dx, dy) + let mut asymmetry_x = 0.0; + let mut asymmetry_y = 0.0; + + for row in min_row..=max_row { + for col in min_col..=max_col { + let index = row * SENSOR_COLS + col; + let value = frame[index]; + if value < active_threshold { + continue; + } + + asymmetry_x += value * ((col as f32 - bbox_center_x) / half_width); + asymmetry_y += value * ((row as f32 - bbox_center_y) / half_height); + } + } + + Some(ContactStats { + total, + peak, + active_total, + active_cells, + min_row, + max_row, + min_col, + max_col, + cop_x, + cop_y, + asymmetry_x: asymmetry_x / active_total, + asymmetry_y: asymmetry_y / active_total, + }) } fn compute_vector_angle(x: f32, y: f32) -> (f32, f32) { - let epsilon = 1e-8; - let mag = (x * x + y * y).sqrt(); - let mut angle = (y).atan2(x + epsilon).to_degrees(); + let magnitude = (x * x + y * y).sqrt(); + if magnitude <= f32::EPSILON { + return (0.0, 0.0); + } + + let mut angle = y.atan2(x).to_degrees(); if angle < 0.0 { angle += 360.0; } - (angle, mag) + + (angle, magnitude) } - fn compute_pzt_angle(px: f32, py: f32) -> (f32, f32) { - Self::compute_vector_angle(px, -py) + fn update_contact_state(&mut self, raw_frame: &[f32], frame: &[f32]) -> bool { + if self.contact_active { + if Self::is_contact_exit_frame(frame) { + self.contact_exit_counter += 1; + if self.contact_exit_counter >= CONTACT_EXIT_FRAMES_REQUIRED { + self.update_idle_baseline(raw_frame, BASELINE_IDLE_ALPHA); + self.reset_tracking_state(); + self.reset_report_state(); + return false; + } + } else { + self.contact_exit_counter = 0; + } + + return true; + } + + if Self::is_contact_enter_frame(frame) { + self.contact_enter_counter += 1; + if self.contact_enter_counter >= CONTACT_ENTER_FRAMES_REQUIRED { + self.contact_active = true; + self.contact_enter_counter = 0; + self.contact_exit_counter = 0; + return true; + } + + return false; + } + + self.contact_enter_counter = 0; + self.update_idle_baseline(raw_frame, BASELINE_IDLE_ALPHA); + false } - pub fn get_pzt_angle(&mut self, adc_data: &[f32]) -> Result { - if adc_data.len() != 84 { + fn store_report(&mut self, mut analysis: PztSpatialAnalysis) -> PztSpatialAnalysis { + analysis.reportable = true; + self.report_active = true; + self.report_hold_counter = 0; + self.held_report = Some(analysis); + analysis + } + + fn hold_or_drop_report(&mut self) -> PztSpatialAnalysis { + if self.report_active && self.report_hold_counter < REPORT_HOLD_FRAMES { + self.report_hold_counter += 1; + if let Some(mut held) = self.held_report { + held.reportable = true; + return held; + } + } + + self.reset_report_state(); + Self::weak_contact_analysis() + } + + fn stabilize_report(&mut self, analysis: PztSpatialAnalysis) -> PztSpatialAnalysis { + if !analysis.contact_active { + self.reset_report_state(); + return analysis; + } + + let can_enter = analysis.magnitude >= REPORT_MAGNITUDE_ENTER + && analysis.confidence >= REPORT_CONFIDENCE_ENTER; + let can_stay = analysis.magnitude >= REPORT_MAGNITUDE_EXIT + && analysis.confidence >= REPORT_CONFIDENCE_EXIT; + + if self.report_active { + if can_stay { + return self.store_report(analysis); + } + + return self.hold_or_drop_report(); + } + + if can_enter { + return self.store_report(analysis); + } + + analysis + } + + pub fn get_pzt_analysis( + &mut self, + adc_data: &[f32], + ) -> Result { + 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 (angle, _) = Self::compute_pzt_angle(dx, dy); + let baseline_subtracted = self.subtract_baseline(adc_data); + if !self.update_contact_state(adc_data, &baseline_subtracted) { + return Ok(Self::inactive_analysis()); + } - Ok(angle) + let Some(stats) = Self::compute_contact_stats(&baseline_subtracted) else { + return Ok(self.stabilize_report(Self::weak_contact_analysis())); + }; + + let Some(anchor_x) = self.anchor_cop_x else { + self.anchor_cop_x = Some(stats.cop_x); + self.anchor_cop_y = Some(stats.cop_y); + self.last_cop_x = Some(stats.cop_x); + self.last_cop_y = Some(stats.cop_y); + + return Ok(self.stabilize_report(Self::weak_contact_analysis())); + }; + let anchor_y = self.anchor_cop_y.unwrap_or(stats.cop_y); + let last_x = self.last_cop_x.unwrap_or(stats.cop_x); + let last_y = self.last_cop_y.unwrap_or(stats.cop_y); + + let drift_x = stats.cop_x - anchor_x; + let drift_y = stats.cop_y - anchor_y; + let motion_x = stats.cop_x - last_x; + let motion_y = stats.cop_y - last_y; + + let combined_x = stats.asymmetry_x * ASYMMETRY_WEIGHT + + drift_x * DRIFT_WEIGHT + + motion_x * MOTION_WEIGHT; + let combined_y = stats.asymmetry_y * ASYMMETRY_WEIGHT + + drift_y * DRIFT_WEIGHT + + motion_y * MOTION_WEIGHT; + + self.smoothed_x += (combined_x - self.smoothed_x) * VECTOR_SMOOTHING_ALPHA; + self.smoothed_y += (combined_y - self.smoothed_y) * VECTOR_SMOOTHING_ALPHA; + + self.anchor_cop_x = Some(anchor_x + drift_x * ANCHOR_LERP_ALPHA); + self.anchor_cop_y = Some(anchor_y + drift_y * ANCHOR_LERP_ALPHA); + self.last_cop_x = Some(stats.cop_x); + self.last_cop_y = Some(stats.cop_y); + + let planar_x = self.smoothed_x; + let planar_y = -self.smoothed_y; + let (angle_deg, magnitude) = Self::compute_vector_angle(planar_x, planar_y); + + let active_span_rows = (stats.max_row - stats.min_row + 1) as f32 / SENSOR_ROWS as f32; + let active_span_cols = (stats.max_col - stats.min_col + 1) as f32 / SENSOR_COLS as f32; + let activity = (stats.active_cells as f32 / SENSOR_COUNT as f32).clamp(0.0, 1.0); + let span = ((active_span_rows + active_span_cols) * 0.5).clamp(0.0, 1.0); + let pressure_ratio = (stats.active_total / stats.total.max(1.0)).clamp(0.0, 1.0); + let peak_ratio = + (stats.peak / (stats.total / stats.active_cells as f32 + 1.0)).clamp(0.0, 1.0); + let confidence = + ((activity * 0.35) + (span * 0.2) + (pressure_ratio * 0.3) + (peak_ratio * 0.15)) + .clamp(0.0, 1.0); + + Ok(self.stabilize_report(PztSpatialAnalysis { + angle_deg, + magnitude, + planar_x, + planar_y, + confidence, + contact_active: true, + reportable: false, + })) + } + + pub fn get_pzt_angle(&mut self, adc_data: &[f32]) -> Result { + Ok(self.get_pzt_analysis(adc_data)?.angle_deg) + } + + pub fn should_report(analysis: &PztSpatialAnalysis) -> bool { + analysis.reportable } pub fn reset_baseline(&mut self) { - self.first_frame = None; - self.reset_cop_state(); + self.baseline_frame = None; + self.reset_tracking_state(); + self.reset_report_state(); + } +} + +#[cfg(test)] +mod tests { + use super::{PztProcessor, SENSOR_COLS, SENSOR_ROWS}; + + fn index(row: usize, col: usize) -> usize { + row * SENSOR_COLS + col + } + + fn make_frame(active: &[(usize, usize, f32)]) -> [f32; SENSOR_ROWS * SENSOR_COLS] { + let mut frame = [0.0; SENSOR_ROWS * SENSOR_COLS]; + for (row, col, value) in active { + frame[index(*row, *col)] = *value; + } + frame + } + + #[test] + fn idle_frame_does_not_report_contact() { + let mut processor = PztProcessor::new(); + let frame = [0.0; SENSOR_ROWS * SENSOR_COLS]; + let analysis = processor.get_pzt_analysis(&frame).unwrap(); + assert!(!analysis.contact_active); + assert!(!analysis.reportable); + assert_eq!(analysis.magnitude, 0.0); + } + + #[test] + fn right_heavy_contact_reports_rightward_angle_after_confirmation() { + let mut processor = PztProcessor::new(); + let baseline = [0.0; SENSOR_ROWS * SENSOR_COLS]; + let contact = make_frame(&[ + (5, 2, 120.0), + (5, 3, 180.0), + (5, 4, 280.0), + (6, 2, 110.0), + (6, 3, 170.0), + (6, 4, 260.0), + (7, 2, 100.0), + (7, 3, 150.0), + (7, 4, 240.0), + ]); + + let _ = processor.get_pzt_analysis(&baseline).unwrap(); + + let mut analysis = processor.get_pzt_analysis(&contact).unwrap(); + for _ in 0..8 { + analysis = processor.get_pzt_analysis(&contact).unwrap(); + } + + assert!(analysis.contact_active); + assert!(analysis.reportable); + assert!(analysis.magnitude > 0.0); + assert!(analysis.angle_deg <= 45.0 || analysis.angle_deg >= 315.0); + } + + #[test] + fn report_stays_active_through_short_weak_gap() { + let mut processor = PztProcessor::new(); + let baseline = [0.0; SENSOR_ROWS * SENSOR_COLS]; + let contact = make_frame(&[ + (5, 2, 120.0), + (5, 3, 180.0), + (5, 4, 280.0), + (6, 2, 110.0), + (6, 3, 170.0), + (6, 4, 260.0), + (7, 2, 100.0), + (7, 3, 150.0), + (7, 4, 240.0), + ]); + let weak = make_frame(&[(5, 3, 55.0), (5, 4, 60.0), (6, 3, 50.0), (6, 4, 58.0)]); + + let _ = processor.get_pzt_analysis(&baseline).unwrap(); + for _ in 0..10 { + let _ = processor.get_pzt_analysis(&contact).unwrap(); + } + + let analysis = processor.get_pzt_analysis(&weak).unwrap(); + assert!(analysis.reportable); } } diff --git a/src-tauri/src/serial_core/serial.rs b/src-tauri/src/serial_core/serial.rs index fca733d..ae4329e 100644 --- a/src-tauri/src/serial_core/serial.rs +++ b/src-tauri/src/serial_core/serial.rs @@ -1,13 +1,13 @@ +#[cfg(feature = "devkit")] +use crate::devkit::{proto::SensorFrame, DevKitState}; use crate::serial_core::codec::Codec; use crate::serial_core::codecs::tactile_a::TactileACodec; use crate::serial_core::frame::{FrameHandler, TactileAFrame, TestFrame}; -use crate::serial_core::model::{HudChartState, HudPacket}; +use crate::serial_core::model::{HudChartState, HudPacket, HudSpatialForce}; #[cfg(feature = "multi-dim")] use crate::serial_core::multi_dim_force::PztProcessor; use crate::serial_core::record::Recording; use crate::serial_core::record::{FrameTiming, RecordedFrame}; -#[cfg(feature = "devkit")] -use crate::devkit::{proto::SensorFrame, DevKitState}; use anyhow::Result; use log::debug; use std::future::pending; @@ -15,9 +15,9 @@ use std::future::pending; use std::sync::atomic::Ordering; use std::sync::{Arc, Mutex}; use std::time::Instant; -use tauri::{AppHandle, Emitter}; #[cfg(feature = "devkit")] use tauri::Manager; +use tauri::{AppHandle, Emitter}; use tokio::io::{AsyncReadExt, AsyncWriteExt}; use tokio::time::{self, Duration, MissedTickBehavior}; use tokio_serial::SerialStream; @@ -33,6 +33,7 @@ pub enum PollMode { struct PendingSubFrame { frame: F, values: Vec, + spatial_force: Option, } pub trait SerialFrame: Clone + Send + 'static { @@ -266,6 +267,7 @@ where let display_values = build_display_values( &mut chart_state, pending.values.as_slice(), + pending.spatial_force, ); if let Some(packet) = pending @@ -309,11 +311,22 @@ where drop(record); if let Some(vals) = decode_res { + let mut spatial_force = None; #[cfg(feature = "multi-dim")] { let pzt_values = vals.iter().map(|value| *value as f32).collect::>(); - if let Ok(angle) = pzt_processor.get_pzt_angle(&pzt_values) { - // debug!("pzt angle: {:.2}", angle); + if let Ok(analysis) = pzt_processor.get_pzt_analysis(&pzt_values) { + debug!( + "spatial force: angle={:.2}°, magnitude={:.2}, dx={:.2}, dy={:.2}", + analysis.angle_deg, analysis.magnitude, analysis.planar_x, analysis.planar_y + ); + if PztProcessor::should_report(&analysis) { + spatial_force = Some(HudSpatialForce { + angle_deg: analysis.angle_deg, + magnitude: analysis.magnitude, + confidence: analysis.confidence, + }); + } } } #[cfg(feature = "devkit")] @@ -326,6 +339,7 @@ where pending_sub_frame = Some(PendingSubFrame { frame: frame.clone(), values: vals, + spatial_force, }); } else if let Some(packet) = frame.to_hud_packet(&mut chart_state, None) { app.emit("hud_stream", packet)?; @@ -337,11 +351,16 @@ where Ok(()) } -fn build_display_values(chart_state: &mut HudChartState, values: &[i32]) -> Option> { +fn build_display_values( + chart_state: &mut HudChartState, + values: &[i32], + spatial_force: Option, +) -> Option> { let summary = values.iter().copied().sum::(); let force = raw_to_g1(summary as u32); chart_state.record_summary(force as f32); chart_state.record_pressure_matrix(values); + chart_state.record_spatial_force(spatial_force); Some(vec![summary]) } diff --git a/src/lib/components/CenterStage.svelte b/src/lib/components/CenterStage.svelte index 49ddb5f..896d090 100644 --- a/src/lib/components/CenterStage.svelte +++ b/src/lib/components/CenterStage.svelte @@ -10,10 +10,12 @@ import NeonBreakoutArena from "$lib/components/NeonBreakoutArena.svelte"; import PressureMatrixViewer from "$lib/components/PressureMatrixViewer.svelte"; import SignalChart from "$lib/components/SignalChart.svelte"; + import SpatialForcePanel from "$lib/components/SpatialForcePanel.svelte"; import SummaryCurve from "$lib/components/SummaryCurve.svelte"; import type { HudColorMapOption, HudSignalPanel, + HudSpatialForce, HudSummary, LocaleCode, MatrixDisplayMode, @@ -26,6 +28,7 @@ export let rightPanels: HudSignalPanel[] = []; export let summary: HudSummary; export let pressureMatrix: number[] | null = null; + export let spatialForce: HudSpatialForce | null = null; export let showConfigPanel = false; export let configPanelTitle = ""; export let configPanelHint = ""; @@ -314,6 +317,19 @@ {/each} +
+ +
+ {#if summaryCurveVisible && summarySide === "right"}
+ import type { HudSpatialForce } from "$lib/types/hud"; + + export let spatialForce: HudSpatialForce | null = null; + export let side: "left" | "right" = "right"; + export let panelIndex = 0; + export let locale: "zh-CN" | "en-US" = "zh-CN"; + + function formatValue(value: number | null, digits = 1): string { + if (value === null || !Number.isFinite(value)) { + return "--"; + } + + return value.toFixed(digits); + } + + function normalizeAngle(value: number): number { + return ((value % 360) + 360) % 360; + } + + function shortestAngleDelta(from: number, to: number): number { + const delta = ((to - from + 540) % 360) - 180; + return delta === -180 ? 180 : delta; + } + + const jumpAngleThresholdDeg = 72; + + let visualAngleDeg = 0; + let previousRawAngleDeg: number | null = null; + let snapVector = false; + let snapResetFrame: number | null = null; + + function setSnapVector(): void { + snapVector = true; + + if (typeof window === "undefined") { + return; + } + + if (snapResetFrame !== null) { + window.cancelAnimationFrame(snapResetFrame); + } + + snapResetFrame = window.requestAnimationFrame(() => { + snapVector = false; + snapResetFrame = null; + }); + } + + function updateVisualAngle(rawAngleDeg: number, active: boolean): void { + if (!active) { + previousRawAngleDeg = null; + visualAngleDeg = 0; + return; + } + + if (previousRawAngleDeg === null) { + previousRawAngleDeg = rawAngleDeg; + visualAngleDeg = rawAngleDeg; + return; + } + + const delta = shortestAngleDelta(previousRawAngleDeg, rawAngleDeg); + if (Math.abs(delta) < 0.001) { + return; + } + + if (Math.abs(delta) >= jumpAngleThresholdDeg) { + setSnapVector(); + } + + visualAngleDeg += delta; + previousRawAngleDeg = rawAngleDeg; + } + + $: i18n = + locale === "zh-CN" + ? { + title: "切向力方向", + waiting: "等待数据", + angle: "ANGLE", + heading: "方向角", + strength: "强度", + confidence: "置信度" + } + : { + title: "Tangential Direction", + waiting: "Waiting", + angle: "ANGLE", + heading: "Heading", + strength: "Strength", + confidence: "Confidence" + }; + + $: hasData = + spatialForce !== null && + Number.isFinite(spatialForce.angleDeg) && + Number.isFinite(spatialForce.magnitude); + $: angleDeg = hasData ? normalizeAngle(spatialForce?.angleDeg ?? 0) : 0; + $: updateVisualAngle(angleDeg, hasData); + $: magnitude = hasData ? spatialForce?.magnitude ?? 0 : null; + $: confidence = hasData ? (spatialForce?.confidence ?? 0) * 100 : null; + + +
+
+
+

TAN

+

{i18n.title}

+
+ + +
+ +
+
+
+
+
+
+
+ {#if hasData} +
+ + +
+ {/if} +
+ 90 + 0 + 270 + 180 +
+ + {#if !hasData} +
+ {i18n.waiting} +
+ {/if} +
+ +
+

{i18n.heading}

+

{i18n.strength}: {formatValue(magnitude, 2)}

+

{i18n.confidence}: {hasData ? `${formatValue(confidence, 0)}%` : "--"}

+
+
+
+ + diff --git a/src/lib/types/hud.ts b/src/lib/types/hud.ts index ceaceee..d283bd6 100644 --- a/src/lib/types/hud.ts +++ b/src/lib/types/hud.ts @@ -41,11 +41,18 @@ export interface HudSignalPanel { max: number | null; } +export interface HudSpatialForce { + angleDeg: number; + magnitude: number; + confidence: number; +} + export interface HudPacket { ts: number; panels: HudSignalPanel[]; summary: HudSummary; pressureMatrix: number[] | null; + spatialForce: HudSpatialForce | null; } export interface HudSummary { diff --git a/src/routes/+page.svelte b/src/routes/+page.svelte index 9c0d190..258f62a 100644 --- a/src/routes/+page.svelte +++ b/src/routes/+page.svelte @@ -22,6 +22,7 @@ HudConfigLink, HudNoticeTone, HudPacket, + HudSpatialForce, PressureColorMapPreset, HudSignalPanel, HudSignalSeries, @@ -228,6 +229,7 @@ let signalPanels: HudSignalPanel[] = buildInactivePanels(); let summary: HudSummary = buildEmptySummary(); let pressureMatrix: number[] | null = null; + let spatialForce: HudSpatialForce | null = null; let matrixRows = 12; let matrixCols = 7; let rangeMin = DEFAULT_PRESSURE_RANGE_MIN; @@ -717,6 +719,7 @@ function resetReplayVisualState(): void { pressureMatrix = buildZeroMatrix(); + spatialForce = null; signalPanels = buildInactivePanels(); summary = buildEmptySummary(); hasSignalData = false; @@ -752,6 +755,7 @@ replayHasDisplayedFrame = true; replayProgress = replayFrames.length > 1 ? safeIndex / (replayFrames.length - 1) : 1; pressureMatrix = frameValuesToMatrix(replayFrames[safeIndex].values); + spatialForce = null; signalPanels = buildInactivePanels(); summary = buildReplaySummaryAt(safeIndex); hasSignalData = true; @@ -1006,7 +1010,8 @@ summary = packet.summary; } pressureMatrix = packet.pressureMatrix; - hasSignalData = signalPanels.length > 0 || packet.summary.points.length > 0; + spatialForce = packet.spatialForce ?? null; + hasSignalData = signalPanels.length > 0 || packet.summary.points.length > 0 || spatialForce !== null; } function clearHudPanels(): void { @@ -1014,17 +1019,18 @@ signalPanels = buildInactivePanels(); summary = buildEmptySummary(); pressureMatrix = null; + spatialForce = null; } function startMockFeed(push: (packet: HudPacket) => void): () => void { let panels = buildInactivePanels(); let summaryValue = buildSummary(createSummaryPoints(randomBetween(480, 1440))); - push({ ts: Date.now(), panels, summary: summaryValue, pressureMatrix: null }); + push({ ts: Date.now(), panels, summary: summaryValue, pressureMatrix: null, spatialForce: null }); const timerId = window.setInterval(() => { summaryValue = evolveSummary(summaryValue); - push({ ts: Date.now(), panels, summary: summaryValue, pressureMatrix: null }); + push({ ts: Date.now(), panels, summary: summaryValue, pressureMatrix: null, spatialForce: null }); }, signalRenderTickMs); return () => { @@ -1930,6 +1936,7 @@ leftPanels={leftSignalPanels} rightPanels={rightSignalPanels} {pressureMatrix} + {spatialForce} showConfigPanel={isConfigPanelOpen} showPrecisionTestPanel={isPrecisionTestOpen} {summary}