Starting fsk demod

oxydsp-dsp/src/blocks.rs

@@ -1,3 +1,5 @@
+pub mod filtering;
 pub mod math;
 pub mod synthesis;
+pub mod ted;
 pub mod utilities;

oxydsp-dsp/src/blocks/filtering.rs

@@ -0,0 +1 @@
+pub mod fir;

oxydsp-dsp/src/blocks/filtering/fir.rs

@@ -0,0 +1,58 @@
+use oxydsp_flowgraph::BlockIO;
+use oxydsp_flowgraph::block::SyncBlock;
+use oxydsp_flowgraph::io::In;
+use oxydsp_flowgraph::io::Out;
+use oxydsp_flowgraph::sync_block;
+use std::iter::Sum;
+use std::ops::Mul;
+
+use crate::filtering::fir::Fir;
+
+#[derive(BlockIO)]
+#[sync_block]
+pub struct FirFilter<F, T, O>
+where
+    T: Clone + 'static,
+    F: Mul<T, Output = O> + Clone + 'static,
+    O: Sum + 'static,
+{
+    #[input]
+    input: In<T>,
+
+    #[output]
+    output: Out<O>,
+
+    filter: crate::filtering::fir::FirFilter<F, T, O>,
+}
+
+impl<F, T, O> FirFilter<F, T, O>
+where
+    T: Clone + 'static,
+    F: Mul<T, Output = O> + Clone + 'static,
+    O: Sum + 'static,
+{
+    pub fn new(input: In<T>, impulse_response: Fir<F>) -> (Self, In<O>)
+    {
+        let (output, filtered) = oxydsp_flowgraph::io::stream();
+        (
+            Self {
+                input,
+                output,
+                filter: crate::filtering::fir::FirFilter::new(impulse_response),
+            },
+            filtered,
+        )
+    }
+}
+
+impl<'view, F, T, O> SyncBlock<'view> for FirFilter<F, T, O>
+where
+    T: Clone + 'view,
+    F: Mul<T, Output = O> + Clone + 'static,
+    O: Sum + 'static,
+{
+    fn sync_work(state: Self::StateView, input: Self::Input) -> Option<Self::Output>
+    {
+        Some(state.filter.next(input))
+    }
+}

oxydsp-dsp/src/blocks/math/basic.rs

@@ -12,7 +12,7 @@ use oxydsp_flowgraph::sync_block;
 use oxydsp_flowgraph::tag::TagMergable;
 
 #[derive(BlockIO)]
-#[sync_block(tagged)]
+#[sync_block]
 pub struct Adder<Ia, Ib, O>
 where
     Ia: Add<Ib, Output = O> + 'static,

oxydsp-dsp/src/blocks/synthesis.rs

@@ -31,7 +31,8 @@ impl<T: Float + From<f32> + 'static> Block for OscillatorSource<T>
 {
     fn work(&mut self) -> oxydsp_flowgraph::block::BlockResult
     {
-        self.output.push_iter((&mut self.nco).map(|x| (x, None)));
+        self.output
+            .push_iter((&mut self.nco).map(|x| (x, None).into()));
         BlockResult::Ok
     }
 }
@@ -82,7 +83,7 @@ impl<T: Float + From<f32> + 'static> Block for Nco<T>
         self.output
             .push_iter(&mut self.frequency.pop_iter().map(|f| {
                 self.nco.set_frequency(f.0);
-                (self.nco.next().unwrap(), f.1)
+                (self.nco.next().unwrap(), f.1).into()
             }));
         BlockResult::Ok
     }

oxydsp-dsp/src/blocks/ted.rs

@@ -0,0 +1 @@
+pub mod early_late;

oxydsp-dsp/src/blocks/ted/early_late.rs

@@ -0,0 +1,111 @@
+use std::collections::VecDeque;
+use std::iter::Sum;
+
+use num::Float;
+use oxydsp_flowgraph::BlockIO;
+use oxydsp_flowgraph::block::SyncBlock;
+use oxydsp_flowgraph::io::In;
+use oxydsp_flowgraph::io::Out;
+use oxydsp_flowgraph::sync_block;
+use oxydsp_flowgraph::tag::Tag;
+
+use crate::filtering::fir::Fir;
+use crate::filtering::fir::FirFilter;
+
+#[derive(BlockIO)]
+#[sync_block(tagged)]
+pub struct EarlyLateGate<T: Float + Sum + Clone + 'static>
+{
+    #[input]
+    input: In<T>,
+
+    #[output]
+    output: Out<T>,
+
+    symbol_length: usize,
+
+    // Window looking at symbol_length samples at a time
+    window: VecDeque<T>,
+
+    // The current location of the window, in relation to the last sample
+    window_location: usize,
+
+    window_center: usize,
+
+    // The next window location, in relation to the last sample such that the window is centered on
+    // a symbol center (hopefully)
+    next_sample: usize,
+    loop_filter: FirFilter<T, T, T>,
+}
+
+impl<T> EarlyLateGate<T>
+where
+    T: Float + Sum + Clone + 'static,
+{
+    pub fn new(input: In<T>, loop_filter: Fir<T>, symbol_length: usize) -> (Self, In<T>)
+    {
+        let (output, samples) = oxydsp_flowgraph::io::stream();
+        (
+            Self {
+                input,
+                output,
+                window: VecDeque::with_capacity(symbol_length),
+                symbol_length,
+                window_location: 0,
+                window_center: symbol_length / 2,
+                next_sample: symbol_length, // We assume that the first symbol is 1.5 windows into
+                // the stream
+                loop_filter: FirFilter::new(loop_filter),
+            },
+            samples,
+        )
+    }
+}
+
+impl<'view, T> SyncBlock<'view> for EarlyLateGate<T>
+where
+    T: Float + Sum + Clone + 'static,
+{
+    fn sync_work(state: Self::StateView, input: Self::Input) -> Option<Self::Output>
+    {
+        if state.window.len() < *state.symbol_length
+        {
+            state.window.push_back(input.0);
+            return Some(input.0.into());
+        }
+
+        // Bring new sample in
+        state.window.pop_front();
+        state.window.push_back(input.0);
+        *state.window_location += 1;
+
+        let sample = state.window[*state.window_center];
+        let mut tag = None;
+        if *state.window_location >= *state.next_sample
+        {
+            let new_tag = Tag::default();
+            new_tag.tag("elg_symbol", ());
+            tag = Some(new_tag);
+
+            let early_index = *state.window_center - (0.25 * *state.symbol_length as f32) as usize;
+            let late_index = *state.window_center + (0.25 * *state.symbol_length as f32) as usize;
+
+            let early_sample = state.window[early_index];
+            let late_sample = state.window[late_index];
+
+            let error = (late_sample - early_sample) * sample;
+            let correction = state.loop_filter.next(error);
+
+            // Figure out next sample location
+            *state.next_sample += (*state.symbol_length as isize
+                + correction.floor().to_isize().unwrap_or(0))
+            .max(0) as usize;
+
+            // Turn everything back relative to current sample
+            *state.next_sample -= *state.window_location;
+            *state.window_location = 0;
+        }
+
+        Some((sample, tag).into())
+    }
+}

oxydsp-dsp/src/blocks/utilities/adapters.rs

@@ -1,11 +1,17 @@
+use core::sync;
+
 use oxydsp_flowgraph::BlockIO;
 use oxydsp_flowgraph::block::Block;
 use oxydsp_flowgraph::block::BlockResult;
+use oxydsp_flowgraph::block::SyncBlock;
 use oxydsp_flowgraph::io::In;
 use oxydsp_flowgraph::io::Out;
 use oxydsp_flowgraph::io::PopIterable;
 use oxydsp_flowgraph::io::stream;
+use oxydsp_flowgraph::sync_block;
 use oxydsp_flowgraph::tag::Tag;
+use oxydsp_flowgraph::tag::TagMergable;
+use oxydsp_flowgraph::tag::Tagged;
 
 #[derive(BlockIO)]
 pub struct Map<I: 'static, O: 'static, F>
@@ -36,12 +42,168 @@ where
 {
     fn work(&mut self) -> oxydsp_flowgraph::block::BlockResult
     {
-        self.output
-            .push_iter(self.input.pop_iter().map(|x| ((&self.map)(x.0), x.1)));
+        self.output.push_iter(
+            self.input
+                .pop_iter()
+                .map(|x| ((&self.map)(x.0), x.1).into()),
+        );
         BlockResult::Ok
     }
 }
 
+#[derive(BlockIO)]
+pub struct MapResult<I: 'static, O: 'static, F>
+{
+    #[input]
+    input: In<I>,
+
+    #[output]
+    output: Out<O>,
+
+    map: F,
+}
+
+impl<I: 'static, O: 'static, F> MapResult<I, O, F>
+where
+    F: Fn(I) -> (O, BlockResult),
+{
+    pub fn new(input: In<I>, map: F) -> (Self, In<O>)
+    {
+        let (output, mapped) = stream();
+        (Self { input, output, map }, mapped)
+    }
+}
+
+impl<I: 'static, O: 'static, F> Block for MapResult<I, O, F>
+where
+    F: Fn(I) -> (O, BlockResult),
+{
+    fn work(&mut self) -> oxydsp_flowgraph::block::BlockResult
+    {
+        let writer = self.output.write();
+        let reader = self.input.read();
+
+        for _ in 0..(writer.len().min(reader.len()))
+        {
+            let (input, tag_opt) = reader.pop().unwrap().into();
+            let (output, result) = (self.map)(input);
+            let _ = writer.push((output, tag_opt).into());
+            match result
+            {
+                BlockResult::Terminated | BlockResult::Exit =>
+                {
+                    return result;
+                }
+                BlockResult::Ok =>
+                {}
+            }
+        }
+        BlockResult::Ok
+    }
+}
+
+#[derive(BlockIO)]
+pub struct MapResultTagged<I: 'static, O: 'static, F>
+{
+    #[input]
+    input: In<I>,
+
+    #[output]
+    output: Out<O>,
+
+    map: F,
+}
+
+impl<I: 'static, O: 'static, F> MapResultTagged<I, O, F>
+where
+    F: Fn(Tagged<I>) -> (Tagged<O>, BlockResult),
+{
+    pub fn new(input: In<I>, map: F) -> (Self, In<O>)
+    {
+        let (output, mapped) = stream();
+        (Self { input, output, map }, mapped)
+    }
+}
+
+impl<I: 'static, O: 'static, F> Block for MapResultTagged<I, O, F>
+where
+    F: Fn(Tagged<I>) -> (Tagged<O>, BlockResult),
+{
+    fn work(&mut self) -> oxydsp_flowgraph::block::BlockResult
+    {
+        let writer = self.output.write();
+        let reader = self.input.read();
+
+        for _ in 0..(writer.len().min(reader.len()))
+        {
+            let (input, tag_opt) = reader.pop().unwrap().into();
+            let (tagged_out, result) = (self.map)((input, tag_opt.clone()).into());
+            let (output, tag_out) = tagged_out.into();
+
+            let _ = writer.push((output, tag_opt.merge(&tag_out)).into());
+            match result
+            {
+                BlockResult::Terminated | BlockResult::Exit =>
+                {
+                    return result;
+                }
+                BlockResult::Ok =>
+                {}
+            }
+        }
+        BlockResult::Ok
+    }
+}
+
+#[derive(BlockIO)]
+#[sync_block]
+pub struct Scan<I: 'static, O: 'static, S, F>
+where
+    F: Fn(&mut S, I) -> O,
+{
+    #[input]
+    input: In<I>,
+
+    #[output]
+    output: Out<O>,
+
+    state: S,
+
+    map: F,
+}
+
+impl<I: 'static, O: 'static, S, F> Scan<I, O, S, F>
+where
+    F: Fn(&mut S, I) -> O,
+{
+    pub fn new(input: In<I>, initial_state: S, map: F) -> (Self, In<O>)
+    {
+        let (output, mapped) = stream();
+        (
+            Self {
+                input,
+                output,
+                state: initial_state,
+                map,
+            },
+            mapped,
+        )
+    }
+}
+
+impl<'view, I, O, S, F> SyncBlock<'view> for Scan<I, O, S, F>
+where
+    I: 'static,
+    O: 'static,
+    S: 'view,
+    F: Fn(&mut S, I) -> O + 'view,
+{
+    fn sync_work(state: Self::StateView, input: Self::Input) -> Option<Self::Output>
+    {
+        Some((*state.map)(state.state, input))
+    }
+}
+
 #[derive(BlockIO)]
 pub struct Repeat<T: 'static>
 {
@@ -110,3 +272,28 @@ impl<T: Clone + 'static> Block for Repeat<T>
         BlockResult::Ok
     }
 }
+
+#[derive(BlockIO)]
+#[sync_block]
+pub struct NullSink<T: 'static>
+{
+    #[input]
+    input: In<T>,
+}
+
+impl<T: 'static> NullSink<T>
+{
+    pub fn new(input: In<T>) -> Self
+    {
+        Self { input }
+    }
+}
+
+impl<'view, I: 'static> SyncBlock<'view> for NullSink<I>
+{
+    fn sync_work(_: Self::StateView, _: Self::Input) -> Option<Self::Output>
+    {
+        // Don't do shit !
+        Some(())
+    }
+}

oxydsp-dsp/src/blocks/utilities/channels.rs

@@ -49,7 +49,9 @@ impl<I: 'static> Block for RxSource<Receiver<I>, I>
 {
     fn work(&mut self) -> oxydsp_flowgraph::block::BlockResult
     {
-        if self.output.push_iter(self.input.iter().map(|x| (x, None)))
+        if self
+            .output
+            .push_iter(self.input.iter().map(|x| (x, None).into()))
         {
             BlockResult::Ok
         }

oxydsp-dsp/src/blocks/utilities/iter.rs

@@ -1,15 +1,21 @@
-use oxydsp_flowgraph::{
-    BlockIO,
-    block::{Block, BlockResult},
-    io::{In, Out, stream},
-};
+use std::iter::Peekable;
+
+use oxydsp_flowgraph::BlockIO;
+use oxydsp_flowgraph::block::Block;
+use oxydsp_flowgraph::block::BlockResult;
+use oxydsp_flowgraph::block::SyncBlock;
+use oxydsp_flowgraph::io::In;
+use oxydsp_flowgraph::io::Out;
+use oxydsp_flowgraph::io::stream;
+use oxydsp_flowgraph::sync_block;
+use oxydsp_flowgraph::tag::Tag;
 
 #[derive(BlockIO)]
 pub struct IterSource<I: Iterator>
 where
     I::Item: 'static,
 {
-    iter: I,
+    iter: Peekable<I>,
 
     #[output]
     output: Out<I::Item>,
@@ -23,7 +29,13 @@ where
     pub fn new(iter: I) -> (Self, In<I::Item>)
     {
         let (output, items) = stream();
-        (Self { iter, output }, items)
+        (
+            Self {
+                iter: iter.peekable(),
+                output,
+            },
+            items,
+        )
     }
 }
 
@@ -34,13 +46,22 @@ where
 {
     fn work(&mut self) -> oxydsp_flowgraph::block::BlockResult
     {
-        if self.output.push_iter((&mut self.iter).map(|x| (x, None)))
+        let writer = self.output.write();
+
+        for _ in 0..writer.len()
         {
-            BlockResult::Ok
-        }
-        else
-        {
-            BlockResult::Terminated
+            if let Some(element) = self.iter.next()
+            {
+                let mut tag = None;
+                if self.iter.peek().is_none()
+                {
+                    let new_tag = Tag::default();
+                    new_tag.tag("itersource_finished", ());
+                    tag = Some(new_tag);
+                }
+                let _ = writer.push((element, tag).into());
+            }
         }
+        BlockResult::Ok
     }
 }

oxydsp-dsp/src/filtering.rs

@@ -0,0 +1 @@
+pub mod fir;

oxydsp-dsp/src/filtering/fir.rs

@@ -0,0 +1,120 @@
+use std::collections::VecDeque;
+use std::f64::consts::PI;
+use std::iter::Sum;
+use std::ops::Div;
+use std::ops::Mul;
+use std::process::Output;
+
+use num::Complex;
+use num::Float;
+use num::One;
+use num::Zero;
+use num::complex::ComplexFloat;
+use num::zero;
+use rustfft::FftNum;
+use rustfft::FftPlanner;
+
+use crate::map;
+use crate::units::DigitalFrequency;
+
+/// Finite impulse response
+pub struct Fir<T>(pub Vec<T>);
+
+impl<T> Fir<Complex<T>>
+where
+    T: FftNum + Float + Clone,
+{
+    pub fn from_transfer_function(tf: impl AsRef<[Complex<T>]>) -> Fir<Complex<T>>
+    {
+        let mut planner = FftPlanner::new();
+        let tf_len = tf.as_ref().len();
+        let ifft = planner.plan_fft_inverse(tf.as_ref().len());
+
+        let mut fir = tf.as_ref().to_vec();
+        ifft.process(fir.as_mut_slice());
+
+        let mut shifted_fir = vec![];
+        for i in 0..tf_len
+        {
+            let k = (tf_len - (tf_len / 2) + i) % tf_len;
+            shifted_fir.push(fir[k]);
+        }
+
+        Fir(shifted_fir)
+    }
+
+    pub fn lowpass(cutoff: DigitalFrequency, length: usize) -> Fir<Complex<T>>
+    {
+        let mut tf = vec![Complex::<T>::zero(); length];
+
+        let cutoff_bin = map(cutoff.as_rad(), 0., 2. * PI, 0., length as f64).floor() as usize;
+        for i in 0..cutoff_bin
+        {
+            tf[i] = Complex::<T>::one();
+            tf[length - i - 1] = Complex::<T>::one();
+        }
+
+        Self::from_transfer_function(tf)
+    }
+}
+
+impl<T> Fir<T>
+where
+    T: ComplexFloat + Div<T::Real, Output = T> + Copy + Sum,
+    T::Real: Float,
+{
+    pub fn normalized(mut self) -> Self
+    {
+        let sum: T = self.0.iter().copied().sum();
+        let len = Float::sqrt(sum.im() * sum.im() + sum.re() * sum.re());
+
+        self.0.iter_mut().for_each(|x| *x = *x / len);
+        self
+    }
+}
+
+pub struct FirFilter<F, T, O>
+where
+    F: Mul<T, Output = O>,
+    O: Sum,
+{
+    fir: Vec<F>,
+    taps: VecDeque<T>,
+}
+
+impl<F, T, O> FirFilter<F, T, O>
+where
+    T: Clone,
+    F: Mul<T, Output = O> + Clone,
+    O: Sum,
+{
+    pub fn new(impulse_response: Fir<F>) -> Self
+    {
+        let len = impulse_response.0.len();
+        Self {
+            fir: impulse_response.0,
+            taps: VecDeque::with_capacity(len),
+        }
+    }
+
+    pub fn next(&mut self, input: T) -> O
+    {
+        if self.taps.len() == self.fir.len()
+        {
+            self.taps.pop_front();
+        }
+        self.taps.push_back(input);
+
+        self.fir
+            .iter()
+            .zip(self.taps.iter())
+            .map(|(a, b)| a.clone() * b.clone())
+            .sum()
+    }
+}
+
+// Completely stolen from sdrpp code
+pub fn estimate_fir_length(transition_width: f64, sample_rate: f64) -> f64
+{
+    3.8 * sample_rate / transition_width
+}

oxydsp-dsp/src/lib.rs

@@ -1,6 +1,7 @@
 use num::Float;
 
 pub mod blocks;
+pub mod filtering;
 pub mod synthesis;
 pub mod units;
 

oxydsp-dsp/src/units.rs

@@ -1,4 +1,5 @@
-use std::{f64::consts::PI, ops::Neg};
+use std::f64::consts::PI;
+use std::ops::Neg;
 
 use crate::map;