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

lockin: refactor Lockin

master
Robert Jördens 2021-01-21 14:55:33 +01:00
parent 5af2b9c63a
commit 948e58c910
5 changed files with 119 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
dsp/src/iir_int.rs
View File

@ -3,6 +3,20 @@ use serde::{Deserialize, Serialize};
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct IIRState(pub [i32; 5]);
impl IIRState {
#[inline(always)]
pub fn get_x(&self, index: usize) -> i32 {
// x0 is at index 0 in a biquad between updates
self.0[index]
}
#[inline(always)]
pub fn get_y(&self, index: usize) -> i32 {
// y0 is at index 2 in a biquad between updates
self.0[2 + index]
}
}
fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
// Rounding bias, half up
let y0 = ((y0 as i64) << shift) + (1 << (shift - 1));

1
dsp/src/lib.rs
View File

@ -118,6 +118,7 @@ where
pub mod iir;
pub mod iir_int;
pub mod lockin;
pub mod pll;
pub mod reciprocal_pll;
pub mod trig;

55
dsp/src/lockin.rs Normal file
View File

@ -0,0 +1,55 @@
use super::{
iir_int,
trig::{atan2, cossin},
Complex,
};
use serde::{Deserialize, Serialize};
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct Lockin {
iir: iir_int::IIR,
iir_state: [iir_int::IIRState; 2],
}
impl Lockin {
pub fn new(_corner: u8) -> Self {
Lockin {
iir: iir_int::IIR::default(), // TODO: lowpass coefficients from corner
iir_state: [iir_int::IIRState::default(); 2],
}
}
pub fn update(&mut self, signal: i32, phase: i32) -> Complex<i32> {
// Get the LO signal for demodulation.
let m = cossin(phase.wrapping_neg());
// Mix with the LO signal, filter with the IIR lowpass,
// return IQ (in-phase and quadrature) data.
// Note: 32x32 -> 64 bit multiplications are pretty much free.
Complex(
self.iir.update(
&mut self.iir_state[0],
((signal as i64 * m.0 as i64) >> 32) as _,
),
self.iir.update(
&mut self.iir_state[1],
((signal as i64 * m.1 as i64) >> 32) as _,
),
)
}
pub fn iq(&self) -> Complex<i32> {
Complex(self.iir_state[0].get_y(0), self.iir_state[1].get_y(0))
}
pub fn power(&self) -> i32 {
let iq = self.iq();
(((iq.0 as i64) * (iq.0 as i64) + (iq.1 as i64) * (iq.1 as i64)) >> 32)
as i32
}
pub fn phase(&self) -> i32 {
let iq = self.iq();
atan2(iq.1, iq.0)
}
}

97
src/bin/lockin.rs
View File

@ -12,25 +12,15 @@ use rtic::cyccnt::{Instant, U32Ext};
use heapless::{consts::*, String};
use stabilizer::{hardware, server};
use dsp::{
iir, iir_int,
reciprocal_pll::TimestampHandler,
trig::{atan2, cossin},
Complex,
use stabilizer::{
hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
};
use dsp::{iir, lockin::Lockin, reciprocal_pll::TimestampHandler};
use hardware::{
Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
};
// Frequency scaling factor for lock-in harmonic demodulation.
const HARMONIC: u32 = 1;
// Phase offset applied to the lock-in demodulation signal.
const PHASE_OFFSET: u32 = 0;
const ADC_SAMPLE_TICKS_LOG2: u16 = 8;
const SAMPLE_BUFFER_SIZE_LOG2: usize = 3;
const SCALE: f32 = ((1 << 15) - 1) as f32;
const TCP_RX_BUFFER_SIZE: usize = 8192;
@ -54,9 +44,8 @@ const APP: () = {
iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
timestamper: InputStamper,
timestamp_handler: TimestampHandler,
iir_lockin: iir_int::IIR,
iir_state_lockin: [iir_int::IIRState; 2],
pll: TimestampHandler,
lockin: Lockin,
}
#[init]
@ -64,14 +53,16 @@ const APP: () = {
// Configure the microcontroller
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
let timestamp_handler = TimestampHandler::new(
4,
3,
let pll = TimestampHandler::new(
4, // relative PLL frequency bandwidth: 2**-4, TODO: expose
3, // relative PLL phase bandwidth: 2**-3, TODO: expose
ADC_SAMPLE_TICKS_LOG2 as usize,
SAMPLE_BUFFER_SIZE_LOG2,
);
let iir_lockin = iir_int::IIR::default();
let iir_state_lockin = [iir_int::IIRState::default(); 2];
let lockin = Lockin::new(
10, // relative Locking lowpass filter bandwidth, TODO: expose
);
// Enable ADC/DAC events
stabilizer.adcs.0.start();
@ -82,6 +73,9 @@ const APP: () = {
// Start sampling ADCs.
stabilizer.adc_dac_timer.start();
// Start recording digital input timestamps.
stabilizer.timestamp_timer.start();
init::LateResources {
afes: stabilizer.afes,
adcs: stabilizer.adcs,
@ -89,9 +83,8 @@ const APP: () = {
net_interface: stabilizer.net.interface,
timestamper: stabilizer.timestamper,
timestamp_handler,
iir_lockin,
iir_state_lockin,
pll,
lockin,
}
}
@ -111,7 +104,9 @@ const APP: () = {
///
/// Because the ADC and DAC operate at the same rate, these two constraints actually implement
/// the same time bounds, meeting one also means the other is also met.
#[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, iir_lockin, iir_state_lockin, timestamp_handler, timestamper], priority=2)]
///
/// TODO: document lockin
#[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, lockin, timestamper, pll], priority=2)]
fn process(c: process::Context) {
let adc_samples = [
c.resources.adcs.0.acquire_buffer(),
@ -123,49 +118,47 @@ const APP: () = {
c.resources.dacs.1.acquire_buffer(),
];
let iir_lockin = c.resources.iir_lockin;
let iir_state_lockin = c.resources.iir_state_lockin;
let iir_ch = c.resources.iir_ch;
let iir_state = c.resources.iir_state;
let lockin = c.resources.lockin;
let (pll_phase, pll_frequency) = c
.resources
.timestamp_handler
.pll
.update(c.resources.timestamper.latest_timestamp());
let frequency = pll_frequency.wrapping_mul(HARMONIC);
let mut phase =
PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC));
// Harmonic index to demodulate
let harmonic: i32 = -1;
// Demodulation LO phase offset
let phase_offset: i32 = 0;
let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
let mut sample_phase = phase_offset
.wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
for i in 0..adc_samples[0].len() {
let m = cossin((phase as i32).wrapping_neg());
phase = phase.wrapping_add(frequency);
// Convert to signed, MSB align the ADC sample.
let input = (adc_samples[0][i] as i16 as i32) << 16;
// Obtain demodulated, filtered IQ sample.
lockin.update(input, sample_phase);
// Advance the sample phase.
sample_phase = sample_phase.wrapping_add(sample_frequency);
let signal = (adc_samples[0][i] as i16 as i32) << 16;
let signal = Complex(
iir_lockin.update(
&mut iir_state_lockin[0],
((signal as i64 * m.0 as i64) >> 32) as _,
),
iir_lockin.update(
&mut iir_state_lockin[1],
((signal as i64 * m.1 as i64) >> 16) as _,
),
);
let mut magnitude =
(signal.0 * signal.0 + signal.1 * signal.1) as _;
let mut phase = atan2(signal.1, signal.0) as _;
// Convert from IQ to power and phase.
let mut power = lockin.power() as _;
let mut phase = lockin.phase() as _;
// Filter power and phase through IIR filters.
// Note: Normalization to be done in filters. Phase will wrap happily.
for j in 0..iir_state[0].len() {
magnitude =
iir_ch[0][j].update(&mut iir_state[0][j], magnitude);
power = iir_ch[0][j].update(&mut iir_state[0][j], power);
phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
}
// Note(unsafe): range clipping to i16 is ensured by IIR filters above.
// Convert to DAC data.
unsafe {
dac_samples[0][i] =
magnitude.to_int_unchecked::<i16>() as u16 ^ 0x8000;
power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
dac_samples[1][i] =
phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
}

6
src/lib.rs
View File

@ -9,7 +9,9 @@ pub mod server;
// The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is
// equal to 10ns per tick.
// Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz
const ADC_SAMPLE_TICKS: u16 = 256;
pub const ADC_SAMPLE_TICKS_LOG2: u16 = 8;
pub const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2;
// The desired ADC sample processing buffer size.
const SAMPLE_BUFFER_SIZE: usize = 8;
pub const SAMPLE_BUFFER_SIZE_LOG2: usize = 3;
pub const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2;