arduino-tnc

goertzel.rs (6216B)

---

 //! Goertzel resonator for AFSK tone detection
 
 #[derive(Clone, Copy, Debug)]
 pub struct GoertzelFilter {
     coeff: f32,
     s0: f32,
     s1: f32,
     s2: f32,
     energy: f32,
 }
 
 impl GoertzelFilter {
     /// Create a new Goertzel filter with a pre-computed coefficient
     ///
     /// The coefficient should be `2cos(2πf / sample_rate)`. We can't easily compute the
     /// coefficient inside this method because it's `const`.
     pub const fn new_with_coeff(coeff: f32) -> Self {
         Self {
             coeff,
             s0: 0.0,
             s1: 0.0,
             s2: 0.0,
             energy: 0.0,
         }
     }
 
     const MARK_COEFF: f32 = 1.961_69;
     const SPACE_COEFF: f32 = 1.872_207_2;
 
     pub const fn mark() -> Self {
         Self::new_with_coeff(Self::MARK_COEFF)
     }
 
     pub const fn space() -> Self {
         Self::new_with_coeff(Self::SPACE_COEFF)
     }
 
     pub fn update(&mut self, x: f32) {
         self.s0 = x + self.coeff * self.s1 - self.s2;
         self.s2 = self.s1;
         self.s1 = self.s0;
         self.energy = self.s1 * self.s1 + self.s2 * self.s2 - self.coeff * self.s1 * self.s2;
     }
 
     pub fn energy(&self) -> f32 {
         self.energy
     }
 
     #[cfg(test)]
     pub fn coeff(&self) -> f32 {
         self.coeff
     }
 
     #[cfg(test)]
     pub fn s0(&self) -> f32 {
         self.s0
     }
 
     #[cfg(test)]
     pub fn s1(&self) -> f32 {
         self.s1
     }
 
     #[cfg(test)]
     pub fn s2(&self) -> f32 {
         self.s2
     }
 
     pub fn reset(&mut self) {
         self.s0 = 0.0;
         self.s1 = 0.0;
         self.s2 = 0.0;
         self.energy = 0.0;
     }
 }
 
 #[cfg(test)]
 mod tests {
     use alloc::vec::Vec;
     use core::f64::consts::TAU;
 
     use rand::rngs::StdRng;
     use rand::SeedableRng;
     use rand_distr::{Distribution, Normal};
 
     use super::*;
     use crate::constants::{MARK_HZ, SAMPLE_RATE_HZ, SPACE_HZ};
 
     /// Generate a phase-continuous tone at the given frequency
     fn generate_tone(freq: f32, samples: usize) -> Vec<f32> {
         let sr = SAMPLE_RATE_HZ as f64;
         let freq = freq as f64;
         let dt = 1.0 / sr;
         (0..samples)
             .map(|i| (TAU * freq * dt * i as f64).sin())
             .map(|v| v as f32)
             .collect()
     }
 
     /// Add Gaussian noise at the given SNR, assuming a full-scale signal
     fn add_noise(samples: &[f32], snr_db: f32, seed: u64) -> Vec<f32> {
         let signal_power = 0.5;
         let noise_std = f32::sqrt(signal_power / 10f32.powf(snr_db / 10.0));
         let normal = Normal::new(0.0, noise_std).unwrap();
         let mut rng = StdRng::seed_from_u64(seed);
         samples
             .iter()
             .zip(normal.sample_iter(&mut rng))
             .map(|(s, n)| s + n)
             .collect()
     }
 
     #[test]
     fn filter_reset_clears_states() {
         let mut filter = GoertzelFilter::mark();
 
         // Feed some samples so energy builds up
         for i in 0..32 {
             let sample = (TAU * 1200.0 * i as f64 / 38460.0).sin() as f32;
             filter.update(sample);
         }
 
         let energy_before = filter.energy();
         assert!(
             energy_before > 0.0,
             "Filter should have non-zero energy after feeding samples"
         );
 
         filter.reset();
 
         assert_eq!(filter.s0(), 0.0, "s0 should be reset to 0.0");
         assert_eq!(filter.s1(), 0.0, "s1 should be reset to 0.0");
         assert_eq!(filter.s2(), 0.0, "s2 should be reset to 0.0");
         assert_eq!(filter.energy(), 0.0, "energy should be reset to 0.0");
     }
 
     #[test]
     fn mark_tone_gives_higher_mark_energy() {
         let mut mark = GoertzelFilter::mark();
         let mut space = GoertzelFilter::space();
 
         let samples = generate_tone(MARK_HZ, 32);
         for &sample in &samples {
             mark.update(sample);
             space.update(sample);
         }
 
         assert!(
             mark.energy() > space.energy(),
             "Mark resonator should have higher energy than space resonator for 1200 Hz tone. \n\
              mark_energy={}, space_energy={}",
             mark.energy(),
             space.energy()
         );
     }
 
     #[test]
     fn space_tone_gives_higher_space_energy() {
         let mut mark = GoertzelFilter::mark();
         let mut space = GoertzelFilter::space();
 
         let samples = generate_tone(SPACE_HZ, 32);
         for &sample in &samples {
             mark.update(sample);
             space.update(sample);
         }
 
         assert!(
             space.energy() > mark.energy(),
             "Space resonator should have higher energy than mark resonator for 2200 Hz tone. \n\
              mark_energy={}, space_energy={}",
             mark.energy(),
             space.energy()
         );
     }
 
     #[test]
     fn mark_tone_noisy_gives_higher_mark_energy() {
         // Verify the filter discriminates mark tone even with noise.
         let snr_db = 15.0;
         let mut mark = GoertzelFilter::mark();
         let mut space = GoertzelFilter::space();
 
         let samples = generate_tone(MARK_HZ, 32);
         let noisy = add_noise(&samples, snr_db, 42);
         for &sample in &noisy {
             mark.update(sample);
             space.update(sample);
         }
 
         assert!(
             mark.energy() > space.energy(),
             "Mark resonator should still exceed space at {} dB SNR. \n\
              mark_energy={}, space_energy={}",
             snr_db,
             mark.energy(),
             space.energy()
         );
     }
 
     #[test]
     fn space_tone_noisy_gives_higher_space_energy() {
         // Verify the filter discriminates space tone even with noise.
         let snr_db = 15.0;
         let mut mark = GoertzelFilter::mark();
         let mut space = GoertzelFilter::space();
 
         let samples = generate_tone(SPACE_HZ, 32);
         let noisy = add_noise(&samples, snr_db, 42);
         for &sample in &noisy {
             mark.update(sample);
             space.update(sample);
         }
 
         assert!(
             space.energy() > mark.energy(),
             "Space resonator should still exceed mark at {} dB SNR. \n\
              mark_energy={}, space_energy={}",
             snr_db,
             mark.energy(),
             space.energy()
         );
     }
 }