pounder_test/dsp/src/lockin.rs

31 lines
893 B
Rust
Raw Normal View History

use super::{lowpass::Lowpass, Complex};
2021-02-11 21:30:05 +08:00
use generic_array::typenum::U4;
2021-01-21 21:55:33 +08:00
#[derive(Clone, Default)]
2021-01-21 21:55:33 +08:00
pub struct Lockin {
2021-02-11 21:30:05 +08:00
state: [Lowpass<U4>; 2],
2021-01-21 21:55:33 +08:00
}
impl Lockin {
2021-02-01 03:32:44 +08:00
/// Create a new Lockin with given IIR coefficients.
2021-02-11 21:30:05 +08:00
pub fn new() -> Self {
let lp = Lowpass::default();
2021-02-01 19:37:44 +08:00
Self {
2021-02-10 20:27:56 +08:00
state: [lp.clone(), lp],
2021-01-21 21:55:33 +08:00
}
}
2021-02-01 03:32:44 +08:00
/// Update the lockin with a sample taken at a given phase.
2021-02-11 21:30:05 +08:00
pub fn update(&mut self, sample: i16, phase: i32, k: u8) -> Complex<i32> {
2021-01-21 21:55:33 +08:00
// Get the LO signal for demodulation.
2021-02-01 03:32:44 +08:00
let lo = Complex::from_angle(phase);
2021-01-21 21:55:33 +08:00
// Mix with the LO signal, filter with the IIR lowpass,
// return IQ (in-phase and quadrature) data.
Complex(
2021-02-11 21:30:05 +08:00
self.state[0].update((sample as i32 * (lo.0 >> 16)) >> 16, k),
self.state[1].update((sample as i32 * (lo.1 >> 16)) >> 16, k),
2021-01-21 21:55:33 +08:00
)
}
}