From 163e893847c6183ce361093fb87150acc8ba5961 Mon Sep 17 00:00:00 2001 From: Sachymetsu Date: Mon, 26 Jan 2026 09:15:43 +0100 Subject: [PATCH] refactor: Signal detection algorithm tweaks --- Cargo.lock | 20 +++++ embassy-strike-driver/Cargo.toml | 2 + embassy-strike-driver/src/analysis.rs | 19 +++-- embassy-strike-driver/src/lib.rs | 104 +++++++++++++++++++------- 4 files changed, 111 insertions(+), 34 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 311c0e3..ad239a8 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -300,6 +300,8 @@ dependencies = [ "embassy-time", "heapless 0.9.2", "pollster", + "rand", + "wyrand", ] [[package]] @@ -839,6 +841,15 @@ dependencies = [ "proc-macro2", ] +[[package]] +name = "rand" +version = "0.9.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6db2770f06117d490610c7488547d543617b21bfa07796d7a12f6f1bd53850d1" +dependencies = [ + "rand_core 0.9.5", +] + [[package]] name = "rand_core" version = "0.6.4" @@ -1137,6 +1148,15 @@ dependencies = [ "windows-link", ] +[[package]] +name = "wyrand" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "15e0359b0b8d9cdef235a1fd4a8c5d02e4c9204e9fac861c14c229a8e803d1a6" +dependencies = [ + "rand_core 0.9.5", +] + [[package]] name = "zerocopy" version = "0.8.33" diff --git a/embassy-strike-driver/Cargo.toml b/embassy-strike-driver/Cargo.toml index 1794a34..33feaf3 100644 --- a/embassy-strike-driver/Cargo.toml +++ b/embassy-strike-driver/Cargo.toml @@ -25,3 +25,5 @@ heapless = { version = "0.9.2", optional = true } embassy-time = { workspace = true, features = ["mock-driver", "generic-queue-8"] } critical-section = { version = "1.1", features = ["std"] } pollster = { version = "0.4", features = ["macro"] } +rand = { version = "0.9", default-features = false } +wyrand = "0.3" diff --git a/embassy-strike-driver/src/analysis.rs b/embassy-strike-driver/src/analysis.rs index f0aac29..448dfc1 100644 --- a/embassy-strike-driver/src/analysis.rs +++ b/embassy-strike-driver/src/analysis.rs @@ -1,6 +1,6 @@ use crate::{BLOCK_SIZE, traits::BufferMut}; -pub fn analyse_buffer_by_stepped_windows>( +pub fn analyse_buffer_by_stepped_windows>( threshold: u16, buf: &[u16], average: u16, @@ -13,12 +13,21 @@ pub fn analyse_buffer_by_stepped_windows>( let mut len = 0u32; for (i, window) in buf.windows(CHUNK_SIZE).enumerate().step_by(CHUNK_STEP) { - let window_total = window.iter().copied().sum::(); - let window_avg = window_total / CHUNK_SIZE as u16; - let diff = average.saturating_sub(window_avg); + let (window_total, window_diff) = + window + .iter() + .copied() + .fold((0, 0), |(total, diff), sample| { + ( + total + sample, + diff + ((average as i16) - sample as i16).unsigned_abs(), + ) + }); + + let diff = window_diff / CHUNK_SIZE as u16; if diff > threshold { - peaks.push(i); + peaks.push((i, diff)); } else { total += window_total as u32; len += CHUNK_SIZE as u32; diff --git a/embassy-strike-driver/src/lib.rs b/embassy-strike-driver/src/lib.rs index ae2ad9e..0631c49 100644 --- a/embassy-strike-driver/src/lib.rs +++ b/embassy-strike-driver/src/lib.rs @@ -67,7 +67,7 @@ pub enum DetectorUpdate<'a> { Detection { timestamp: i64, samples: &'a [u16], - peaks: &'a [usize], + peaks: &'a [(usize, u16)], }, } @@ -112,14 +112,11 @@ where } pub async fn tune(&mut self, samples: &mut [u16]) { - // info!("Tuning Detector for correct voltage settings"); let mut duty = 0; self.pwm.set_duty(duty); Timer::after_secs(2).await; let mut act_value = self.adc.sample_average(samples).await; - // info!("initial ACT: {}", act_value); - while act_value < 1364 { duty += 2; if duty >= 256 { @@ -127,19 +124,16 @@ where self.pwm.set_duty(duty); Timer::after_secs(2).await; act_value = self.adc.sample_average(samples).await; - // info!("Restarting tuning"); continue; } self.pwm.set_duty(duty); Timer::after_millis(250).await; act_value = self.adc.sample_average(samples).await; - // info!("ACT: {}, Duty: {}", act_value, duty); } self.state.max_duty.set(duty as u8); - duty = (duty / 3) * 2; + duty = (duty / 6) * 5; self.pwm.set_duty(duty); self.state.duty.set(duty as u8); - // info!("Set detection duty to: {}", duty); // Allow voltage level to stabilize after tuning Timer::after_secs(2).await; let avg = self.adc.sample_average(samples).await; @@ -166,7 +160,7 @@ where peaks: &mut B, update: F, ) where - B: BufferMut, + B: BufferMut<(usize, u16)>, F: Fn(DetectorUpdate<'_>), { peaks.clear(); @@ -201,7 +195,7 @@ where peaks: &'d mut B, update: F, ) where - B: BufferMut, + B: BufferMut<(usize, u16)>, F: Fn(DetectorUpdate<'_>), { loop { @@ -256,8 +250,9 @@ where #[cfg(test)] mod tests { - use core::future::poll_fn; + use core::{future::poll_fn, ops::RangeBounds, slice::SliceIndex}; use embassy_time::MockDriver; + use rand::{Rng, distr::uniform::SampleRange}; #[cfg(not(feature = "alloc"))] extern crate alloc; @@ -310,8 +305,8 @@ mod tests { } #[cfg(not(feature = "alloc"))] - impl BufferMut for alloc::vec::Vec { - fn push(&mut self, value: usize) { + impl BufferMut<(usize, u16)> for alloc::vec::Vec<(usize, u16)> { + fn push(&mut self, value: (usize, u16)) { self.push(value); } @@ -327,7 +322,7 @@ mod tests { self.is_empty() } - fn as_slice(&self) -> &[usize] { + fn as_slice(&self) -> &[(usize, u16)] { self } } @@ -351,7 +346,26 @@ mod tests { .await; } - fn generate_signal(samples: &mut [u16]) { + fn generate_noisy_signal< + I: SliceIndex<[u16], Output = [u16]> + RangeBounds, + S: RangeBounds + SampleRange + Clone, + >( + samples: &mut [u16], + amplitude: S, + range: I, + ) { + let mut noise = wyrand::WyRand::new(141); + + let samples = samples + .get_mut(range) + .expect("Range to be sized the same or smaller than the slice"); + + for sample in samples.iter_mut() { + *sample = ((*sample as i16) - noise.random_range(amplitude.clone())) as u16; + } + } + + fn generate_drop_signal(samples: &mut [u16]) { // Example voltage drop signal samples[5] -= 60; samples[6] -= 55; @@ -396,8 +410,8 @@ mod tests { tune_detector_manually(&mut detector, &mut buf, driver).await; assert_eq!(detector.state.max_duty.get(), 98); - assert_eq!(detector.state.duty.get(), 64); - assert_eq!(detector.adc.sample_average(&mut buf).await, 896); + assert_eq!(detector.state.duty.get(), 80); + assert_eq!(detector.adc.sample_average(&mut buf).await, 1120); } #[pollster::test] @@ -466,20 +480,18 @@ mod tests { let mut peaks = alloc::vec::Vec::with_capacity(512); - let update = |_update: DetectorUpdate<'_>| { + detector.detect_from_sample(0, &samples, &mut peaks, |_| { panic!("This update function shouldn't be called"); - }; - - detector.detect_from_sample(0, &samples, &mut peaks, update); + }); assert_eq!(peaks.len(), 0); - generate_signal(&mut samples); + generate_drop_signal(&mut samples); - let expected_peaks = alloc::vec![0, 8]; + let expected_peaks = alloc::vec![(0, 25), (8, 21)]; let called = Cell::new(false); - let update = |update: DetectorUpdate<'_>| { + detector.detect_from_sample(0, &samples, &mut peaks, |update: DetectorUpdate<'_>| { called.set(true); assert_eq!( update, @@ -489,12 +501,46 @@ mod tests { peaks: expected_peaks.as_slice() } ) - }; - - detector.detect_from_sample(0, &samples, &mut peaks, update); + }); assert_eq!(peaks.len(), 2); assert!(called.get()); + + called.set(false); + + let mut samples = alloc::vec![0; BLOCK_SIZE]; + let expected_peaks = alloc::vec![(16, 15), (24, 19), (32, 18)]; + + detector.adc.sample(&mut samples).await; + + // Big noisy signal + generate_noisy_signal(&mut samples, -40..=40, 20..55); + + detector.detect_from_sample(0, &samples, &mut peaks, |update| { + called.set(true); + assert_eq!( + update, + DetectorUpdate::Detection { + timestamp: 0, + samples: &samples, + peaks: expected_peaks.as_slice() + } + ) + }); + + assert_eq!(peaks.len(), 3); + assert!(called.get()); + + detector.adc.sample(&mut samples).await; + + // Smaller noisy signal + generate_noisy_signal(&mut samples, -15..=15, 10..75); + + detector.detect_from_sample(0, &samples, &mut peaks, |_| { + panic!("This update function shouldn't be called"); + }); + + assert_eq!(peaks.len(), 0); } #[pollster::test] @@ -512,13 +558,13 @@ mod tests { detector.state.avg.set(896); let mut samples = alloc::vec![0; BLOCK_SIZE]; - let mut peaks: alloc::vec::Vec = alloc::vec::Vec::with_capacity(BLOCK_SIZE); + let mut peaks = alloc::vec::Vec::with_capacity(BLOCK_SIZE); // Require bigger size blips. detector.config.blip_size.set(3); detector.adc.sample(&mut samples).await; - generate_signal(&mut samples); + generate_drop_signal(&mut samples); detector.detect_from_sample(0, &samples, &mut peaks, |_update| { panic!("Update shouldn't be called"); -- 2.51.2