complex: richer API

This commit is contained in:
Robert Jördens 2021-01-30 18:05:54 +01:00
parent c34e330663
commit 0d1b237202
3 changed files with 87 additions and 26 deletions

View File

@ -1,17 +1,58 @@
use super::atan2; use super::{atan2, cossin};
use serde::{Deserialize, Serialize}; use serde::{Deserialize, Serialize};
#[derive(Copy, Clone, Default, Deserialize, Serialize)] #[derive(Copy, Clone, Default, PartialEq, Debug, Deserialize, Serialize)]
pub struct Complex<T>(pub T, pub T); pub struct Complex<T>(pub T, pub T);
impl Complex<i32> { impl Complex<i32> {
pub fn power(&self) -> i32 { /// Return a Complex on the unit circle given an angle.
(((self.0 as i64) * (self.0 as i64) ///
+ (self.1 as i64) * (self.1 as i64)) /// Example:
>> 32) as i32 ///
/// ```
/// use dsp::Complex;
/// Complex::<i32>::from_angle(0);
/// Complex::<i32>::from_angle(1 << 30); // pi/2
/// Complex::<i32>::from_angle(-1 << 30); // -pi/2
/// ```
#[inline(always)]
pub fn from_angle(angle: i32) -> Complex<i32> {
cossin(angle)
} }
pub fn phase(&self) -> i32 { /// Return the absolute square (the squared magnitude).
///
/// Note: Normalization is `1 << 31`, i.e. Q0.31.
///
/// Example:
///
/// ```
/// use dsp::Complex;
/// assert_eq!(Complex(i32::MAX, 0).abs_sqr(), i32::MAX - 1);
/// assert_eq!(Complex(i32::MIN + 1, 0).abs_sqr(), i32::MAX - 1);
/// ```
pub fn abs_sqr(&self) -> i32 {
(((self.0 as i64) * (self.0 as i64)
+ (self.1 as i64) * (self.1 as i64))
>> 31) as i32
}
/// Return the angle.
///
/// Note: Normalization is `1 << 31 == pi`.
///
/// Example:
///
/// ```
/// use dsp::Complex;
/// assert_eq!(Complex(i32::MAX, 0).arg(), 0);
/// assert_eq!(Complex(-i32::MAX, 1).arg(), i32::MAX);
/// assert_eq!(Complex(-i32::MAX, -1).arg(), -i32::MAX);
/// assert_eq!(Complex(0, -i32::MAX).arg(), -i32::MAX >> 1);
/// assert_eq!(Complex(0, i32::MAX).arg(), (i32::MAX >> 1) + 1);
/// assert_eq!(Complex(i32::MAX, i32::MAX).arg(), (i32::MAX >> 2) + 1);
/// ```
pub fn arg(&self) -> i32 {
atan2(self.1, self.0) atan2(self.1, self.0)
} }
} }

View File

@ -1,4 +1,4 @@
use super::{cossin, iir_int, Complex}; use super::{iir_int, Complex};
use serde::{Deserialize, Serialize}; use serde::{Deserialize, Serialize};
#[derive(Copy, Clone, Default, Deserialize, Serialize)] #[derive(Copy, Clone, Default, Deserialize, Serialize)]
@ -19,7 +19,7 @@ impl Lockin {
pub fn update(&mut self, signal: i32, phase: i32) -> Complex<i32> { pub fn update(&mut self, signal: i32, phase: i32) -> Complex<i32> {
// Get the LO signal for demodulation. // Get the LO signal for demodulation.
let m = cossin(phase); let m = Complex::from_angle(phase);
// Mix with the LO signal, filter with the IIR lowpass, // Mix with the LO signal, filter with the IIR lowpass,
// return IQ (in-phase and quadrature) data. // return IQ (in-phase and quadrature) data.
@ -35,6 +35,23 @@ impl Lockin {
), ),
) )
} }
pub fn feed<I: IntoIterator<Item = i32>>(
&mut self,
signal: I,
phase: i32,
frequency: i32,
) -> Option<Complex<i32>> {
let mut phase = phase;
signal
.into_iter()
.map(|s| {
phase = phase.wrapping_add(frequency);
self.update(s, phase)
})
.last()
}
} }
#[cfg(test)] #[cfg(test)]

View File

@ -123,27 +123,26 @@ const APP: () = {
22, // relative PLL phase bandwidth: 2**-22, TODO: expose 22, // relative PLL phase bandwidth: 2**-22, TODO: expose
); );
// Harmonic index of the LO: -1 to _de_modulate the fundamental // Harmonic index of the LO: -1 to _de_modulate the fundamental (complex conjugate)
let harmonic: i32 = -1; let harmonic: i32 = -1;
// Demodulation LO phase offset // Demodulation LO phase offset
let phase_offset: i32 = 0; let phase_offset: i32 = 0;
let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2) let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2)
as i32) as i32)
.wrapping_mul(harmonic); // TODO: maybe rounding bias .wrapping_mul(harmonic); // TODO: maybe rounding bias
let mut sample_phase = let sample_phase =
phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
for i in 0..adc_samples[0].len() { if let Some(output) = lockin.feed(
// Convert to signed, MSB align the ADC sample. adc_samples[0].iter().map(|&i|
let input = (adc_samples[0][i] as i16 as i32) << 16; // Convert to signed, MSB align the ADC sample.
// Obtain demodulated, filtered IQ sample. (i as i16 as i32) << 16),
let output = lockin.update(input, sample_phase); sample_phase,
// Advance the sample phase. sample_frequency,
sample_phase = sample_phase.wrapping_add(sample_frequency); ) {
// Convert from IQ to power and phase. // Convert from IQ to power and phase.
let mut power = output.power() as _; let mut power = output.abs_sqr() as _;
let mut phase = output.phase() as _; let mut phase = output.arg() as _;
// Filter power and phase through IIR filters. // Filter power and phase through IIR filters.
// Note: Normalization to be done in filters. Phase will wrap happily. // Note: Normalization to be done in filters. Phase will wrap happily.
@ -152,13 +151,17 @@ const APP: () = {
phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
} }
// TODO: IIR filter DC gain needs to be 1/(1 << 16)
// Note(unsafe): range clipping to i16 is ensured by IIR filters above. // Note(unsafe): range clipping to i16 is ensured by IIR filters above.
// Convert to DAC data. // Convert to DAC data.
unsafe { for i in 0..dac_samples[0].len() {
dac_samples[0][i] = unsafe {
power.to_int_unchecked::<i16>() as u16 ^ 0x8000; dac_samples[0][i] =
dac_samples[1][i] = power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
phase.to_int_unchecked::<i16>() as u16 ^ 0x8000; dac_samples[1][i] =
phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
}
} }
} }
} }