arduino-tnc

demodulator.rs (16860B)

---

 //! Bell 202 demodulator — produces byte-aligned NRZI-decoded output from raw AFSK samples
 //!
 //! Uses two [`GoertzelFilter`] resonators (mark @ 1200 Hz, space @ 2200 Hz), updates them
 //! per-sample via [`Bell202Demodulator::sample_update`], then calls
 //! [`Bell202Demodulator::symbol_update`] once per symbol period to get NRZI-decoded,
 //! HDLC-flag-aware byte output.
 //!
 //! All frequency/timing parameters are defined in [`super::constants`].
 
 use crate::goertzel::GoertzelFilter;
 
 #[derive(PartialEq, Clone, Copy, Debug)]
 pub enum Symbol {
     Mark,
     Space,
 }
 
 pub struct Bell202Demodulator {
     mark: GoertzelFilter,
     space: GoertzelFilter,
     symbol_estimate: Symbol,
     previous_symbol_estimate: Symbol,
     symbol_phase: u8,
     byte: u8,
     byte_phase: u8,
     output: Option<u8>,
 }
 
 impl Bell202Demodulator {
     pub const fn new() -> Self {
         Self {
             mark: GoertzelFilter::mark(),
             space: GoertzelFilter::space(),
             symbol_estimate: Symbol::Space,
             previous_symbol_estimate: Symbol::Space,
             symbol_phase: 0,
             byte: 0,
             byte_phase: 0,
             output: None,
         }
     }
 
     pub fn sample_update(&mut self, sample: f32) {
         self.mark.update(sample);
         self.space.update(sample);
     }
 
     pub fn symbol_update(&mut self) -> Option<u8> {
         let mark_energy = self.mark.energy();
         let space_energy = self.space.energy();
         self.previous_symbol_estimate = self.symbol_estimate;
         self.symbol_estimate = if mark_energy > space_energy {
             Symbol::Mark
         } else {
             Symbol::Space
         };
 
         self.byte <<= 1;
         if self.symbol_estimate == self.previous_symbol_estimate {
             self.byte += 1;
         }
 
         self.byte_phase = (self.byte_phase + 1) % 8;
         if self.byte == 0x7e {
             self.byte_phase = 0;
         }
 
         if self.byte_phase == 0 {
             self.output = Some(self.byte);
         } else {
             self.output = None;
         }
 
         self.mark.reset();
         self.space.reset();
 
         self.output
     }
 
     pub fn reset(&mut self) {
         self.mark.reset();
         self.space.reset();
         self.symbol_estimate = Symbol::Space;
         self.previous_symbol_estimate = Symbol::Space;
         self.symbol_phase = 0;
         self.byte = 0;
         self.byte_phase = 0;
         self.output = None;
     }
 
     pub fn get_output(&self) -> Option<u8> {
         self.output
     }
 
     #[cfg(test)]
     pub fn byte_phase(&self) -> u8 {
         self.byte_phase
     }
 }
 
 #[cfg(test)]
 mod tests {
     use alloc::vec;
     use alloc::vec::Vec;
     use core::iter;
 
     use rand::rngs::StdRng;
     use rand::{Rng, SeedableRng};
     use rand_distr::{Distribution, Normal};
 
     use super::*;
     use crate::constants::SAMPLES_PER_SYMBOL;
     use crate::test_utils::generate_audio_phase_continuous;
 
     /// Add Gaussian noise at the given SNR
     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()
     }
 
     /// Generate random symbols with the given seed
     fn generate_random_symbols(seed: u64, n: usize) -> Vec<Symbol> {
         let mut rng = StdRng::seed_from_u64(seed);
         (0..n)
             .map(|_| {
                 if rng.gen::<u8>() % 2 == 0 {
                     Symbol::Mark
                 } else {
                     Symbol::Space
                 }
             })
             .collect()
     }
 
     #[test]
     fn mark_only_tone_yields_mark_decisions() {
         let mut demod = Bell202Demodulator::new();
         let symbols = vec![Symbol::Mark; 64];
         let audio = generate_audio_phase_continuous(&symbols);
         let sps = SAMPLES_PER_SYMBOL as usize;
 
         let mut mark_count = 0;
         let mut total_decisions = 0;
 
         for (i, &sample) in audio.iter().enumerate() {
             demod.sample_update(sample);
             if i > 0 && i % sps == 0 {
                 demod.symbol_update();
                 if demod.symbol_estimate == Symbol::Mark {
                     mark_count += 1;
                 }
                 total_decisions += 1;
             }
         }
 
         assert_eq!(
             mark_count, total_decisions,
             "All {} symbol decisions should be Mark, but only {} were",
             total_decisions, mark_count
         );
     }
 
     #[test]
     fn space_only_tone_yields_space_decisions() {
         let mut demod = Bell202Demodulator::new();
         let symbols = vec![Symbol::Space; 64];
         let audio = generate_audio_phase_continuous(&symbols);
         let sps = SAMPLES_PER_SYMBOL as usize;
 
         let mut space_count = 0;
         let mut total_decisions = 0;
 
         for (i, &sample) in audio.iter().enumerate() {
             demod.sample_update(sample);
             if i > 0 && i % sps == 0 {
                 demod.symbol_update();
                 if demod.symbol_estimate == Symbol::Space {
                     space_count += 1;
                 }
                 total_decisions += 1;
             }
         }
 
         assert_eq!(
             space_count, total_decisions,
             "All {} symbol decisions should be Space, but only {} were",
             total_decisions, space_count
         );
     }
 
     #[test]
     fn phase_shift_resets_on_tone_transition() {
         let mut demod = Bell202Demodulator::new();
         let sps = SAMPLES_PER_SYMBOL as usize;
 
         let mut all_symbols = vec![Symbol::Space; 10];
         all_symbols.extend(iter::repeat(Symbol::Mark).take(10));
         let audio = generate_audio_phase_continuous(&all_symbols);
 
         let mut prev_est = Symbol::Space;
         let mut transitions: usize = 0;
 
         for (i, &sample) in audio.iter().enumerate() {
             demod.sample_update(sample);
             if i > 0 && i % sps == 0 {
                 demod.symbol_update();
                 let current_est = demod.symbol_estimate;
                 if current_est != prev_est {
                     transitions += 1;
                 }
                 prev_est = current_est;
             }
         }
 
         assert!(
             transitions == 1,
             "Expected exactly one tone transition (Space→Mark), got {}",
             transitions
         );
     }
 
     #[test]
     fn byte_phase_advances_correctly() {
         // Verify that the demodulator's byte_phase advances correctly through byte boundaries, and
         // that flag bytes reset it while non-flag bytes complete two full cycles.
         //
         // Helper: run sample_update for audio, then call symbol_update for any remaining symbol
         // that hasn't been processed yet (the loop's modulo check misses the last symbol
         // boundary).
         macro_rules! feed {
             ($demod:expr, $audio:expr, $sps:expr) => {
                 for (i, &sample) in $audio.iter().enumerate() {
                     $demod.sample_update(sample);
                     if i > 0 && i % $sps == 0 {
                         $demod.symbol_update();
                     }
                 }
                 // Flush last symbol
                 $demod.symbol_update();
             };
         }
 
         let mut demod = Bell202Demodulator::new();
         let sps = SAMPLES_PER_SYMBOL as usize;
 
         // --- Phase 1: 16 Space symbols (0xff 0xff). byte_phase wraps 0→7 twice without resetting
         // (0xff ≠ 0x7e).
         let symbols = vec![Symbol::Space; 16];
         let audio = generate_audio_phase_continuous(&symbols);
         feed!(demod, audio, sps);
         // get_output() returns the last completed byte (0xff), not None.
         assert_eq!(
             demod.byte_phase(),
             0,
             "byte_phase should wrap to 0 after 2 bytes"
         );
 
         // --- Phase 2: 8 Space symbols. byte_phase wraps 0→0 (full byte, 0xff again).
         let symbols = vec![Symbol::Space; 8];
         let audio = generate_audio_phase_continuous(&symbols);
         feed!(demod, audio, sps);
         assert_eq!(demod.byte_phase(), 0, "byte_phase wraps to 0 after 1 byte");
 
         // --- Phase 3: 4 Space symbols. byte_phase advances 0→4.
         let symbols = vec![Symbol::Space; 4];
         let audio = generate_audio_phase_continuous(&symbols);
         feed!(demod, audio, sps);
         assert_eq!(
             demod.byte_phase(),
             4,
             "byte_phase should advance to 4 (0.5 byte)"
         );
 
         // --- Phase 4: 4 more Space symbols completes a byte. byte_phase wraps 4→0. get_output()
         // returns 0xff again.
         let symbols = vec![Symbol::Space; 4];
         let audio = generate_audio_phase_continuous(&symbols);
         feed!(demod, audio, sps);
         assert_eq!(
             demod.byte_phase(),
             0,
             "byte_phase wraps to 0 after completing byte"
         );
     }
 
     #[test]
     fn flag_byte_detection() {
         // Verify flag byte 0x7e is detected correctly with block-based Goertzel (filters reset at
         // symbol boundaries)
         //
         // 0x7e = 01111110 → NRZI: flip(S→M), keep(M)×6, flip(M→S) → 7 Marks + 1 Space
         let mut demod = Bell202Demodulator::new();
         let nrzi = vec![
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Space,
         ];
         let audio = generate_audio_phase_continuous(&nrzi);
         let sps = SAMPLES_PER_SYMBOL as usize;
 
         for (i, &sample) in audio.iter().enumerate() {
             demod.sample_update(sample);
             if i > 0 && i % sps == 0 {
                 demod.symbol_update();
             }
         }
         demod.symbol_update();
 
         assert_eq!(
             demod.get_output(),
             Some(0x7e),
             "Flag byte 0x7e should be detected correctly with block-based \
              Goertzel (filters reset at symbol boundaries)"
         );
     }
 
     #[test]
     fn consecutive_flag_bytes() {
         // Verify two consecutive 0x7e flag bytes are both detected
         //
         // NRZI sequence: M×7, S, M×7, S (16 symbols); The demodulator's NRZI state carries over;
         // after the first 0x7e, prev=Space. The second flag starts with S→M (flip, data bit=0).
         let mut demod = Bell202Demodulator::new();
         let nrzi = vec![
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Space,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Space,
         ];
         let audio = generate_audio_phase_continuous(&nrzi);
         let sps = SAMPLES_PER_SYMBOL as usize;
 
         let mut outputs = Vec::new();
         for (i, &sample) in audio.iter().enumerate() {
             demod.sample_update(sample);
             if i > 0 && i % sps == 0 {
                 demod.symbol_update();
                 if let Some(byte) = demod.get_output() {
                     outputs.push(byte);
                 }
             }
         }
         demod.symbol_update();
         if let Some(byte) = demod.get_output() {
             outputs.push(byte);
         }
 
         assert_eq!(
             outputs,
             vec![0x7e, 0x7e],
             "Both consecutive 0x7e flag bytes should be detected. Got {:?}",
             outputs
         );
     }
 
     #[test]
     fn mixed_bytes() {
         // Verify demodulator handles a realistic mix of flag and data bytes.
         //
         // Sequence (NRZI, state carries over between bytes):
         //   0x7e: M×7, S (flag)
         //   0xff: S×8 (all 1s → same NRZI state)
         //   0x7e: S→M, M×6, M→S (flag)
         //   0x00: M,S,M,S,M,S,M,S (alternating, from S state)
         let mut demod = Bell202Demodulator::new();
         let nrzi = vec![
             // 0x7e flag: M×7, S
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Space,
             // 0xff: all S (same state) → S×8
             Symbol::Space,
             Symbol::Space,
             Symbol::Space,
             Symbol::Space,
             Symbol::Space,
             Symbol::Space,
             Symbol::Space,
             Symbol::Space,
             // 0x7e flag: S→M, M×6, M→S
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Mark,
             Symbol::Space,
             // 0x00: all 0s → flip every bit from S state → M,S,M,S,M,S,M,S
             Symbol::Mark,
             Symbol::Space,
             Symbol::Mark,
             Symbol::Space,
             Symbol::Mark,
             Symbol::Space,
             Symbol::Mark,
             Symbol::Space,
         ];
         let audio = generate_audio_phase_continuous(&nrzi);
         let sps = SAMPLES_PER_SYMBOL as usize;
 
         let mut outputs = Vec::new();
         for (i, &sample) in audio.iter().enumerate() {
             demod.sample_update(sample);
             if i > 0 && i % sps == 0 {
                 demod.symbol_update();
                 if let Some(byte) = demod.get_output() {
                     outputs.push(byte);
                 }
             }
         }
         demod.symbol_update();
         if let Some(byte) = demod.get_output() {
             outputs.push(byte);
         }
 
         assert_eq!(
             outputs,
             vec![0x7e, 0xff, 0x7e, 0x00],
             "All four bytes should decode correctly with block-based Goertzel. \n\
              Got {:?}",
             outputs
         );
     }
 
     // ── Noisy tests ────────────────────────────────────────────────────
 
     #[test]
     fn alternating_symbols_15_db_snr() {
         let symbols: Vec<Symbol> = (0..64)
             .map(|i| {
                 if i % 2 == 0 {
                     Symbol::Mark
                 } else {
                     Symbol::Space
                 }
             })
             .collect();
         let audio = generate_audio_phase_continuous(&symbols);
         let noisy = add_noise(&audio, 15.0, 42);
         let sps = SAMPLES_PER_SYMBOL as usize;
         let mut demod = Bell202Demodulator::new();
 
         let mut correct = 0usize;
         for (i, &sample) in noisy.iter().enumerate() {
             demod.sample_update(sample);
             if i > 0 && i % sps == 0 {
                 demod.symbol_update();
                 let expected = symbols[i / sps - 1] == Symbol::Mark;
                 let actual = demod.symbol_estimate == Symbol::Mark;
                 if actual == expected {
                     correct += 1;
                 }
             }
         }
 
         let total = symbols.len();
         let accuracy = correct as f64 / total as f64;
         assert!(
             accuracy >= 0.95,
             "Alternating symbols @ 15 dB SNR: {:.1}% ({} / {}); \
              Block-based Goertzel should handle 50% transition rate correctly",
             accuracy * 100.0,
             correct,
             total
         );
     }
 
     #[test]
     fn random_symbols_15_db_snr() {
         let symbols = generate_random_symbols(42, 200);
         let audio = generate_audio_phase_continuous(&symbols);
         let noisy = add_noise(&audio, 15.0, 42);
         let sps = SAMPLES_PER_SYMBOL as usize;
         let mut demod = Bell202Demodulator::new();
 
         let mut correct = 0usize;
         for (i, &sample) in noisy.iter().enumerate() {
             demod.sample_update(sample);
             if i > 0 && i % sps == 0 {
                 demod.symbol_update();
                 let expected = symbols[i / sps - 1] == Symbol::Mark;
                 let actual = demod.symbol_estimate == Symbol::Mark;
                 if actual == expected {
                     correct += 1;
                 }
             }
         }
 
         let total = symbols.len();
         let accuracy = correct as f64 / total as f64;
         assert!(
             accuracy >= 0.95,
             "Random symbols @ 15 dB SNR: {:.1}% ({} / {}); \
              Block-based Goertzel should exceed 95% at 15 dB SNR",
             accuracy * 100.0,
             correct,
             total
         );
     }
 }