101 lines
3.2 KiB
Rust
101 lines
3.2 KiB
Rust
use super::{divide_round, pll::PLL};
|
|
|
|
/// Processes external timestamps to produce the frequency and initial phase 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,
|
|
adc_sample_ticks_log2: usize,
|
|
sample_buffer_size_log2: usize,
|
|
}
|
|
|
|
impl TimestampHandler {
|
|
/// Construct a new `TimestampHandler` instance.
|
|
///
|
|
/// # Args
|
|
/// * `pll_shift_frequency` - See `PLL::update()`.
|
|
/// * `pll_shift_phase` - See `PLL::update()`.
|
|
/// * `adc_sample_ticks_log2` - Number of ticks in one ADC sampling timer period.
|
|
/// * `sample_buffer_size_log2` - Number of ADC samples in one processing batch.
|
|
///
|
|
/// # Returns
|
|
/// New `TimestampHandler` instance.
|
|
pub fn new(
|
|
pll_shift_frequency: u8,
|
|
pll_shift_phase: u8,
|
|
adc_sample_ticks_log2: usize,
|
|
sample_buffer_size_log2: usize,
|
|
) -> Self {
|
|
TimestampHandler {
|
|
pll: PLL::default(),
|
|
pll_shift_frequency,
|
|
pll_shift_phase,
|
|
batch_index: 0,
|
|
reference_phase: 0,
|
|
reference_frequency: 0,
|
|
adc_sample_ticks_log2,
|
|
sample_buffer_size_log2,
|
|
}
|
|
}
|
|
|
|
/// Compute the initial phase 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: u32;
|
|
let demodulation_initial_phase: u32;
|
|
|
|
if self.reference_frequency == 0 {
|
|
demodulation_frequency = u32::MAX;
|
|
demodulation_initial_phase = u32::MAX;
|
|
} else {
|
|
demodulation_frequency = divide_round(
|
|
1 << (32 + self.adc_sample_ticks_log2),
|
|
self.reference_frequency,
|
|
) as u32;
|
|
demodulation_initial_phase = divide_round(
|
|
(((self.batch_index as i64)
|
|
<< (self.adc_sample_ticks_log2
|
|
+ self.sample_buffer_size_log2))
|
|
- self.reference_phase)
|
|
<< 32,
|
|
self.reference_frequency,
|
|
) as u32;
|
|
}
|
|
|
|
if self.batch_index
|
|
< (1 << (32
|
|
- self.adc_sample_ticks_log2
|
|
- self.sample_buffer_size_log2))
|
|
- 1
|
|
{
|
|
self.batch_index += 1;
|
|
} else {
|
|
self.batch_index = 0;
|
|
self.reference_phase -= 1 << 32;
|
|
}
|
|
|
|
(demodulation_initial_phase, demodulation_frequency)
|
|
}
|
|
}
|