WRPLL: add filter for DRTIO 100MHz

src/libboard_artiq/src/si549.rs

@@ -335,16 +335,44 @@ 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;
+    // y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2]
+    struct FilterParameters {
+        pub b0: f64,
+        pub b1: f64,
+        pub b2: f64,
+        pub a1: f64,
+        pub a2: f64,
+    }
+
+    #[cfg(rtio_frequency = "100.0")]
+    const LPF: FilterParameters = FilterParameters {
+        b0: 0.03967479060647884,
+        b1: 0.07934958121295768,
+        b2: 0.03967479060647884,
+        a1: -1.3865593741228928,
+        a2: 0.5452585365488082,
+    };
+
+    #[cfg(rtio_frequency = "125.0")]
+    const LPF: FilterParameters = FilterParameters {
+        b0: 0.07209205036273991,
+        b1: 0.14418410072547982,
+        b2: 0.07209205036273991,
+        a1: -0.6114078511562919,
+        a2: -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 +500,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 +551,14 @@ 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 = ((LPF.b0 * period_err as f64) + (LPF.b1 * PERIOD_ERR1 as f64) + (LPF.b2 * PERIOD_ERR2 as f64)
+                - (LPF.a1 * H_ADPLL1 as f64)
+                - (LPF.a2 * H_ADPLL2 as f64)) as i32;
             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 +566,14 @@ 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 = ((LPF.b0 * phase_err as f64) + (LPF.b1 * PHASE_ERR1 as f64) + (LPF.b2 * PHASE_ERR2 as f64)
+                - (LPF.a1 * M_ADPLL1 as f64)
+                - (LPF.a2 * M_ADPLL2 as f64)) as i32;
             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(())
     }