2021-02-10 01:30:50 +08:00
|
|
|
use super::{lowpass::Lowpass, Complex};
|
2021-02-12 06:15:32 +08:00
|
|
|
use generic_array::typenum::U2;
|
2021-01-21 21:55:33 +08:00
|
|
|
|
2021-02-10 01:30:50 +08:00
|
|
|
#[derive(Clone, Default)]
|
2021-01-21 21:55:33 +08:00
|
|
|
pub struct Lockin {
|
2021-02-12 06:15:32 +08:00
|
|
|
state: [Lowpass<U2>; 2],
|
2021-01-21 21:55:33 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
impl Lockin {
|
2021-02-01 03:32:44 +08:00
|
|
|
/// Update the lockin with a sample taken at a given phase.
|
2021-02-12 18:05:50 +08:00
|
|
|
/// The lowpass has a gain of `1 << k`.
|
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
|
|
|
|
2021-02-12 19:03:53 +08:00
|
|
|
// Mix with the LO signal
|
|
|
|
let mix = lo * sample;
|
|
|
|
|
|
|
|
// Filter with the IIR lowpass,
|
2021-01-21 21:55:33 +08:00
|
|
|
// return IQ (in-phase and quadrature) data.
|
|
|
|
Complex(
|
2021-02-12 19:03:53 +08:00
|
|
|
self.state[0].update(mix.0, k),
|
|
|
|
self.state[1].update(mix.1, k),
|
2021-01-21 21:55:33 +08:00
|
|
|
)
|
|
|
|
}
|
|
|
|
}
|