lockin-internal: document, streamline sequence

This commit is contained in:
Robert Jördens 2021-02-01 15:59:11 +01:00
parent ab7e3d229b
commit 656e3253ab
3 changed files with 24 additions and 24 deletions

View File

@ -17,7 +17,7 @@ use stabilizer::{hardware, server};
use dsp::iir; use dsp::iir;
use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
const SCALE: f32 = ((1 << 15) - 1) as f32; const SCALE: f32 = i16::MAX as f32;
const TCP_RX_BUFFER_SIZE: usize = 8192; const TCP_RX_BUFFER_SIZE: usize = 8192;
const TCP_TX_BUFFER_SIZE: usize = 8192; const TCP_TX_BUFFER_SIZE: usize = 8192;
@ -36,7 +36,7 @@ const APP: () = {
// Format: iir_state[ch][cascade-no][coeff] // Format: iir_state[ch][cascade-no][coeff]
#[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])] #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2], iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
#[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] #[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE, y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
} }

View File

@ -21,7 +21,7 @@ use hardware::{
Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
}; };
const SCALE: f32 = ((1 << 15) - 1) as f32; const SCALE: f32 = i16::MAX as f32;
const TCP_RX_BUFFER_SIZE: usize = 8192; const TCP_RX_BUFFER_SIZE: usize = 8192;
const TCP_TX_BUFFER_SIZE: usize = 8192; const TCP_TX_BUFFER_SIZE: usize = 8192;
@ -40,7 +40,7 @@ const APP: () = {
// Format: iir_state[ch][cascade-no][coeff] // Format: iir_state[ch][cascade-no][coeff]
#[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])] #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2], iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
#[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] #[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE, y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
timestamper: InputStamper, timestamper: InputStamper,
@ -158,9 +158,9 @@ const APP: () = {
for i in 0..dac_samples[0].len() { for i in 0..dac_samples[0].len() {
unsafe { unsafe {
dac_samples[0][i] = dac_samples[0][i] =
(power.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000; power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
dac_samples[1][i] = dac_samples[1][i] =
(phase.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000; phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
} }
} }
} }

View File

@ -3,13 +3,16 @@
#![no_main] #![no_main]
#![cfg_attr(feature = "nightly", feature(core_intrinsics))] #![cfg_attr(feature = "nightly", feature(core_intrinsics))]
// A constant sinusoid to send on the DAC output.
const DAC_SEQUENCE: [f32; 8] =
[0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
use dsp::{iir_int, lockin::Lockin, Accu}; use dsp::{iir_int, lockin::Lockin, Accu};
use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
use stabilizer::hardware; use stabilizer::{hardware, SAMPLE_BUFFER_SIZE, SAMPLE_BUFFER_SIZE_LOG2};
// A constant sinusoid to send on the DAC output.
// Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V.
const ONE: i16 = (0.1 * u16::MAX as f32) as _;
const SQRT2: i16 = (ONE as f32 * 0.707) as _;
const DAC_SEQUENCE: [i16; SAMPLE_BUFFER_SIZE] =
[0, SQRT2, ONE, SQRT2, 0, -SQRT2, -ONE, -SQRT2]; // TODO: rotate by -2 samples to start with ONE
#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
const APP: () = { const APP: () = {
@ -74,36 +77,33 @@ const APP: () = {
]; ];
// DAC0 always generates a fixed sinusoidal output. // DAC0 always generates a fixed sinusoidal output.
for (i, value) in DAC_SEQUENCE.iter().enumerate() { dac_samples[0]
// Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V. .iter_mut()
let y = value * (0.1 * i16::MAX as f32); .zip(DAC_SEQUENCE.iter())
// Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized. .for_each(|(d, s)| *d = *s as u16 ^ 0x8000);
let y = unsafe { y.to_int_unchecked::<i16>() };
// Convert to DAC code
dac_samples[0][i] = y as u16 ^ 0x8000;
}
// Reference phase and frequency are known.
let pll_phase = 0; let pll_phase = 0;
// 1/8 of the sample rate: log2(DAC_SEQUENCE.len()) == 3 let pll_frequency = 1i32 << (32 - SAMPLE_BUFFER_SIZE_LOG2);
let pll_frequency = 1i32 << (32 - 3);
// Harmonic index of the LO: -1 to _de_modulate the fundamental // Harmonic index of the LO: -1 to _de_modulate the fundamental
let harmonic: i32 = -1; let harmonic: i32 = -1;
// Demodulation LO phase offset // Demodulation LO phase offset
let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32; let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32; // TODO: adapt to sequence rotation above
let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic); let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
let sample_phase = phase_offset let sample_phase = phase_offset
.wrapping_add((pll_phase as i32).wrapping_mul(harmonic)); .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
if let Some(output) = adc_samples if let Some(output) = adc_samples
.iter() .iter()
// Zip in the LO phase.
.zip(Accu::new(sample_phase, sample_frequency)) .zip(Accu::new(sample_phase, sample_frequency))
// Convert to signed, MSB align the ADC sample. // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
.map(|(&sample, phase)| { .map(|(&sample, phase)| {
lockin.update((sample as i16 as i32) << 16, phase) lockin.update((sample as i16 as i32) << 16, phase)
}) })
// Decimate
.last() .last()
{ {
// Convert from IQ to power and phase. // Convert from IQ to power and phase.