dc block + lowpass

src/dc_blocker.rs

@@ -0,0 +1,42 @@
+use crate::fm_demod::Sample;
+
+pub struct DcBlocker<I> {
+    pub inner: I,
+    pub x_prev: Sample,
+    pub y_prev: Sample,
+    pub alpha: f32,
+}
+
+impl<I> DcBlocker<I> {
+    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<I, E> Iterator for DcBlocker<I>
+where
+    I: Iterator<Item = Result<Sample, E>>,
+{
+    type Item = Result<Sample, E>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        match self.inner.next()? {
+            Ok(sample) => Some(Ok(self.process_sample(sample))),
+            Err(e) => Some(Err(e)),
+        }
+    }
+}

src/low_pass.rs

@@ -0,0 +1,39 @@
+use crate::fm_demod::Sample;
+
+pub struct LowPass<I> {
+    inner: I,
+    y_prev: Sample,
+    alpha: f32,
+}
+
+impl<I> LowPass<I> {
+    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<I, E> Iterator for LowPass<I>
+where
+    I: Iterator<Item = Result<Sample, E>>,
+{
+    type Item = Result<Sample, E>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        match self.inner.next()? {
+            Ok(sample) => Some(Ok(self.process_sample(sample))),
+            Err(e) => Some(Err(e)),
+        }
+    }
+}

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<dyn Error>> {
     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 {
+    for s_r in pipeline {
-        let phase = phase_r?;
+        let s = s_r?;
-        // println!("phase : {}", phase);
+        println!("phase : {}", s);
     }
 
     Ok(())

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<E>: Iterator<Item = Result<IqSample, E>> + Sized {
     fn agc(self, sample_rate: f32, target_power: f32, min_gain: f32, max_gain: f32) -> Agc<Self> {
@@ -18,3 +20,15 @@ pub trait DspPipelineExt<E>: Iterator<Item = Result<IqSample, E>> + Sized {
 }
 
 impl<I, E> DspPipelineExt<E> for I where I: Iterator<Item = Result<IqSample, E>> {}
+
+pub trait AfterDemodPiplineExt<E>: Iterator<Item = Result<Sample, E>> + Sized {
+    fn dc_block(self, alpha: f32) -> DcBlocker<Self> {
+        DcBlocker::new(self, alpha)
+    }
+
+    fn lowpass(self, alpha: f32) -> LowPass<Self> {
+        LowPass::new(self, alpha)
+    }
+}
+
+impl<I, E> AfterDemodPiplineExt<E> for I where I: Iterator<Item = Result<Sample, E>> {}