move timestamp handling into new TimestampHandler struct
This commit is contained in:
Matt Huszagh
parent
e14aa8b613
commit
4c033c0f3e
137
src/main.rs
137
src/main.rs
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue