pounder_test/dsp/src/lockin.rs

23 lines
725 B
Rust
Raw Normal View History

use super::{Complex, ComplexExt, Lowpass, MulScaled};
2021-02-23 23:46:16 +08:00
use generic_array::ArrayLength;
2021-01-21 21:55:33 +08:00
#[derive(Clone, Default)]
2021-02-23 23:46:16 +08:00
pub struct Lockin<N: ArrayLength<i32>> {
state: [Lowpass<N>; 2],
2021-01-21 21:55:33 +08:00
}
2021-02-23 23:46:16 +08:00
impl<N: ArrayLength<i32>> Lockin<N> {
2021-02-01 03:32:44 +08:00
/// Update the lockin with a sample taken at a given phase.
pub fn update(&mut self, sample: i32, phase: i32, k: u8) -> Complex<i32> {
2021-02-18 20:17:24 +08:00
// Get the LO signal for demodulation and mix the sample;
let mix = Complex::from_angle(phase).mul_scaled(sample);
2021-02-12 19:03:53 +08:00
// Filter with the IIR lowpass,
2021-01-21 21:55:33 +08:00
// return IQ (in-phase and quadrature) data.
2021-02-18 20:17:24 +08:00
Complex {
re: self.state[0].update(mix.re, k),
im: self.state[1].update(mix.im, k),
}
2021-01-21 21:55:33 +08:00
}
}