move timestamp handling into new TimestampHandler struct

This commit is contained in:
Matt Huszagh 2021-01-12 13:04:40 -08:00
parent e14aa8b613
commit 4c033c0f3e
1 changed files with 84 additions and 53 deletions

View File

@ -68,6 +68,7 @@ const SAMPLE_BUFFER_SIZE_LOG2: usize = 3;
const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2;
// The number of ADC batches in one timer overflow period.
// TODO almost the same as `calculate_timestamp_timer_period`.
pub const ADC_BATCHES_LOG2: usize =
32 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_SAMPLE_TICKS_LOG2 as usize;
pub const ADC_BATCHES: usize = 1 << ADC_BATCHES_LOG2;
@ -76,18 +77,11 @@ pub const ADC_BATCHES: usize = 1 << ADC_BATCHES_LOG2;
const IIR_CASCADE_LENGTH: usize = 1;
// TODO should these be global consts?
// 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;
// The PLL locks to an external reference signal. See `PLL::update()`
// for a description of shift_frequency and shift_phase.
const PLL_SHIFT_FREQUENCY: u8 = 4;
const PLL_SHIFT_PHASE: u8 = 3;
#[link_section = ".sram3.eth"]
static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new();
@ -169,6 +163,79 @@ type AFE1 = afe::ProgrammableGainAmplifier<
hal::gpio::gpiod::PD15<hal::gpio::Output<hal::gpio::PushPull>>,
>;
/// Locks a PLL to an external reference and computes the initial phase value and frequency of the
/// demodulation signal.
pub struct TimestampHandler {
pll: PLL,
pll_shift_frequency: u8,
pll_shift_phase: u8,
// Index of the current ADC batch.
batch_index: u32,
// Most recent phase and frequency values of the external reference.
reference_phase: i64,
reference_frequency: i64,
}
impl TimestampHandler {
/// Construct a new `TimestampHandler` instance.
///
/// # Args
/// * `pll_shift_frequency` - See `PLL::update()`.
/// * `pll_shift_phase` - See `PLL::update()`.
///
/// # Returns
/// New `TimestampHandler` instance.
pub fn new(pll_shift_frequency: u8, pll_shift_phase: u8) -> Self {
TimestampHandler {
pll: PLL::default(),
pll_shift_frequency,
pll_shift_phase,
batch_index: 0,
reference_phase: 0,
reference_frequency: 0,
}
}
/// Compute the initial phase value and frequency of the demodulation signal.
///
/// # Args
/// * `timestamp` - Counter value corresponding to an external reference edge.
///
/// # Returns
/// Tuple consisting of the initial phase value and frequency of the demodulation signal.
pub fn update(&mut self, timestamp: Option<u32>) -> (u32, u32) {
if let Some(t) = timestamp {
let (phase, frequency) = self.pll.update(
t as i32,
self.pll_shift_frequency,
self.pll_shift_phase,
);
self.reference_phase = phase as u32 as i64;
self.reference_frequency = frequency as u32 as i64;
}
let demodulation_frequency = divide_round(
1 << (64 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_BATCHES_LOG2),
self.reference_frequency,
) as u32;
let demodulation_initial_phase = divide_round(
(((self.batch_index as i64) << (32 - ADC_BATCHES_LOG2))
- self.reference_phase)
<< 32,
self.reference_frequency,
) as u32;
if self.batch_index < ADC_BATCHES as u32 - 1 {
self.batch_index += 1;
} else {
self.batch_index = 0;
self.reference_phase -= 1 << 32;
}
(demodulation_initial_phase, demodulation_frequency)
}
}
macro_rules! route_request {
($request:ident,
readable_attributes: [$($read_attribute:tt: $getter:tt),*],
@ -229,10 +296,7 @@ const APP: () = {
adcs: (Adc0Input, Adc1Input),
dacs: (Dac0Output, Dac1Output),
input_stamper: digital_input_stamper::InputStamper,
pll: PLL,
batch_index: u32,
reference_phase: i64,
reference_frequency: i64,
timestamp_handler: TimestampHandler,
iir_lockin: iir_int::IIR,
iir_state_lockin: [iir_int::IIRState; 2],
@ -957,10 +1021,7 @@ const APP: () = {
#[cfg(not(feature = "pounder_v1_1"))]
let pounder_stamper = None;
let pll = PLL::default();
let batch_index = 0;
let reference_phase = 0;
let reference_frequency = 0;
let timestamp_handler = TimestampHandler::new(4, 3);
let iir_lockin = iir_int::IIR { ba: [0; 5] };
let iir_state_lockin = [[0; 5]; 2];
@ -979,10 +1040,7 @@ const APP: () = {
pounder: pounder_devices,
pounder_stamper,
pll,
batch_index,
reference_phase,
reference_frequency,
timestamp_handler,
iir_lockin,
iir_state_lockin,
@ -993,7 +1051,7 @@ const APP: () = {
}
}
#[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper, pll, iir_lockin, iir_state_lockin, batch_index, reference_phase, reference_frequency], priority=2)]
#[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper, timestamp_handler, iir_lockin, iir_state_lockin], priority=2)]
fn process(c: process::Context) {
if let Some(stamper) = c.resources.pounder_stamper {
let pounder_timestamps = stamper.acquire_buffer();
@ -1009,37 +1067,10 @@ const APP: () = {
c.resources.dacs.1.acquire_buffer(),
];
let reference_phase = c.resources.reference_phase;
let reference_frequency = c.resources.reference_frequency;
if let Some(t) = c.resources.input_stamper.latest_timestamp() {
let res = c.resources.pll.update(
t as i32,
PLL_SHIFT_FREQUENCY,
PLL_SHIFT_PHASE,
);
*reference_phase = res.0 as u32 as i64;
*reference_frequency = res.1 as u32 as i64;
}
let demodulation_frequency = divide_round(
1 << (64 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_BATCHES_LOG2),
*reference_frequency,
) as u32;
let batch_index = c.resources.batch_index;
let demodulation_initial_phase = divide_round(
(((*batch_index as i64) << (32 - ADC_BATCHES_LOG2))
- *reference_phase)
<< 32,
*reference_frequency,
) as u32;
if *batch_index < ADC_BATCHES as u32 - 1 {
*batch_index += 1;
} else {
*batch_index = 0;
*reference_phase -= 1 << 32;
}
let (demodulation_initial_phase, demodulation_frequency) = c
.resources
.timestamp_handler
.update(c.resources.input_stamper.latest_timestamp());
let [dac0, dac1] = dac_samples;
let iir_lockin = c.resources.iir_lockin;
@ -1064,10 +1095,10 @@ const APP: () = {
signal.0 =
iir_lockin.update(&mut iir_state_lockin[0], signal.0);
signal.1 =
iir_lockin.update(&mut iir_state_lockin[1], signal.0);
iir_lockin.update(&mut iir_state_lockin[1], signal.1);
let magnitude = signal.0 * signal.0 + signal.1 * signal.1;
let phase = atan2(signal.0, signal.0);
let phase = atan2(signal.1, signal.0);
*d0 = (magnitude >> 16) as i16 as u16;
*d1 = (phase >> 16) as i16 as u16;