kai
/
pounder_test
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge pull request #267 from quartiq/rj/lowpass2 

lowpass2
master
Robert Jördens 2021-02-12 12:23:26 +01:00 committed by GitHub
parent c89f348f5e 8918952b96
commit 16c986ab1b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 122 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
dsp/src/complex.rs
View File

@ -1,8 +1,19 @@
use core::ops::Mul;
use super::{atan2, cossin};
#[derive(Copy, Clone, Default, PartialEq, Debug)]
pub struct Complex<T>(pub T, pub T);
impl<T: Copy> Complex<T> {
pub fn map<F>(&self, func: F) -> Self
where
F: Fn(T) -> T,
{
Complex(func(self.0), func(self.1))
}
}
impl Complex<i32> {
/// Return a Complex on the unit circle given an angle.
///
@ -21,19 +32,43 @@ impl Complex<i32> {
/// Return the absolute square (the squared magnitude).
///
/// Note: Normalization is `1 << 31`, i.e. Q0.31.
/// Note: Normalization is `1 << 32`, i.e. U0.32.
///
/// Note(panic): This will panic for `Complex(i32::MIN, i32::MIN)`
///
/// 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);
/// assert_eq!(Complex(i32::MIN, 0).abs_sqr(), 1 << 31);
/// assert_eq!(Complex(i32::MAX, i32::MAX).abs_sqr(), u32::MAX - 3);
/// ```
pub fn abs_sqr(&self) -> i32 {
pub fn abs_sqr(&self) -> u32 {
(((self.0 as i64) * (self.0 as i64)
+ (self.1 as i64) * (self.1 as i64))
>> 31) as i32
>> 31) as u32
}
/// log2(power) re full scale approximation
///
/// TODO: scale up, interpolate
///
/// Panic:
/// This will panic for `Complex(i32::MIN, i32::MIN)`
///
/// Example:
///
/// ```
/// use dsp::Complex;
/// assert_eq!(Complex(i32::MAX, i32::MAX).log2(), -1);
/// assert_eq!(Complex(i32::MAX, 0).log2(), -2);
/// assert_eq!(Complex(1, 0).log2(), -63);
/// assert_eq!(Complex(0, 0).log2(), -64);
/// ```
pub fn log2(&self) -> i32 {
let a = (self.0 as i64) * (self.0 as i64)
+ (self.1 as i64) * (self.1 as i64);
-(a.leading_zeros() as i32)
}
/// Return the angle.
@ -44,14 +79,51 @@ impl Complex<i32> {
///
/// ```
/// use dsp::Complex;
/// assert_eq!(Complex(i32::MAX, 0).arg(), 0);
/// assert_eq!(Complex(1, 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);
/// assert_eq!(Complex(0, -1).arg(), -i32::MAX >> 1);
/// assert_eq!(Complex(0, 1).arg(), (i32::MAX >> 1) + 1);
/// assert_eq!(Complex(1, 1).arg(), (i32::MAX >> 2) + 1);
/// ```
pub fn arg(&self) -> i32 {
atan2(self.1, self.0)
}
}
impl Mul for Complex<i32> {
type Output = Self;
fn mul(self, other: Self) -> Self {
let a = self.0 as i64;
let b = self.1 as i64;
let c = other.0 as i64;
let d = other.1 as i64;
Complex(
((a * c - b * d + (1 << 31)) >> 32) as i32,
((b * c + a * d + (1 << 31)) >> 32) as i32,
)
}
}
impl Mul<i32> for Complex<i32> {
type Output = Self;
fn mul(self, other: i32) -> Self {
Complex(
((other as i64 * self.0 as i64 + (1 << 31)) >> 32) as i32,
((other as i64 * self.1 as i64 + (1 << 31)) >> 32) as i32,
)
}
}
impl Mul<i16> for Complex<i32> {
type Output = Self;
fn mul(self, other: i16) -> Self {
Complex(
(other as i32 * (self.0 >> 16) + (1 << 15)) >> 16,
(other as i32 * (self.1 >> 16) + (1 << 15)) >> 16,
)
}
}

23
dsp/src/lockin.rs
View File

@ -1,35 +1,34 @@
use super::{lowpass::Lowpass, Complex};
use generic_array::typenum::U3;
use generic_array::typenum::U2;
#[derive(Clone, Default)]
pub struct Lockin {
state: [Lowpass<U3>; 2],
k: u8,
state: [Lowpass<U2>; 2],
}
impl Lockin {
/// Create a new Lockin with given IIR coefficients.
pub fn new(k: u8) -> Self {
pub fn new() -> Self {
let lp = Lowpass::default();
Self {
state: [lp.clone(), lp],
k,
}
}
/// Update the lockin with a sample taken at a given phase.
pub fn update(&mut self, sample: i32, phase: i32) -> Complex<i32> {
/// The lowpass has a gain of `1 << k`.
pub fn update(&mut self, sample: i16, phase: i32, k: u8) -> Complex<i32> {
// Get the LO signal for demodulation.
let lo = Complex::from_angle(phase);
// Mix with the LO signal, filter with the IIR lowpass,
// Mix with the LO signal
let mix = lo * sample;
// Filter with the IIR lowpass,
// return IQ (in-phase and quadrature) data.
// Note: 32x32 -> 64 bit multiplications are pretty much free.
Complex(
self.state[0]
.update(((sample as i64 * lo.0 as i64) >> 32) as _, self.k),
self.state[1]
.update(((sample as i64 * lo.1 as i64) >> 32) as _, self.k),
self.state[0].update(mix.0, k),
self.state[1].update(mix.1, k),
)
}
}

31
dsp/src/lowpass.rs
View File

@ -1,38 +1,35 @@
use generic_array::{ArrayLength, GenericArray};
/// Arbitrary order, high dynamic range, wide coefficient range,
/// lowpass filter implementation.
/// lowpass filter implementation. DC gain is 1.
///
/// Type argument N is the filter order + 1.
/// Type argument N is the filter order.
#[derive(Clone, Default)]
pub struct Lowpass<N: ArrayLength<i32>> {
// IIR state storage
xy: GenericArray<i32, N>,
y: GenericArray<i32, N>,
}
impl<N: ArrayLength<i32>> Lowpass<N> {
/// Update the filter with a new sample.
///
/// # Args
/// * `x`: Input data
/// * `k`: Log2 time constant, 1..32
/// * `x`: Input data, needs `k` bits headroom.
/// * `k`: Log2 time constant, 0..31.
///
/// # Return
/// Filtered output y
/// Filtered output y, with gain of `1 << k`.
pub fn update(&mut self, x: i32, k: u8) -> i32 {
debug_assert!((1..32).contains(&k));
debug_assert!(k & 31 == k);
// This is an unrolled and optimized first-order IIR loop
// that works for all possible time constants.
// Note the zero(s) at Nyquist.
let mut x0 = x;
let mut x1 = self.xy[0];
self.xy[0] = x0;
for y1 in self.xy[1..].iter_mut() {
x0 = *y1
+ (((x0 >> 2) + (x1 >> 2) - (*y1 >> 1) + (1 << (k - 1))) >> k);
x1 = *y1;
*y1 = x0;
// Note DF-II and the zeros at Nyquist.
let mut x = x << k;
for y in self.y.iter_mut() {
let dy = (x - *y + (1 << (k - 1))) >> k;
*y += dy;
x = *y - (dy >> 1);
}
x0
x
}
}

11
src/bin/lockin-external.rs
View File

@ -34,7 +34,7 @@ const APP: () = {
+ design_parameters::SAMPLE_BUFFER_SIZE_LOG2,
);
let lockin = Lockin::new(10);
let lockin = Lockin::new();
// Enable ADC/DAC events
stabilizer.adcs.0.start();
@ -102,6 +102,9 @@ const APP: () = {
// Demodulation LO phase offset
let phase_offset: i32 = 0; // TODO: expose
// Log2 lowpass time constant
let time_constant: u8 = 6; // TODO: expose
let sample_frequency = ((pll_frequency
// .wrapping_add(1 << design_parameters::SAMPLE_BUFFER_SIZE_LOG2 - 1) // half-up rounding bias
>> design_parameters::SAMPLE_BUFFER_SIZE_LOG2)
@ -115,7 +118,7 @@ const APP: () = {
.zip(Accu::new(sample_phase, sample_frequency))
// Convert to signed, MSB align the ADC sample.
.map(|(&sample, phase)| {
lockin.update((sample as i16 as i32) << 16, phase)
lockin.update(sample as i16, phase, time_constant)
})
.last()
.unwrap();
@ -123,8 +126,8 @@ const APP: () = {
let conf = "frequency_discriminator";
let output = match conf {
// Convert from IQ to power and phase.
"power_phase" => [output.abs_sqr(), output.arg()],
"frequency_discriminator" => [pll_frequency as i32, output.arg()],
"power_phase" => [(output.log2() << 24) as _, output.arg()],
"frequency_discriminator" => [pll_frequency as _, output.arg()],
_ => [output.0, output.1],
};

24
src/bin/lockin-internal.rs
View File

@ -28,7 +28,7 @@ const APP: () = {
// Configure the microcontroller
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
let lockin = Lockin::new(10); // TODO: expose
let lockin = Lockin::new();
// Enable ADC/DAC events
stabilizer.adcs.1.start();
@ -85,10 +85,14 @@ const APP: () = {
1i32 << (32 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2);
// Harmonic index of the LO: -1 to _de_modulate the fundamental
let harmonic: i32 = -1;
let harmonic: i32 = -1; // TODO: expose
// Demodulation LO phase offset
let phase_offset: i32 = (0.25 * i32::MAX as f32) as i32;
let phase_offset: i32 = (0.25 * i32::MAX as f32) as i32; // TODO: expose
// Log2 lowpass time constant.
let time_constant: u8 = 8;
let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
let sample_phase = phase_offset
.wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
@ -99,24 +103,14 @@ const APP: () = {
.zip(Accu::new(sample_phase, sample_frequency))
// Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
.map(|(&sample, phase)| {
lockin.update((sample as i16 as i32) << 16, phase)
lockin.update(sample as i16, phase, time_constant)
})
// Decimate
.last()
.unwrap();
// convert i/q to power/phase,
let power_phase = true; // TODO: expose
let output = if power_phase {
// Convert from IQ to power and phase.
[output.abs_sqr(), output.arg()]
} else {
[output.0, output.1]
};
for value in dac_samples[1].iter_mut() {
*value = (output[1] >> 16) as u16 ^ 0x8000;
*value = (output.arg() >> 16) as u16 ^ 0x8000;
}
}