From 84097881586890fb822911016499fdb05b8714c7 Mon Sep 17 00:00:00 2001 From: morgan Date: Tue, 7 May 2024 16:15:31 +0800 Subject: [PATCH] WRPLL: replace PI controller with low pass filter --- src/libboard_artiq/src/si549.rs | 60 +++++++++++++++++++++++---------- 1 file changed, 42 insertions(+), 18 deletions(-) diff --git a/src/libboard_artiq/src/si549.rs b/src/libboard_artiq/src/si549.rs index 286d462..6384c57 100644 --- a/src/libboard_artiq/src/si549.rs +++ b/src/libboard_artiq/src/si549.rs @@ -335,16 +335,28 @@ pub mod wrpll { const COUNTER_WIDTH: u32 = 24; const DIV_WIDTH: u32 = 2; - const KP: i32 = 6; - const KI: i32 = 2; // 4 ppm capture range const ADPLL_LIM: i32 = (4.0 / 0.0001164) as i32; + // Low pass filter + // y[n] = B0*x[n] + B1*x[n-1] + B2*x[n-2] - A1*y[n-1] - A2*y[n-2] + const B0: f64 = 0.07209205036273991; + const B1: f64 = 0.14418410072547982; + const B2: f64 = 0.07209205036273991; + + const A1: f64 = -0.6114078511562919; + const A2: f64 = -0.10022394739274834; + + static mut H_ADPLL1: i32 = 0; + static mut H_ADPLL2: i32 = 0; + static mut PERIOD_ERR1: i32 = 0; + static mut PERIOD_ERR2: i32 = 0; + + static mut M_ADPLL1: i32 = 0; + static mut M_ADPLL2: i32 = 0; + static mut PHASE_ERR1: i32 = 0; + static mut PHASE_ERR2: i32 = 0; static mut BASE_ADPLL: i32 = 0; - static mut H_LAST_ADPLL: i32 = 0; - static mut LAST_PERIOD_ERR: i32 = 0; - static mut M_LAST_ADPLL: i32 = 0; - static mut LAST_PHASE_ERR: i32 = 0; #[derive(Clone, Copy)] pub enum ISR { @@ -472,10 +484,14 @@ pub mod wrpll { fn reset_plls(timer: &mut GlobalTimer) -> Result<(), &'static str> { unsafe { - H_LAST_ADPLL = 0; - LAST_PERIOD_ERR = 0; - M_LAST_ADPLL = 0; - LAST_PHASE_ERR = 0; + H_ADPLL1 = 0; + H_ADPLL2 = 0; + PERIOD_ERR1 = 0; + PERIOD_ERR2 = 0; + M_ADPLL1 = 0; + M_ADPLL2 = 0; + PHASE_ERR1 = 0; + PHASE_ERR2 = 0; } set_adpll(i2c::DCXO::Main, 0)?; set_adpll(i2c::DCXO::Helper, 0)?; @@ -519,11 +535,15 @@ pub mod wrpll { fn helper_pll() -> Result<(), &'static str> { let period_err = tag_collector::get_period_error(); unsafe { - // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw?view#Integral-wind-up-and-output-limiting - let adpll = (H_LAST_ADPLL + (KP + KI) * period_err - KP * LAST_PERIOD_ERR).clamp(-ADPLL_LIM, ADPLL_LIM); + let adpll = (((B0 * period_err as f64) + (B1 * PERIOD_ERR1 as f64) + (B2 * PERIOD_ERR2 as f64) + - (A1 * H_ADPLL1 as f64) + - (A2 * H_ADPLL2 as f64)) as i32) + .clamp(-ADPLL_LIM, ADPLL_LIM); set_adpll(i2c::DCXO::Helper, BASE_ADPLL + adpll)?; - H_LAST_ADPLL = adpll; - LAST_PERIOD_ERR = period_err; + H_ADPLL2 = H_ADPLL1; + PERIOD_ERR2 = PERIOD_ERR1; + H_ADPLL1 = adpll; + PERIOD_ERR1 = period_err; }; Ok(()) } @@ -531,11 +551,15 @@ pub mod wrpll { fn main_pll() -> Result<(), &'static str> { let phase_err = tag_collector::get_phase_error(); unsafe { - // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw?view#Integral-wind-up-and-output-limiting - let adpll = (M_LAST_ADPLL + (KP + KI) * phase_err - KP * LAST_PHASE_ERR).clamp(-ADPLL_LIM, ADPLL_LIM); + let adpll = (((B0 * phase_err as f64) + (B1 * PHASE_ERR1 as f64) + (B2 * PHASE_ERR2 as f64) + - (A1 * M_ADPLL1 as f64) + - (A2 * M_ADPLL2 as f64)) as i32) + .clamp(-ADPLL_LIM, ADPLL_LIM); set_adpll(i2c::DCXO::Main, BASE_ADPLL + adpll)?; - M_LAST_ADPLL = adpll; - LAST_PHASE_ERR = phase_err; + M_ADPLL2 = M_ADPLL1; + PHASE_ERR2 = PHASE_ERR1; + M_ADPLL1 = adpll; + PHASE_ERR1 = phase_err; }; Ok(()) }