diff --git a/src/dc_blocker.rs b/src/dc_blocker.rs new file mode 100644 index 0000000..8d5dac5 --- /dev/null +++ b/src/dc_blocker.rs @@ -0,0 +1,42 @@ +use crate::fm_demod::Sample; + +pub struct DcBlocker { + pub inner: I, + pub x_prev: Sample, + pub y_prev: Sample, + pub alpha: f32, +} + +impl DcBlocker { + pub fn new(inner: I, alpha: f32) -> Self { + Self { + inner, + x_prev: 0.0, + y_prev: 0.0, + alpha, + } + } + + pub fn process_sample(&mut self, x_n: Sample) -> Sample { + let y_n = x_n - self.x_prev + self.alpha * self.y_prev; + + self.x_prev = x_n; + self.y_prev = y_n; + + y_n + } +} + +impl Iterator for DcBlocker +where + I: Iterator>, +{ + type Item = Result; + + fn next(&mut self) -> Option { + match self.inner.next()? { + Ok(sample) => Some(Ok(self.process_sample(sample))), + Err(e) => Some(Err(e)), + } + } +} diff --git a/src/low_pass.rs b/src/low_pass.rs new file mode 100644 index 0000000..3bca137 --- /dev/null +++ b/src/low_pass.rs @@ -0,0 +1,39 @@ +use crate::fm_demod::Sample; + +pub struct LowPass { + inner: I, + y_prev: Sample, + alpha: f32, +} + +impl LowPass { + pub fn new(inner: I, alpha: f32) -> Self { + Self { + inner, + y_prev: 0.0, + alpha, + } + } + + pub fn process_sample(&mut self, x_n: Sample) -> Sample { + let y_n = self.alpha * x_n + (1.0 - self.alpha) * self.y_prev; + + self.y_prev = y_n; + + y_n + } +} + +impl Iterator for LowPass +where + I: Iterator>, +{ + type Item = Result; + + fn next(&mut self) -> Option { + match self.inner.next()? { + Ok(sample) => Some(Ok(self.process_sample(sample))), + Err(e) => Some(Err(e)), + } + } +} diff --git a/src/main.rs b/src/main.rs index 01b140f..2ce43c7 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,11 +1,13 @@ use crate::iq_reader::FileSource; -use crate::pipeline::DspPipelineExt; +use crate::pipeline::{AfterDemodPiplineExt, DspPipelineExt}; use std::error::Error; mod agc; +mod dc_blocker; mod fir; mod fm_demod; mod iq_reader; +mod low_pass; mod pipeline; mod utils; @@ -18,11 +20,13 @@ fn main() -> Result<(), Box> { let pipeline = source .agc(20_000_000.0, 1.0, 0.001, 100.0) .fir::<64>(taps, 4) - .demodulate(); + .demodulate() + .dc_block(0.995) + .lowpass(0.75); - for phase_r in pipeline { - let phase = phase_r?; - // println!("phase : {}", phase); + for s_r in pipeline { + let s = s_r?; + println!("phase : {}", s); } Ok(()) diff --git a/src/pipeline.rs b/src/pipeline.rs index 2c91cbc..1a94b98 100644 --- a/src/pipeline.rs +++ b/src/pipeline.rs @@ -1,7 +1,9 @@ use crate::agc::Agc; +use crate::dc_blocker::DcBlocker; use crate::fir::Fir; -use crate::fm_demod::FmDemod; +use crate::fm_demod::{FmDemod, Sample}; use crate::iq_reader::IqSample; +use crate::low_pass::LowPass; pub trait DspPipelineExt: Iterator> + Sized { fn agc(self, sample_rate: f32, target_power: f32, min_gain: f32, max_gain: f32) -> Agc { @@ -18,3 +20,15 @@ pub trait DspPipelineExt: Iterator> + Sized { } impl DspPipelineExt for I where I: Iterator> {} + +pub trait AfterDemodPiplineExt: Iterator> + Sized { + fn dc_block(self, alpha: f32) -> DcBlocker { + DcBlocker::new(self, alpha) + } + + fn lowpass(self, alpha: f32) -> LowPass { + LowPass::new(self, alpha) + } +} + +impl AfterDemodPiplineExt for I where I: Iterator> {}