71 lines
1.8 KiB
Rust
71 lines
1.8 KiB
Rust
// 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) => {
|
|
// self.process_chunk(&mut chunk);
|
|
// Some(Ok(chunk))
|
|
Some(Ok(self.process_chunk(&chunk)))
|
|
}
|
|
Err(e) => Some(Err(e)),
|
|
}
|
|
}
|
|
}
|