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

rpll: auto-align counter

Browse Source
This commit is contained in:
Robert Jördens 2021-01-26 22:30:46 +01:00
parent c769bdbd4c
commit 45e7d6de3c
2 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
dsp/src/rpll.rs
View File

@ -18,14 +18,12 @@ impl RPLL {
/// ///
/// Args: /// Args:
/// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio. /// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio.
/// * t: Counter time. Counter value at the first update() call. Typically 0.
/// ///
/// Returns: /// Returns:
/// Initialized RPLL instance. /// Initialized RPLL instance.
pub fn new(dt2: u8, t: i32) -> RPLL { pub fn new(dt2: u8) -> RPLL {
RPLL { RPLL {
dt2, dt2,
t,
..Default::default() ..Default::default()
} }
} }
@ -69,7 +67,19 @@ impl RPLL {
// Update frequency lock // Update frequency lock
self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig)); self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig));
// Time in counter cycles between timestamp and "now" // Time in counter cycles between timestamp and "now"
let dt = self.t.wrapping_sub(x); let mut dt = self.t.wrapping_sub(x);
if dt < 0 {
// Timestamp is in the future
// Advance phase and time until we are just past the most recent
// timestamp.
let mut dt_ceil = dt >> self.dt2;
self.y = self.y.wrapping_sub(self.f.wrapping_mul(dt_ceil));
dt_ceil <<= self.dt2;
self.t = self.t.wrapping_sub(dt_ceil);
dt = dt.wrapping_sub(dt_ceil);
}
// Reference phase estimate "now" // Reference phase estimate "now"
let y_ref = (self.f >> self.dt2).wrapping_mul(dt); let y_ref = (self.f >> self.dt2).wrapping_mul(dt);
// Phase error // Phase error

2
src/bin/lockin.rs
View File

@ -53,7 +53,7 @@ const APP: () = {
// Configure the microcontroller // Configure the microcontroller
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0); let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2);
let lockin = Lockin::new( let lockin = Lockin::new(
&iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose