From 160ff543687ab4cb6cec5d229937b3fbee51537f Mon Sep 17 00:00:00 2001 From: lenn Date: Tue, 9 Jun 2026 11:05:18 +0800 Subject: [PATCH] Prepare sample delivery UI --- package-lock.json | 104 +- package.json | 4 +- src-tauri/src/serial_core/model.rs | 4 + src-tauri/src/serial_core/multi_dim_force.rs | 1002 +++++++---------- src-tauri/src/serial_core/serial.rs | 81 +- src/lib/components/CenterStage.svelte | 12 +- .../components/PressureMatrixViewer.svelte | 5 + src/lib/components/SignalChart.svelte | 11 +- src/lib/components/SpatialForcePanel.svelte | 121 +- src/lib/components/SummaryCurve.svelte | 243 ++-- src/routes/+page.svelte | 209 +++- 11 files changed, 970 insertions(+), 826 deletions(-) diff --git a/package-lock.json b/package-lock.json index 9db4a2c..40101d9 100644 --- a/package-lock.json +++ b/package-lock.json @@ -9,7 +9,7 @@ "version": "0.4.0", "license": "MIT", "dependencies": { - "@tauri-apps/api": "^2", + "@tauri-apps/api": "^2.11.0", "@tauri-apps/plugin-opener": "^2", "@tauri-apps/plugin-process": "^2.3.1", "@tauri-apps/plugin-updater": "^2.10.1", @@ -19,7 +19,7 @@ "@sveltejs/adapter-static": "^3.0.6", "@sveltejs/kit": "^2.9.0", "@sveltejs/vite-plugin-svelte": "^5.0.0", - "@tauri-apps/cli": "^2", + "@tauri-apps/cli": "^2.11.2", "@types/three": "^0.183.1", "svelte": "^5.0.0", "svelte-check": "^4.0.0", @@ -1032,9 +1032,9 @@ } }, "node_modules/@tauri-apps/api": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/api/-/api-2.10.1.tgz", - "integrity": "sha512-hKL/jWf293UDSUN09rR69hrToyIXBb8CjGaWC7gfinvnQrBVvnLr08FeFi38gxtugAVyVcTa5/FD/Xnkb1siBw==", + "version": "2.11.0", + "resolved": "https://registry.npmjs.org/@tauri-apps/api/-/api-2.11.0.tgz", + "integrity": "sha512-7CinYODhky9lmO23xHnUFv0Xt43fbtWMyxZcLcRBlFkcgXKuEirBvHpmtJ89YMhyeGcq20Wuc47Fa4XjyniywA==", "license": "Apache-2.0 OR MIT", "funding": { "type": "opencollective", @@ -1042,9 +1042,9 @@ } }, "node_modules/@tauri-apps/cli": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli/-/cli-2.10.1.tgz", - "integrity": "sha512-jQNGF/5quwORdZSSLtTluyKQ+o6SMa/AUICfhf4egCGFdMHqWssApVgYSbg+jmrZoc8e1DscNvjTnXtlHLS11g==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli/-/cli-2.11.2.tgz", + "integrity": "sha512-bk3HemqvGRoy+5D/dVMUQHKMYLglD0jVnMm/0iGMH6ufZ+p8r14m6BpIixwij3PBvZdvORUp1YifTD8QxVZ1Nw==", "dev": true, "license": "Apache-2.0 OR MIT", "bin": { @@ -1058,23 +1058,23 @@ "url": "https://opencollective.com/tauri" }, "optionalDependencies": { - "@tauri-apps/cli-darwin-arm64": "2.10.1", - "@tauri-apps/cli-darwin-x64": "2.10.1", - "@tauri-apps/cli-linux-arm-gnueabihf": "2.10.1", - "@tauri-apps/cli-linux-arm64-gnu": "2.10.1", - "@tauri-apps/cli-linux-arm64-musl": "2.10.1", - "@tauri-apps/cli-linux-riscv64-gnu": "2.10.1", - "@tauri-apps/cli-linux-x64-gnu": "2.10.1", - "@tauri-apps/cli-linux-x64-musl": "2.10.1", - "@tauri-apps/cli-win32-arm64-msvc": "2.10.1", - "@tauri-apps/cli-win32-ia32-msvc": "2.10.1", - "@tauri-apps/cli-win32-x64-msvc": "2.10.1" + "@tauri-apps/cli-darwin-arm64": "2.11.2", + "@tauri-apps/cli-darwin-x64": "2.11.2", + "@tauri-apps/cli-linux-arm-gnueabihf": "2.11.2", + "@tauri-apps/cli-linux-arm64-gnu": "2.11.2", + "@tauri-apps/cli-linux-arm64-musl": "2.11.2", + "@tauri-apps/cli-linux-riscv64-gnu": "2.11.2", + "@tauri-apps/cli-linux-x64-gnu": "2.11.2", + "@tauri-apps/cli-linux-x64-musl": "2.11.2", + "@tauri-apps/cli-win32-arm64-msvc": "2.11.2", + "@tauri-apps/cli-win32-ia32-msvc": "2.11.2", + "@tauri-apps/cli-win32-x64-msvc": "2.11.2" } }, "node_modules/@tauri-apps/cli-darwin-arm64": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-darwin-arm64/-/cli-darwin-arm64-2.10.1.tgz", - "integrity": "sha512-Z2OjCXiZ+fbYZy7PmP3WRnOpM9+Fy+oonKDEmUE6MwN4IGaYqgceTjwHucc/kEEYZos5GICve35f7ZiizgqEnQ==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-darwin-arm64/-/cli-darwin-arm64-2.11.2.tgz", + "integrity": "sha512-+4UZzLt+eOAEQCwgd+TqKgyUJMrvx+BgdXLLaqJYmPqzP+nE6YZr/hY6CWLYGQb8jFn99jEkmC6uA3tNvamA1w==", "cpu": [ "arm64" ], @@ -1089,9 +1089,9 @@ } }, "node_modules/@tauri-apps/cli-darwin-x64": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-darwin-x64/-/cli-darwin-x64-2.10.1.tgz", - "integrity": "sha512-V/irQVvjPMGOTQqNj55PnQPVuH4VJP8vZCN7ajnj+ZS8Kom1tEM2hR3qbbIRoS3dBKs5mbG8yg1WC+97dq17Pw==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-darwin-x64/-/cli-darwin-x64-2.11.2.tgz", + "integrity": "sha512-VjYYtZUPqDMLutSfJEyxFE3Bz+DPi7c8wC3imckgvciLDZLq4qwKJxBicg0BXGhXjJsl8vKWgWRFNMPELQ+Xyg==", "cpu": [ "x64" ], @@ -1106,9 +1106,9 @@ } }, "node_modules/@tauri-apps/cli-linux-arm-gnueabihf": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-arm-gnueabihf/-/cli-linux-arm-gnueabihf-2.10.1.tgz", - "integrity": "sha512-Hyzwsb4VnCWKGfTw+wSt15Z2pLw2f0JdFBfq2vHBOBhvg7oi6uhKiF87hmbXOBXUZaGkyRDkCHsdzJcIfoJC2w==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-arm-gnueabihf/-/cli-linux-arm-gnueabihf-2.11.2.tgz", + "integrity": "sha512-yMemD6f4i95AQriS8EazyOFzbE34yjnP16i3IOzpHGQvBoy2DjypFMFBq0NtPuITURv/cOGguRtHR5d79/9CSA==", "cpu": [ "arm" ], @@ -1123,9 +1123,9 @@ } }, "node_modules/@tauri-apps/cli-linux-arm64-gnu": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-arm64-gnu/-/cli-linux-arm64-gnu-2.10.1.tgz", - "integrity": "sha512-OyOYs2t5GkBIvyWjA1+h4CZxTcdz1OZPCWAPz5DYEfB0cnWHERTnQ/SLayQzncrT0kwRoSfSz9KxenkyJoTelA==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-arm64-gnu/-/cli-linux-arm64-gnu-2.11.2.tgz", + "integrity": "sha512-cgI91D2wL8GSgoWwZXDqt+DwnuZCP2/bz03QAE4TrhgAKIsrB4hX26W/H1EONPUUNkqrsgeCD0wU6pcNjV/5kw==", "cpu": [ "arm64" ], @@ -1143,9 +1143,9 @@ } }, "node_modules/@tauri-apps/cli-linux-arm64-musl": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-arm64-musl/-/cli-linux-arm64-musl-2.10.1.tgz", - "integrity": "sha512-MIj78PDDGjkg3NqGptDOGgfXks7SYJwhiMh8SBoZS+vfdz7yP5jN18bNaLnDhsVIPARcAhE1TlsZe/8Yxo2zqg==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-arm64-musl/-/cli-linux-arm64-musl-2.11.2.tgz", + "integrity": "sha512-X1rm0BERqAAggtYTESSgXrS3sz4Sb/OiPiz54UqISlXW+GkR3vNIGnsy/lejNmoXGVqri3Q53BCfQiclOIyRPw==", "cpu": [ "arm64" ], @@ -1163,9 +1163,9 @@ } }, "node_modules/@tauri-apps/cli-linux-riscv64-gnu": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-riscv64-gnu/-/cli-linux-riscv64-gnu-2.10.1.tgz", - "integrity": "sha512-X0lvOVUg8PCVaoEtEAnpxmnkwlE1gcMDTqfhbefICKDnOTJ5Est3qL0SrWxizDackIOKBcvtpejrSiVpuJI1kw==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-riscv64-gnu/-/cli-linux-riscv64-gnu-2.11.2.tgz", + "integrity": "sha512-usbMLJbT3KtkOrBMDVeGYNM35aTHXx38SJSzTMSqqjeUIOQ+iVPjb2yAGNAE+KqmBbAx4FOFIyMeKXx2M/JKGQ==", "cpu": [ "riscv64" ], @@ -1183,9 +1183,9 @@ } }, "node_modules/@tauri-apps/cli-linux-x64-gnu": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-x64-gnu/-/cli-linux-x64-gnu-2.10.1.tgz", - "integrity": "sha512-2/12bEzsJS9fAKybxgicCDFxYD1WEI9kO+tlDwX5znWG2GwMBaiWcmhGlZ8fi+DMe9CXlcVarMTYc0L3REIRxw==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-x64-gnu/-/cli-linux-x64-gnu-2.11.2.tgz", + "integrity": "sha512-Ru4gwJKPG0ctVGchRGpRup4Y4lW2SSfFnrbQcyHhCliKy4g8Qz97TrUgCur4CbWyAgKxvGh3SjrkA0LDYzDGiw==", "cpu": [ "x64" ], @@ -1203,9 +1203,9 @@ } }, "node_modules/@tauri-apps/cli-linux-x64-musl": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-x64-musl/-/cli-linux-x64-musl-2.10.1.tgz", - "integrity": "sha512-Y8J0ZzswPz50UcGOFuXGEMrxbjwKSPgXftx5qnkuMs2rmwQB5ssvLb6tn54wDSYxe7S6vlLob9vt0VKuNOaCIQ==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-linux-x64-musl/-/cli-linux-x64-musl-2.11.2.tgz", + "integrity": "sha512-eUm7T6clN1MMmNSRQ9gaWsQdyehQx2Gmn5hht/QUlqZQI/qcP2OJK5dnaxqwFzCr2HdsEo9ydxaqcS1oJzMvUw==", "cpu": [ "x64" ], @@ -1223,9 +1223,9 @@ } }, "node_modules/@tauri-apps/cli-win32-arm64-msvc": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-win32-arm64-msvc/-/cli-win32-arm64-msvc-2.10.1.tgz", - "integrity": "sha512-iSt5B86jHYAPJa/IlYw++SXtFPGnWtFJriHn7X0NFBVunF6zu9+/zOn8OgqIWSl8RgzhLGXQEEtGBdR4wzpVgg==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-win32-arm64-msvc/-/cli-win32-arm64-msvc-2.11.2.tgz", + "integrity": "sha512-HeeZW80jU+gVTOEX4X/hC6NVSAdDVXajwP5fxIZ/3z9WvUC7qrudX2GMTilYq6Dg0e0sk0XgsAJD1hZ5wPBXUA==", "cpu": [ "arm64" ], @@ -1240,9 +1240,9 @@ } }, "node_modules/@tauri-apps/cli-win32-ia32-msvc": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-win32-ia32-msvc/-/cli-win32-ia32-msvc-2.10.1.tgz", - "integrity": "sha512-gXyxgEzsFegmnWywYU5pEBURkcFN/Oo45EAwvZrHMh+zUSEAvO5E8TXsgPADYm31d1u7OQU3O3HsYfVBf2moHw==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-win32-ia32-msvc/-/cli-win32-ia32-msvc-2.11.2.tgz", + "integrity": "sha512-YhjQNZcXfbkCLyazSv1nPnJ9iRFE1wm6kc51FDbU10/Dk09io+6PAGMLjkxnX2GdM0qMnDmTjstY8mTDVvtKeA==", "cpu": [ "ia32" ], @@ -1257,9 +1257,9 @@ } }, "node_modules/@tauri-apps/cli-win32-x64-msvc": { - "version": "2.10.1", - "resolved": "https://registry.npmjs.org/@tauri-apps/cli-win32-x64-msvc/-/cli-win32-x64-msvc-2.10.1.tgz", - "integrity": "sha512-6Cn7YpPFwzChy0ERz6djKEmUehWrYlM+xTaNzGPgZocw3BD7OfwfWHKVWxXzdjEW2KfKkHddfdxK1XXTYqBRLg==", + "version": "2.11.2", + "resolved": "https://registry.npmjs.org/@tauri-apps/cli-win32-x64-msvc/-/cli-win32-x64-msvc-2.11.2.tgz", + "integrity": "sha512-d2JchlFIpZevZVReyqhQOekJmb1UH3rhZ5VX6sH3ty9ETE0TKQavpihvoScUXfKKpW6HZC0MrFGRU0ZtD+w3gA==", "cpu": [ "x64" ], diff --git a/package.json b/package.json index 1344517..35049e0 100644 --- a/package.json +++ b/package.json @@ -15,7 +15,7 @@ }, "license": "MIT", "dependencies": { - "@tauri-apps/api": "^2", + "@tauri-apps/api": "^2.11.0", "@tauri-apps/plugin-opener": "^2", "@tauri-apps/plugin-process": "^2.3.1", "@tauri-apps/plugin-updater": "^2.10.1", @@ -25,7 +25,7 @@ "@sveltejs/adapter-static": "^3.0.6", "@sveltejs/kit": "^2.9.0", "@sveltejs/vite-plugin-svelte": "^5.0.0", - "@tauri-apps/cli": "^2", + "@tauri-apps/cli": "^2.11.2", "@types/three": "^0.183.1", "svelte": "^5.0.0", "svelte-check": "^4.0.0", diff --git a/src-tauri/src/serial_core/model.rs b/src-tauri/src/serial_core/model.rs index 3c07ca2..de2c99c 100644 --- a/src-tauri/src/serial_core/model.rs +++ b/src-tauri/src/serial_core/model.rs @@ -118,6 +118,10 @@ impl HudChartState { push_summary_point(&mut self.summary_points, value); } + pub fn clear_summary(&mut self) { + self.summary_points.clear(); + } + pub fn record_pressure_matrix(&mut self, values: &[i32]) { if values.is_empty() { return; diff --git a/src-tauri/src/serial_core/multi_dim_force.rs b/src-tauri/src/serial_core/multi_dim_force.rs index f9481cf..cb6e17f 100644 --- a/src-tauri/src/serial_core/multi_dim_force.rs +++ b/src-tauri/src/serial_core/multi_dim_force.rs @@ -2,38 +2,21 @@ 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 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; -const EDGE_ASYMMETRY_DAMPING: f32 = 0.35; -const EDGE_INWARD_ROLLING_BIAS: f32 = 0.55; -const EDGE_START_COP_THRESHOLD: f32 = 0.45; -const EDGE_START_BIAS_WEIGHT: f32 = 1.1; -const ROLLING_FRICTION_ALPHA: f32 = 0.68; -const ROLLING_FRICTION_MIN_MAGNITUDE: f32 = 0.05; +const NOISE_COLLECT_MS: f32 = 300.0; +const THRESH_K: f32 = 5.0; +const MIN_THRESHOLD: f32 = 50.0; +const CONTACT_CONFIRM_MS: f32 = 20.0; +const RELEASE_CONFIRM_MS: f32 = 50.0; +const INIT_COLLECT_MS: f32 = 80.0; +const SNAP_CENTER_X: f32 = 3.0; +const SNAP_CENTER_Y: f32 = 5.5; +const SNAP_RANGE_X: f32 = 0.25; +const SNAP_RANGE_Y: f32 = 0.25; +const POST_REFINE_WINDOW_MS: f32 = 800.0; +const POST_STABLE_MS: f32 = 200.0; +const POST_STABLE_THRESH: f32 = 0.1; +const COP_LPF_ALPHA: f32 = 0.25; +const EPSILON: f32 = 1e-8; #[derive(Debug, Clone, Copy)] pub struct PztSpatialAnalysis { @@ -46,514 +29,140 @@ pub struct PztSpatialAnalysis { pub reportable: bool, } -pub struct PztProcessor { - 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, - edge_start_bias_x: f32, - edge_start_bias_y: f32, - smoothed_x: f32, - smoothed_y: f32, - report_active: bool, - report_hold_counter: usize, - held_report: Option, +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +enum CoPState { + NoContact = 0, + InitCollecting = 1, + PostRefining = 2, + Ready = 3, } -#[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, +pub struct PztProcessor { + dynamic_thresh: Option, + noise_samples: Vec, + noise_start_ms: Option, + first_contact_cop_x: Option, + first_contact_cop_y: Option, + state: CoPState, + init_x_buf: Vec, + init_y_buf: Vec, + init_start_ms: Option, + post_start_ms: Option, + post_stable_start_ms: Option, + post_cand_x: Option, + post_cand_y: Option, + post_refined: bool, + contact_candidate_start_ms: Option, + release_candidate_start_ms: Option, + filtered_cop_x: Option, + filtered_cop_y: Option, + synthetic_timestamp_ms: f32, } impl PztProcessor { pub fn new() -> Self { - Self { - 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, - edge_start_bias_x: 0.0, - edge_start_bias_y: 0.0, - smoothed_x: 0.0, - smoothed_y: 0.0, - report_active: false, - report_hold_counter: 0, - held_report: None, - } + let mut processor = Self { + dynamic_thresh: None, + noise_samples: Vec::new(), + noise_start_ms: None, + first_contact_cop_x: None, + first_contact_cop_y: None, + state: CoPState::NoContact, + init_x_buf: Vec::new(), + init_y_buf: Vec::new(), + init_start_ms: None, + post_start_ms: None, + post_stable_start_ms: None, + post_cand_x: None, + post_cand_y: None, + post_refined: false, + contact_candidate_start_ms: None, + release_candidate_start_ms: None, + filtered_cop_x: None, + filtered_cop_y: None, + synthetic_timestamp_ms: 0.0, + }; + processor.reset_all(); + processor } - 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.edge_start_bias_x = 0.0; - self.edge_start_bias_y = 0.0; - self.smoothed_x = 0.0; - self.smoothed_y = 0.0; + fn reset_all(&mut self) { + self.dynamic_thresh = None; + self.noise_samples.clear(); + self.noise_start_ms = None; + self.reset_contact_state(); } - fn reset_report_state(&mut self) { - self.report_active = false; - self.report_hold_counter = 0; - self.held_report = None; + fn reset_contact_state(&mut self) { + self.first_contact_cop_x = None; + self.first_contact_cop_y = None; + self.state = CoPState::NoContact; + self.init_x_buf.clear(); + self.init_y_buf.clear(); + self.init_start_ms = None; + self.post_start_ms = None; + self.post_stable_start_ms = None; + self.post_cand_x = None; + self.post_cand_y = None; + self.post_refined = false; + self.contact_candidate_start_ms = None; + self.release_candidate_start_ms = None; + self.filtered_cop_x = None; + self.filtered_cop_y = 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); + pub fn get_pzt_analysis_at( + &mut self, + adc_data: &[f32], + timestamp_ms: f32, + ) -> Result { + if adc_data.len() != SENSOR_COUNT { + return Err("ADC data length must be 84"); } - let baseline = self - .baseline_frame - .as_ref() - .expect("baseline should exist after bootstrap"); + let frame = Self::prepare_frame(adc_data); + let total_pressure = frame.iter().sum::(); + self.update_dynamic_threshold(total_pressure, timestamp_ms); - raw_frame - .iter() - .zip(baseline.iter()) - .map(|(raw, base)| (raw - base - BASELINE_NOISE_FLOOR).max(0.0)) - .collect() - } - - 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 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; + let raw_contact = self.is_raw_contact(total_pressure); + let contact_valid = self.debounce_contact(raw_contact, timestamp_ms); + if !contact_valid { + self.handle_no_contact(); + return Ok(Self::empty_analysis(false)); } - let peak = frame.iter().copied().fold(0.0, f32::max); - if peak <= 0.0 { - return None; - } + let Some((cop_x, cop_y)) = Self::compute_cop(&frame, total_pressure) else { + return Ok(Self::empty_analysis(false)); + }; + let (cop_x, cop_y) = self.filter_cop(cop_x, cop_y); + self.update_state_machine(cop_x, cop_y, timestamp_ms); - let active_threshold = (peak * ACTIVE_CELL_PEAK_RATIO).max(ACTIVE_CELL_MIN_VALUE); + let (base_x, base_y, dx, dy) = match (self.first_contact_cop_x, self.first_contact_cop_y) { + (Some(base_x), Some(base_y)) => (base_x, base_y, cop_x - base_x, base_y - cop_y), + _ => (cop_x, cop_y, 0.0, 0.0), + }; + let _ = (base_x, base_y); - 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; + let (angle_deg, magnitude) = Self::compute_vector_angle(dx, dy); + let reportable = self.state != CoPState::NoContact && magnitude > 0.0; - 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); - } - } - - if active_cells < MIN_ACTIVE_CELLS || active_total <= 0.0 { - return None; - } - - 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; - - 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, + Ok(PztSpatialAnalysis { + angle_deg, + magnitude, + planar_x: dx, + planar_y: dy, + confidence: (self.state as i32 as f32 / CoPState::Ready as i32 as f32).clamp(0.0, 1.0), + contact_active: self.state != CoPState::NoContact, + reportable, }) } - fn compute_vector_angle(x: f32, y: f32) -> (f32, f32) { - 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, magnitude) - } - - fn contact_touches_edge(stats: &ContactStats) -> bool { - stats.min_row == 0 - || stats.max_row == SENSOR_ROWS - 1 - || stats.min_col == 0 - || stats.max_col == SENSOR_COLS - 1 - } - - fn damp_edge_asymmetry( - stats: &ContactStats, - kinematic_x: f32, - kinematic_y: f32, - ) -> (f32, f32) { - let mut asymmetry_x = stats.asymmetry_x * ASYMMETRY_WEIGHT; - let mut asymmetry_y = stats.asymmetry_y * ASYMMETRY_WEIGHT; - - if stats.min_col == 0 && asymmetry_x < 0.0 { - asymmetry_x = -asymmetry_x * EDGE_INWARD_ROLLING_BIAS; - } - if stats.max_col == SENSOR_COLS - 1 && asymmetry_x > 0.0 { - asymmetry_x = -asymmetry_x * EDGE_INWARD_ROLLING_BIAS; - } - if stats.min_row == 0 && asymmetry_y < 0.0 { - asymmetry_y = -asymmetry_y * EDGE_INWARD_ROLLING_BIAS; - } - if stats.max_row == SENSOR_ROWS - 1 && asymmetry_y > 0.0 { - asymmetry_y = -asymmetry_y * EDGE_INWARD_ROLLING_BIAS; - } - - if Self::contact_touches_edge(stats) { - let opposing_dot = asymmetry_x * kinematic_x + asymmetry_y * kinematic_y; - let kinematic_mag = (kinematic_x * kinematic_x + kinematic_y * kinematic_y).sqrt(); - if opposing_dot < 0.0 && kinematic_mag >= ROLLING_FRICTION_MIN_MAGNITUDE { - asymmetry_x *= EDGE_ASYMMETRY_DAMPING; - asymmetry_y *= EDGE_ASYMMETRY_DAMPING; - } - } - - (asymmetry_x, asymmetry_y) - } - - fn edge_start_bias(stats: &ContactStats) -> (f32, f32) { - let center_x = (SENSOR_COLS - 1) as f32 * 0.5; - let center_y = (SENSOR_ROWS - 1) as f32 * 0.5; - let normalized_x = ((stats.cop_x - center_x) / center_x.max(1.0)).clamp(-1.0, 1.0); - let normalized_y = ((stats.cop_y - center_y) / center_y.max(1.0)).clamp(-1.0, 1.0); - - let mut bias_x = 0.0; - let mut bias_y = 0.0; - - if stats.min_col == 0 || stats.max_col == SENSOR_COLS - 1 { - bias_x = Self::edge_start_axis_bias(normalized_x); - } - if stats.min_row == 0 || stats.max_row == SENSOR_ROWS - 1 { - bias_y = Self::edge_start_axis_bias(normalized_y); - } - - (bias_x, bias_y) - } - - fn edge_start_axis_bias(normalized_axis: f32) -> f32 { - let distance = normalized_axis.abs(); - if distance <= EDGE_START_COP_THRESHOLD { - return 0.0; - } - - let strength = ((distance - EDGE_START_COP_THRESHOLD) / (1.0 - EDGE_START_COP_THRESHOLD)) - .clamp(0.0, 1.0); - -normalized_axis.signum() * strength * EDGE_START_BIAS_WEIGHT - } - - fn apply_rolling_friction( - previous_x: f32, - previous_y: f32, - current_x: f32, - current_y: f32, - ) -> (f32, f32) { - let previous_mag = (previous_x * previous_x + previous_y * previous_y).sqrt(); - let current_mag = (current_x * current_x + current_y * current_y).sqrt(); - - if previous_mag < ROLLING_FRICTION_MIN_MAGNITUDE - || current_mag < ROLLING_FRICTION_MIN_MAGNITUDE - { - return (current_x, current_y); - } - - let dot = previous_x * current_x + previous_y * current_y; - if dot >= 0.0 { - return (current_x, current_y); - } - - let mixed_x = current_x * (1.0 - ROLLING_FRICTION_ALPHA) - + previous_x * ROLLING_FRICTION_ALPHA; - let mixed_y = current_y * (1.0 - ROLLING_FRICTION_ALPHA) - + previous_y * ROLLING_FRICTION_ALPHA; - - if mixed_x * previous_x + mixed_y * previous_y >= 0.0 { - return (mixed_x, mixed_y); - } - - let keep_mag = previous_mag.min(current_mag) * 0.5; - ( - previous_x / previous_mag * keep_mag, - previous_y / previous_mag * keep_mag, - ) - } - - 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 - } - - 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_subtracted = self.subtract_baseline(adc_data); - if !self.update_contact_state(adc_data, &baseline_subtracted) { - return Ok(Self::inactive_analysis()); - } - - 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); - let (edge_start_bias_x, edge_start_bias_y) = Self::edge_start_bias(&stats); - self.edge_start_bias_x = edge_start_bias_x; - self.edge_start_bias_y = edge_start_bias_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 kinematic_x = drift_x * DRIFT_WEIGHT + motion_x * MOTION_WEIGHT; - let kinematic_y = drift_y * DRIFT_WEIGHT + motion_y * MOTION_WEIGHT; - let edge_bias_x = self.edge_start_bias_x; - let edge_bias_y = self.edge_start_bias_y; - let (asymmetry_x, asymmetry_y) = - Self::damp_edge_asymmetry(&stats, kinematic_x + edge_bias_x, kinematic_y + edge_bias_y); - - let combined_x = asymmetry_x + kinematic_x + edge_bias_x; - let combined_y = asymmetry_y + kinematic_y + edge_bias_y; - let (combined_x, combined_y) = Self::apply_rolling_friction( - self.smoothed_x, - self.smoothed_y, - combined_x, - combined_y, - ); - - self.smoothed_x += (combined_x - self.smoothed_x) * VECTOR_SMOOTHING_ALPHA; - self.smoothed_y += (combined_y - self.smoothed_y) * VECTOR_SMOOTHING_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, - })) + self.synthetic_timestamp_ms += 16.667; + self.get_pzt_analysis_at(adc_data, self.synthetic_timestamp_ms) } pub fn get_pzt_angle(&mut self, adc_data: &[f32]) -> Result { @@ -565,15 +174,251 @@ impl PztProcessor { } pub fn reset_baseline(&mut self) { - self.baseline_frame = None; - self.reset_tracking_state(); - self.reset_report_state(); + self.reset_all(); + self.synthetic_timestamp_ms = 0.0; + } + + fn prepare_frame(adc_data: &[f32]) -> [f32; SENSOR_COUNT] { + let mut frame = [0.0; SENSOR_COUNT]; + for (target, value) in frame.iter_mut().zip(adc_data.iter().copied()) { + *target = value.max(0.0); + } + frame + } + + fn update_dynamic_threshold(&mut self, total_pressure: f32, timestamp_ms: f32) { + if self.dynamic_thresh.is_some() { + return; + } + + if self.noise_start_ms.is_none() { + self.noise_start_ms = Some(timestamp_ms); + } + + self.noise_samples.push(total_pressure); + let noise_start = self.noise_start_ms.unwrap_or(timestamp_ms); + if timestamp_ms - noise_start >= NOISE_COLLECT_MS { + let mean = mean(&self.noise_samples); + let std = stddev(&self.noise_samples, mean); + self.dynamic_thresh = Some((mean + THRESH_K * std).max(MIN_THRESHOLD)); + } + } + + fn is_raw_contact(&self, total_pressure: f32) -> bool { + self.dynamic_thresh + .map(|threshold| total_pressure >= threshold) + .unwrap_or(false) + } + + fn debounce_contact(&mut self, raw_contact: bool, timestamp_ms: f32) -> bool { + let currently_in_contact = self.state != CoPState::NoContact; + + if raw_contact { + self.release_candidate_start_ms = None; + + if currently_in_contact { + return true; + } + + let contact_start = *self.contact_candidate_start_ms.get_or_insert(timestamp_ms); + return timestamp_ms - contact_start >= CONTACT_CONFIRM_MS; + } + + self.contact_candidate_start_ms = None; + if !currently_in_contact { + return false; + } + + let release_start = *self.release_candidate_start_ms.get_or_insert(timestamp_ms); + timestamp_ms - release_start < RELEASE_CONFIRM_MS + } + + fn handle_no_contact(&mut self) { + if self.state != CoPState::NoContact { + self.reset_contact_state(); + } + } + + fn compute_cop(frame: &[f32; SENSOR_COUNT], total_pressure: f32) -> Option<(f32, f32)> { + if total_pressure <= 0.0 { + return None; + } + + let mut weighted_col_sum = 0.0; + let mut weighted_row_sum = 0.0; + for row in 0..SENSOR_ROWS { + for col in 0..SENSOR_COLS { + let value = frame[row * SENSOR_COLS + col]; + weighted_col_sum += value * col as f32; + weighted_row_sum += value * row as f32; + } + } + + Some(( + weighted_col_sum / total_pressure, + weighted_row_sum / total_pressure, + )) + } + + fn filter_cop(&mut self, cop_x: f32, cop_y: f32) -> (f32, f32) { + if COP_LPF_ALPHA <= 0.0 { + return (cop_x, cop_y); + } + + match (self.filtered_cop_x, self.filtered_cop_y) { + (Some(prev_x), Some(prev_y)) => { + let next_x = COP_LPF_ALPHA * cop_x + (1.0 - COP_LPF_ALPHA) * prev_x; + let next_y = COP_LPF_ALPHA * cop_y + (1.0 - COP_LPF_ALPHA) * prev_y; + self.filtered_cop_x = Some(next_x); + self.filtered_cop_y = Some(next_y); + (next_x, next_y) + } + _ => { + self.filtered_cop_x = Some(cop_x); + self.filtered_cop_y = Some(cop_y); + (cop_x, cop_y) + } + } + } + + fn update_state_machine(&mut self, cop_x: f32, cop_y: f32, timestamp_ms: f32) { + if self.state == CoPState::NoContact { + self.state = CoPState::InitCollecting; + self.init_start_ms = Some(timestamp_ms); + self.init_x_buf.clear(); + self.init_y_buf.clear(); + } + + if self.state == CoPState::InitCollecting { + self.init_x_buf.push(cop_x); + self.init_y_buf.push(cop_y); + + let init_start = self.init_start_ms.unwrap_or(timestamp_ms); + if timestamp_ms - init_start >= INIT_COLLECT_MS { + let mut base_x = median(&self.init_x_buf); + let mut base_y = median(&self.init_y_buf); + (base_x, base_y) = Self::apply_center_snap(base_x, base_y); + + self.first_contact_cop_x = Some(base_x); + self.first_contact_cop_y = Some(base_y); + self.post_start_ms = Some(timestamp_ms); + self.post_cand_x = None; + self.post_cand_y = None; + self.post_stable_start_ms = None; + self.state = CoPState::PostRefining; + } + return; + } + + if self.state == CoPState::PostRefining { + self.post_refine(cop_x, cop_y, timestamp_ms); + } + } + + fn apply_center_snap(base_x: f32, base_y: f32) -> (f32, f32) { + if (base_x - SNAP_CENTER_X).abs() <= SNAP_RANGE_X + && (base_y - SNAP_CENTER_Y).abs() <= SNAP_RANGE_Y + { + return (SNAP_CENTER_X, SNAP_CENTER_Y); + } + + (base_x, base_y) + } + + fn post_refine(&mut self, cop_x: f32, cop_y: f32, timestamp_ms: f32) { + let post_start = *self.post_start_ms.get_or_insert(timestamp_ms); + if timestamp_ms - post_start >= POST_REFINE_WINDOW_MS { + self.post_refined = true; + self.state = CoPState::Ready; + return; + } + + let (Some(cand_x), Some(cand_y)) = (self.post_cand_x, self.post_cand_y) else { + self.post_cand_x = Some(cop_x); + self.post_cand_y = Some(cop_y); + self.post_stable_start_ms = Some(timestamp_ms); + return; + }; + + let dist = ((cop_x - cand_x).powi(2) + (cop_y - cand_y).powi(2)).sqrt(); + if dist <= POST_STABLE_THRESH { + let stable_start = *self.post_stable_start_ms.get_or_insert(timestamp_ms); + if timestamp_ms - stable_start >= POST_STABLE_MS { + let (refined_x, refined_y) = Self::apply_center_snap(cand_x, cand_y); + self.first_contact_cop_x = Some(refined_x); + self.first_contact_cop_y = Some(refined_y); + self.post_refined = true; + self.state = CoPState::Ready; + } + } else { + self.post_cand_x = Some(cop_x); + self.post_cand_y = Some(cop_y); + self.post_stable_start_ms = Some(timestamp_ms); + } + } + + fn compute_vector_angle(x: f32, y: f32) -> (f32, f32) { + 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) + } + + fn empty_analysis(contact_active: bool) -> PztSpatialAnalysis { + PztSpatialAnalysis { + angle_deg: 0.0, + magnitude: 0.0, + planar_x: 0.0, + planar_y: 0.0, + confidence: 0.0, + contact_active, + reportable: false, + } + } +} + +fn mean(values: &[f32]) -> f32 { + if values.is_empty() { + return 0.0; + } + values.iter().sum::() / values.len() as f32 +} + +fn stddev(values: &[f32], mean: f32) -> f32 { + if values.is_empty() { + return 0.0; + } + let variance = values + .iter() + .map(|value| { + let delta = value - mean; + delta * delta + }) + .sum::() + / values.len() as f32; + variance.sqrt() +} + +fn median(values: &[f32]) -> f32 { + if values.is_empty() { + return 0.0; + } + + let mut sorted = values.to_vec(); + sorted.sort_by(|left, right| left.partial_cmp(right).unwrap_or(std::cmp::Ordering::Equal)); + let mid = sorted.len() / 2; + if sorted.len() % 2 == 0 { + (sorted[mid - 1] + sorted[mid]) * 0.5 + } else { + sorted[mid] } } #[cfg(test)] mod tests { - use super::{ContactStats, PztProcessor, SENSOR_COLS, SENSOR_ROWS}; + use super::{PztProcessor, SENSOR_COLS, SENSOR_ROWS}; fn index(row: usize, col: usize) -> usize { row * SENSOR_COLS + col @@ -587,56 +432,52 @@ mod tests { frame } - fn stats_touching_bottom_edge() -> ContactStats { - ContactStats { - total: 1000.0, - peak: 300.0, - active_total: 900.0, - active_cells: 6, - min_row: SENSOR_ROWS - 2, - max_row: SENSOR_ROWS - 1, - min_col: 2, - max_col: 4, - cop_x: 3.0, - cop_y: 10.5, - asymmetry_x: 0.0, - asymmetry_y: 1.0, - } - } - #[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(); + let analysis = processor.get_pzt_analysis_at(&frame, 0.0).unwrap(); assert!(!analysis.contact_active); assert!(!analysis.reportable); assert_eq!(analysis.magnitude, 0.0); } #[test] - fn right_heavy_contact_reports_rightward_angle_after_confirmation() { + fn rightward_cop_shift_reports_rightward_angle() { let mut processor = PztProcessor::new(); - let baseline = [0.0; SENSOR_ROWS * SENSOR_COLS]; - let contact = make_frame(&[ + let quiet = [0.0; SENSOR_ROWS * SENSOR_COLS]; + for step in 0..20 { + let _ = processor + .get_pzt_analysis_at(&quiet, step as f32 * 20.0) + .unwrap(); + } + + let center_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), + (5, 4, 120.0), + (6, 2, 120.0), + (6, 3, 180.0), + (6, 4, 120.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(); + for step in 20..70 { + let _ = processor + .get_pzt_analysis_at(¢er_contact, step as f32 * 20.0) + .unwrap(); } + let right_contact = make_frame(&[ + (5, 3, 120.0), + (5, 4, 220.0), + (5, 5, 260.0), + (6, 3, 120.0), + (6, 4, 220.0), + (6, 5, 260.0), + ]); + let analysis = processor + .get_pzt_analysis_at(&right_contact, 1_500.0) + .unwrap(); + assert!(analysis.contact_active); assert!(analysis.reportable); assert!(analysis.magnitude > 0.0); @@ -644,53 +485,24 @@ mod tests { } #[test] - fn report_stays_active_through_short_weak_gap() { + fn release_debounce_holds_short_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 quiet = [0.0; SENSOR_ROWS * SENSOR_COLS]; + for step in 0..20 { + let _ = processor + .get_pzt_analysis_at(&quiet, step as f32 * 20.0) + .unwrap(); } - let analysis = processor.get_pzt_analysis(&weak).unwrap(); - assert!(analysis.reportable); - } + let contact = make_frame(&[(5, 3, 300.0), (5, 4, 300.0), (6, 3, 300.0), (6, 4, 300.0)]); + for step in 20..70 { + let _ = processor + .get_pzt_analysis_at(&contact, step as f32 * 20.0) + .unwrap(); + } - #[test] - fn bottom_edge_outward_gradient_is_turned_inward() { - let stats = stats_touching_bottom_edge(); - let (_asymmetry_x, asymmetry_y) = PztProcessor::damp_edge_asymmetry(&stats, 0.0, -0.2); - - assert!(asymmetry_y < 0.0); - assert!(asymmetry_y > -1.1); - } - - #[test] - fn bottom_edge_start_adds_fixed_upward_bias() { - let stats = stats_touching_bottom_edge(); - let (_bias_x, bias_y) = PztProcessor::edge_start_bias(&stats); - - assert!(bias_y < 0.0); - } - - #[test] - fn rolling_friction_resists_one_frame_reversal() { - let (x, y) = PztProcessor::apply_rolling_friction(0.4, 0.0, -0.6, 0.0); - - assert!(x > 0.0); - assert_eq!(y, 0.0); + let weak = make_frame(&[(5, 3, 10.0), (5, 4, 10.0), (6, 3, 10.0), (6, 4, 10.0)]); + let analysis = processor.get_pzt_analysis_at(&weak, 1_410.0).unwrap(); + assert!(analysis.contact_active); } } diff --git a/src-tauri/src/serial_core/serial.rs b/src-tauri/src/serial_core/serial.rs index 24d95bb..d78ab4f 100644 --- a/src-tauri/src/serial_core/serial.rs +++ b/src-tauri/src/serial_core/serial.rs @@ -317,7 +317,7 @@ where #[cfg(feature = "multi-dim")] { let pzt_values = vals.iter().map(|value| *value as f32).collect::>(); - if let Ok(analysis) = pzt_processor.get_pzt_analysis(&pzt_values) { + if let Ok(analysis) = pzt_processor.get_pzt_analysis_at(&pzt_values, frame.dts_ms() as f32) { if PztProcessor::should_report(&analysis) { spatial_force = Some(HudSpatialForce { angle_deg: analysis.angle_deg, @@ -355,11 +355,81 @@ fn build_display_values( 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]) + + match ad_to_x(summary as f64) { + Some(x) => { + if x <= MIN_DISPLAY_FORCE { + let zero_values = vec![0; values.len()]; + chart_state.record_summary(0.0); + chart_state.record_pressure_matrix(&zero_values); + return Some(vec![0]); + } + + chart_state.record_pressure_matrix(values); + chart_state.record_summary(x as f32); + Some(vec![x.round() as i32]) + } + None => { + chart_state.record_pressure_matrix(values); + chart_state.record_summary(MAX_DISPLAY_FORCE as f32); + Some(vec![MAX_DISPLAY_FORCE.round() as i32]) + } + } +} + +const MIN_DISPLAY_FORCE: f64 = 0.1; +const MAX_DISPLAY_FORCE: f64 = 25.6; + +fn ad_to_x(ad: f64) -> Option { + const CUBIC_LIMIT: f64 = 6.57; + const QUADRATIC_A: f64 = -377.8; + const QUADRATIC_B: f64 = 26040.0; + const EPSILON: f64 = 0.000_001; + + let cubic_min = ad_from_cubic_x(0.0); + if ad <= cubic_min { + return Some(0.0); + } + + let cubic_threshold = ad_from_cubic_x(CUBIC_LIMIT); + if ad <= cubic_threshold { + return Some(solve_monotonic_ad(ad, 0.0, CUBIC_LIMIT, ad_from_cubic_x)); + } + + let quadratic_vertex = -QUADRATIC_B / (2.0 * QUADRATIC_A); + let quadratic_max = ad_from_quadratic_x(quadratic_vertex); + if ad - EPSILON > quadratic_max { + return Some(MAX_DISPLAY_FORCE); + } + + Some(solve_monotonic_ad( + ad, + CUBIC_LIMIT, + quadratic_vertex, + ad_from_quadratic_x, + )) +} + +fn solve_monotonic_ad(ad: f64, mut low: f64, mut high: f64, f: fn(f64) -> f64) -> f64 { + for _ in 0..80 { + let mid = (low + high) / 2.0; + if f(mid) < ad { + low = mid; + } else { + high = mid; + } + } + + (low + high) / 2.0 +} + +fn ad_from_cubic_x(x: f64) -> f64 { + -5.732 * x.powi(3) - 131.5 * x.powi(2) + 31980.0 * x + 13490.0 +} + +fn ad_from_quadratic_x(x: f64) -> f64 { + -377.8 * x.powi(2) + 26040.0 * x + 51120.0 } #[cfg(feature = "devkit")] @@ -414,6 +484,7 @@ fn infer_matrix_shape(len: usize) -> (u32, u32) { (best.0 as u32, best.1 as u32) } +#[cfg(feature = "devkit")] fn raw_to_g1(raw: u32) -> f64 { const X: [u32; 12] = [ 0, 84402, 117218, 140176, 159126, 175812, 191484, 208758, 224703, 252448, 302361, 352703, diff --git a/src/lib/components/CenterStage.svelte b/src/lib/components/CenterStage.svelte index 285c49c..e0a102e 100644 --- a/src/lib/components/CenterStage.svelte +++ b/src/lib/components/CenterStage.svelte @@ -61,6 +61,8 @@ export let replayFileName = ""; export let replayFrameInfo = ""; export let sessionStartedAt: number = Date.now(); + export let summaryReleasePending = false; + export let spatialForcePanelVisible = false; let stagePlaneEl: HTMLDivElement | undefined; let panelZoneEl: HTMLDivElement | undefined; @@ -87,7 +89,9 @@ $: replaySide = summarySide === "left" ? "right" : "left"; $: replayToggleButtonText = replayIsPlaying ? replayPauseLabel : replayPlayLabel; $: replayProgressPercent = Math.round(Math.min(1, Math.max(0, replayProgress)) * 100); - $: summaryCurveVisible = summary.points.length > 0 && summary.points.some((value) => Number.isFinite(value) && Math.abs(value) >= 0.0001); + $: summaryCurveVisible = + summary.points.length > 0 && + (summaryReleasePending || summary.points.some((value) => Number.isFinite(value) && Math.abs(value) >= 0.0001)); $: isModelStage = stageViewMode === "model3d"; function toPxNumber(rawValue: string): number { @@ -276,9 +280,7 @@ {/each} - - - {#if spatialForce} + {#if spatialForcePanelVisible}
{/if} diff --git a/src/lib/components/PressureMatrixViewer.svelte b/src/lib/components/PressureMatrixViewer.svelte index ad52a11..173594b 100644 --- a/src/lib/components/PressureMatrixViewer.svelte +++ b/src/lib/components/PressureMatrixViewer.svelte @@ -67,6 +67,7 @@ const CAMERA_TARGET_Z = MATRIX_OFFSET_Z - 0.4; const MATRIX_ROTATION_Y = 0; const rangeStopPositions = [0, 0.25, 0.5, 0.75, 0.875, 1] as const; + const maxDisplayForce = 25.6; const labelVector = new THREE.Vector3(); $: resolvedColorPalette = pressureColorPalettes[colorMapPreset] ?? pressureColorPalettes.emerald; @@ -145,6 +146,10 @@ return "--"; } + if (value >= maxDisplayForce) { + return `${maxDisplayForce.toFixed(1)}+`; + } + return value.toFixed(1); } diff --git a/src/lib/components/SignalChart.svelte b/src/lib/components/SignalChart.svelte index 484090a..13b75b9 100644 --- a/src/lib/components/SignalChart.svelte +++ b/src/lib/components/SignalChart.svelte @@ -11,6 +11,7 @@ const viewportWidth = 100; const viewportHeight = 36; + const maxDisplayForce = 25.6; const toneColorMap: Record = { cyan: "62 232 255", @@ -32,7 +33,15 @@ } function formatMetric(value: number | null): string { - return value === null ? "--" : value.toFixed(1); + if (value === null) { + return "--"; + } + + if (value >= maxDisplayForce) { + return `${maxDisplayForce.toFixed(1)}+`; + } + + return value.toFixed(1); } function resolveBounds(seriesCollection: HudSignalPanel["series"]): { min: number; max: number } { diff --git a/src/lib/components/SpatialForcePanel.svelte b/src/lib/components/SpatialForcePanel.svelte index b98cc8b..51162a8 100644 --- a/src/lib/components/SpatialForcePanel.svelte +++ b/src/lib/components/SpatialForcePanel.svelte @@ -94,37 +94,38 @@ $: hasData = spatialForce !== null && Number.isFinite(spatialForce.angleDeg) && - (!requireMagnitude || Number.isFinite(spatialForce.magnitude)); + (!requireMagnitude || (Number.isFinite(spatialForce.magnitude) && Math.abs(spatialForce.magnitude) >= 0.0001)); $: angleDeg = hasData ? normalizeAngle(spatialForce?.angleDeg ?? 0) : 0; $: updateVisualAngle(angleDeg, hasData); -{#if hasData} -
-
-
-

{panelCode}

-

{resolvedTitle}

+
+
+
+

{panelCode}

+

{resolvedTitle}

+
+ + {#if resolvedBadgeLabel} + + {/if} +
- {#if resolvedBadgeLabel} - - {/if} -
- -
-
-
-
-
-
-
+
+
+
+
+
+
+
+ {#if hasData}
-
- 90 - 0 - 270 - 180 -
+ {/if} +
+ 90 + 0 + 270 + 180
-
-{/if} + + +