complex: richer API
This commit is contained in:
parent
c34e330663
commit
0d1b237202
@ -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)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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)]
|
||||||
|
@ -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;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user