fm mod init

.gitignore

@@ -1 +1,2 @@
 /target
+test.iq

Cargo.lock

@@ -0,0 +1,34 @@
+# This file is automatically @generated by Cargo.
+# It is not intended for manual editing.
+version = 4
+
+[[package]]
+name = "analog-sdr-video-demodulator"
+version = "0.1.0"
+dependencies = [
+ "num-complex",
+]
+
+[[package]]
+name = "autocfg"
+version = "1.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53"
+
+[[package]]
+name = "num-complex"
+version = "0.4.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "num-traits"
+version = "0.2.19"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841"
+dependencies = [
+ "autocfg",
+]

Cargo.toml

@@ -4,3 +4,4 @@ version = "0.1.0"
 edition = "2024"
 
 [dependencies]
+num-complex = "0.4.6"

src/agc.rs

@@ -0,0 +1,107 @@
+use crate::iq_reader::IqChunk;
+use crate::iq_reader::IqSample;
+
+// Automatic Gain Control
+pub struct Agc<I> {
+    inner: I,
+    // Previous power estimate
+    pub power_estimate: f32,
+    // Previous gain
+    pub current_gain: f32,
+    pub target_power: f32,
+    pub alpha_attack: f32,
+    pub alpha_release: f32,
+    pub beta: f32,
+    pub min_gain: f32,
+    pub max_gain: f32,
+}
+
+impl<I> Agc<I> {
+    pub fn new(
+        inner: I,
+        sample_rate: f32,
+        target_power: f32,
+        min_gain: f32,
+        max_gain: f32,
+    ) -> Self {
+        // Target attack time 5 ms
+        let tau_attack = 0.005;
+
+        // Target release time 50 ms
+        let tau_release = 0.05;
+
+        let alpha_attack = 1.0 - f32::exp(-1.0 / (sample_rate * tau_attack));
+        let alpha_release = 1.0 - f32::exp(-1.0 / (sample_rate * tau_release));
+
+        let beta = 0.999;
+
+        let power_estimate = 0.0;
+        let current_gain = 1.0;
+
+        Self {
+            inner,
+            power_estimate,
+            current_gain,
+            target_power,
+            alpha_attack,
+            alpha_release,
+            beta,
+            min_gain,
+            max_gain,
+        }
+    }
+
+    pub fn process_chunk(&mut self, chunk: &mut IqChunk) {
+        for z in chunk.samples.iter_mut() {
+            let i = z.re;
+            let q = z.im;
+
+            // Instant Power
+            let inst_power = i * i + q * q;
+
+            let alpha = if inst_power > self.power_estimate {
+                self.alpha_attack
+            } else {
+                self.alpha_release
+            };
+
+            // IIR filter
+            let power_estimate = alpha * inst_power + (1.0 - alpha) * self.power_estimate;
+
+            // Update Power
+            self.power_estimate = power_estimate.max(1e-10);
+
+            let raw_gain = (self.target_power / self.power_estimate).sqrt();
+
+            // Gain in [min_gain ; max_gain]
+            let raw_gain = match raw_gain {
+                g if g < self.min_gain => self.min_gain,
+                g if g > self.max_gain => self.max_gain,
+                _ => raw_gain,
+            };
+
+            let final_gain = self.beta * self.current_gain + (1.0 - self.beta) * raw_gain;
+
+            self.current_gain = final_gain;
+
+            *z = IqSample::new(i * final_gain, q * final_gain);
+        }
+    }
+}
+
+impl<I, E> Iterator for Agc<I>
+where
+    I: Iterator<Item = Result<IqChunk, E>>,
+{
+    type Item = Result<IqChunk, E>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        match self.inner.next()? {
+            Ok(mut chunk) => {
+                self.process_chunk(&mut chunk);
+                Some(Ok(chunk))
+            }
+            Err(e) => Some(Err(e)),
+        }
+    }
+}

src/fir.rs

@@ -0,0 +1,66 @@
+// Finite Impulse response + Decimation
+use crate::{iq_reader::IqChunk, iq_reader::IqSample, utils::ring_buffer::RingBuffer};
+
+pub struct Fir<I, const N: usize> {
+    inner: I,
+    // Filter coefs
+    pub taps: [f32; N],
+
+    // Ring Buffer of samples
+    pub history: RingBuffer<IqSample>,
+
+    // Factor of decimation
+    pub decimation_factor: usize,
+
+    // Track decimation
+    pub decimation_index: usize,
+}
+
+impl<I, const N: usize> Fir<I, N> {
+    pub fn new(inner: I, taps: [f32; N], decimation_factor: usize) -> Self {
+        Self {
+            inner,
+            taps,
+            history: RingBuffer::<IqSample>::new(N),
+            decimation_factor,
+            decimation_index: 0,
+        }
+    }
+
+    pub fn process_chunk(&mut self, chunk: &IqChunk) -> IqChunk {
+        let mut chunk_out = IqChunk::new();
+
+        for iq in chunk.samples.iter() {
+            self.history.push(*iq);
+
+            // Decimation
+            if self.decimation_index.is_multiple_of(self.decimation_factor) {
+                let mut y_n = IqSample::default();
+                for k in 0..N {
+                    if let Some(sample) = self.history.read_at(k) {
+                        y_n += *sample * self.taps[k];
+                    }
+                }
+
+                chunk_out.samples.push(y_n);
+            }
+
+            self.decimation_index += 1;
+        }
+        chunk_out
+    }
+}
+
+impl<I, E, const N: usize> Iterator for Fir<I, N>
+where
+    I: Iterator<Item = Result<IqChunk, E>>,
+{
+    type Item = Result<IqChunk, E>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        match self.inner.next()? {
+            Ok(chunk) => Some(Ok(self.process_chunk(&chunk))),
+            Err(e) => Some(Err(e)),
+        }
+    }
+}

src/fm_demod.rs

@@ -0,0 +1,39 @@
+use std::{os::unix::process, slice::Chunks};
+
+use crate::iq_reader::{IqChunk, IqSample};
+
+pub struct FmDemod<I> {
+    pub inner: I,
+    pub prev_sample: IqSample,
+}
+
+impl<I> FmDemod<I> {
+    pub fn new(inner: I) -> Self {
+        Self {
+            inner,
+            prev_sample: IqSample::new(1.0, 0.0),
+        }
+    }
+
+    pub fn process_chunk(&mut self, chunk: &mut IqChunk) {
+        // TODO: FM Demodulation
+        todo!();
+    }
+}
+
+impl<I, E> Iterator for FmDemod<I>
+where
+    I: Iterator<Item = Result<IqChunk, E>>,
+{
+    type Item = Result<IqChunk, E>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        match self.inner.next()? {
+            Ok(mut chunk) => {
+                self.process_chunk(&mut chunk);
+                Some(Ok(chunk))
+            }
+            Err(e) => Some(Err(e)),
+        }
+    }
+}

src/iq_reader.rs

@@ -0,0 +1,81 @@
+use num_complex::Complex;
+use std::error::Error;
+use std::fs::File;
+use std::io::{BufReader, ErrorKind, Read};
+
+pub type IqSample = Complex<f32>;
+
+// Data chunk
+#[derive(Debug)]
+pub struct IqChunk {
+    pub samples: Vec<IqSample>,
+}
+
+impl IqChunk {
+    pub fn new() -> Self {
+        IqChunk {
+            samples: Vec::<IqSample>::new(),
+        }
+    }
+}
+
+pub struct FileSource {
+    // Buffer
+    pub reader: BufReader<File>,
+    // Hackrf I & Q 8 bits
+    pub raw_buffer: Vec<u8>,
+    // Size of a sample chunk
+    pub chunk_samples_size: usize,
+}
+
+impl FileSource {
+    pub fn new(file_path: &str, chunk_samples_size: usize) -> Result<Self, Box<dyn Error>> {
+        let file = File::open(file_path)?;
+
+        // Init buffer with size 16 Mo
+        let reader = BufReader::with_capacity(16 * 1024 * 1024, file);
+
+        // 1 sample = 2 bytes (1 byte : I, 1 byte : Q)
+        let raw_reader = vec![0; chunk_samples_size * 2];
+
+        Ok(Self {
+            reader,
+            raw_buffer: raw_reader,
+            chunk_samples_size,
+        })
+    }
+}
+
+impl Iterator for FileSource {
+    type Item = Result<IqChunk, Box<dyn Error>>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        // Buffer read
+        match self.reader.read_exact(&mut self.raw_buffer) {
+            Ok(_) => {}
+
+            // EOF
+            Err(e) if e.kind() == ErrorKind::UnexpectedEof => {
+                return None;
+            }
+
+            Err(e) => {
+                return Some(Err(e.into()));
+            }
+        }
+
+        // Output samples
+        let mut samples = Vec::with_capacity(self.chunk_samples_size);
+
+        // Buffer read
+        for iq in self.raw_buffer.chunks(2) {
+            let i = iq[0] as i8;
+            let q = iq[1] as i8;
+            let i_f32 = (i as f32) / 128.0;
+            let q_f32 = (q as f32) / 128.0;
+            samples.push(Complex::new(i_f32, q_f32));
+        }
+
+        Some(Ok(IqChunk { samples }))
+    }
+}

src/main.rs

@@ -1,3 +1,28 @@
-fn main() {
+use crate::iq_reader::FileSource;
-    println!("Hello, world!");
+use crate::pipeline::DspPipelineExt;
+use std::error::Error;
+
+mod agc;
+mod fir;
+mod fm_demod;
+mod iq_reader;
+mod pipeline;
+mod utils;
+
+fn main() -> Result<(), Box<dyn Error>> {
+    let source = FileSource::new("test.iq", 32769)?;
+
+    // Fir coefs
+    let taps = [0.5; 64];
+
+    let pipeline = source
+        .agc(20_000_000.0, 1.0, 0.001, 100.0)
+        .fir::<64>(taps, 4);
+
+    for chunk_r in pipeline {
+        let chunk = chunk_r?;
+        println!("size : {}", chunk.samples.len());
+    }
+
+    Ok(())
 }

src/pipeline.rs

@@ -0,0 +1,15 @@
+use crate::agc::Agc;
+use crate::fir::Fir;
+use crate::iq_reader::IqChunk;
+
+pub trait DspPipelineExt<E>: Iterator<Item = Result<IqChunk, E>> + Sized {
+    fn agc(self, sample_rate: f32, target_power: f32, min_gain: f32, max_gain: f32) -> Agc<Self> {
+        Agc::new(self, sample_rate, target_power, min_gain, max_gain)
+    }
+
+    fn fir<const N: usize>(self, taps: [f32; N], decimation_factor: usize) -> Fir<Self, N> {
+        Fir::new(self, taps, decimation_factor)
+    }
+}
+
+impl<I, E> DspPipelineExt<E> for I where I: Iterator<Item = Result<IqChunk, E>> {}

src/utils/mod.rs

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

src/utils/ring_buffer.rs

@@ -0,0 +1,78 @@
+pub struct RingBuffer<T: Copy + Default> {
+    pub data: Vec<T>,
+
+    // Index to next writing element
+    pub head: usize,
+
+    // Actual size
+    pub size: usize,
+
+    pub capacity: usize,
+}
+
+impl<T: Copy + Default> RingBuffer<T> {
+    pub fn new(capacity: usize) -> Self {
+        assert!(capacity != 0);
+        Self {
+            data: vec![T::default(); capacity],
+            head: 0,
+            size: 0,
+            capacity,
+        }
+    }
+
+    pub fn push(&mut self, value: T) {
+        self.data[self.head] = value;
+        self.head = (self.head + 1) % self.capacity;
+        self.size = (self.size + 1).min(self.capacity);
+    }
+
+    pub fn pop(&mut self) -> Option<T> {
+        if self.size == 0 {
+            return None;
+        }
+        let tail = (self.head + self.capacity - self.size) % self.capacity;
+        self.size -= 1;
+        Some(self.data[tail])
+    }
+
+    pub fn peek(&self) -> Option<&T> {
+        if self.size == 0 {
+            return None;
+        }
+        let last = (self.head + self.capacity - 1) % self.capacity;
+        Some(&self.data[last])
+    }
+
+    // Read N samples before
+    pub fn read_at(&self, delay: usize) -> Option<&T> {
+        if delay >= self.size {
+            return None;
+        }
+        let index = (self.head + self.capacity - 1 - delay) % self.capacity;
+        Some(&self.data[index])
+    }
+
+    pub fn write_read(&mut self, value: T, delay: usize) -> Option<T> {
+        let delayed = self.read_at(delay).copied();
+        self.push(value);
+        delayed
+    }
+
+    pub fn len(&self) -> usize {
+        self.size
+    }
+
+    pub fn is_empty(&self) -> bool {
+        self.size == 0
+    }
+
+    pub fn is_full(&self) -> bool {
+        self.size == self.capacity
+    }
+
+    pub fn clear(&mut self) {
+        self.head = 0;
+        self.size = 0;
+    }
+}