wavemeter: filter position

Cargo.lock

@@ -10,16 +10,30 @@ name = "autocfg"
 version = "0.1.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 
+[[package]]
+name = "biquad"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "libm 0.1.4 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
 [[package]]
 name = "itoa"
 version = "0.4.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 
+[[package]]
+name = "libm"
+version = "0.1.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+
 [[package]]
 name = "noptica-rs"
 version = "0.1.0"
 dependencies = [
  "argparse 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)",
+ "biquad 0.3.0 (registry+https://github.com/rust-lang/crates.io-index)",
  "num-traits 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)",
  "serde 1.0.99 (registry+https://github.com/rust-lang/crates.io-index)",
  "serde_derive 1.0.99 (registry+https://github.com/rust-lang/crates.io-index)",
@@ -98,7 +112,9 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 [metadata]
 "checksum argparse 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)" = "3f8ebf5827e4ac4fd5946560e6a99776ea73b596d80898f357007317a7141e47"
 "checksum autocfg 0.1.6 (registry+https://github.com/rust-lang/crates.io-index)" = "b671c8fb71b457dd4ae18c4ba1e59aa81793daacc361d82fcd410cef0d491875"
+"checksum biquad 0.3.0 (registry+https://github.com/rust-lang/crates.io-index)" = "1a97c1c98ab5d392fe83bfd084e9290199037f1fdd6228fea9fd1701be254445"
 "checksum itoa 0.4.4 (registry+https://github.com/rust-lang/crates.io-index)" = "501266b7edd0174f8530248f87f99c88fbe60ca4ef3dd486835b8d8d53136f7f"
+"checksum libm 0.1.4 (registry+https://github.com/rust-lang/crates.io-index)" = "7fc7aa29613bd6a620df431842069224d8bc9011086b1db4c0e0cd47fa03ec9a"
 "checksum num-traits 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)" = "6ba9a427cfca2be13aa6f6403b0b7e7368fe982bfa16fccc450ce74c46cd9b32"
 "checksum proc-macro2 1.0.1 (registry+https://github.com/rust-lang/crates.io-index)" = "4c5c2380ae88876faae57698be9e9775e3544decad214599c3a6266cca6ac802"
 "checksum quote 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)" = "053a8c8bcc71fcce321828dc897a98ab9760bef03a4fc36693c231e5b3216cfe"

Cargo.toml

@@ -18,3 +18,4 @@ serde = "1.0"
 serde_derive = "1.0"
 serde_json = "1.0"
 argparse = "0.2.2"
+biquad = "0.3.0"

src/wavemeter.rs

@@ -2,10 +2,11 @@ extern crate argparse;
 extern crate num_traits;
 extern crate serde_derive;
 extern crate serde_json;
+extern crate biquad;
 
 use argparse::{ArgumentParser, StoreTrue, Store};
-
 use serde_derive::Deserialize;
+use biquad::Biquad;
 
 use std::error::Error;
 use std::fs::File;
@@ -37,7 +38,9 @@ struct Config {
     ref_wavelength: f64,    // Wavelength of the reference laser in m.
 
     position_mon_time: f64, // The time during which position is monitored to compute min/max
-    duty_cycle: f64         // Fraction of the scan used for counting input laser fringes
+    duty_cycle: f64,        // Fraction of the scan used for counting input laser fringes
+
+    motion_cutoff: f64,     // Cut-off frequency of the motion filter
 }
 
 fn read_config_from_file<P: AsRef<Path>>(path: P) -> Result<Config, Box<dyn Error>> {
@@ -111,36 +114,6 @@ fn do_calibrate(config: &Config) {
     })
 }
 
-
-struct MotionTracker {
-    last_position: i64,
-    last_position_ago: u32,
-    speed: i64
-}
-
-impl MotionTracker {
-    pub fn new() -> MotionTracker {
-        MotionTracker {
-            last_position: 0,
-            last_position_ago: 0,
-            speed: 0
-        }
-    }
-
-    pub fn extrapolated_position(&self) -> i64 {
-        self.last_position + self.speed*(self.last_position_ago as i64)
-    }
-
-    pub fn tick(&mut self, position: Option<i64>) {
-        self.last_position_ago += 1;
-        if let Some(position) = position {
-            self.speed = (position - self.last_position)/(self.last_position_ago as i64);
-            self.last_position = position;
-            self.last_position_ago = 0;
-        }
-    }
-}
-
 fn do_wavemeter(config: &Config) {
     let mut refpll = noptica::Dpll::new(
         noptica::Dpll::frequency_to_ftw(config.ref_min, config.sample_rate),
@@ -148,9 +121,15 @@ fn do_wavemeter(config: &Config) {
         config.refpll_ki,
         config.refpll_kp);
     let mut position_tracker = noptica::PositionTracker::new();
-    let mut motion_tracker = MotionTracker::new();
+    let mut position = 0;
     let mut min_max_monitor = MinMaxMonitor::new(
         ((config.ref_min + config.ref_max)/2.0*config.position_mon_time) as u32);
+    let motion_filter_coeffs = biquad::Coefficients::<f64>::from_params(
+        biquad::Type::LowPass,
+        biquad::frequency::Hertz::<f64>::from_hz(config.sample_rate).unwrap(),
+        biquad::frequency::Hertz::<f64>::from_hz(config.motion_cutoff).unwrap(),
+        biquad::Q_BUTTERWORTH_F64).unwrap();
+    let mut motion_filter = biquad::DirectForm2Transposed::<f64>::new(motion_filter_coeffs);
 
     // Update duty_min and duty_max when the position is near the middle
     // to avoid glitches.
@@ -170,9 +149,8 @@ fn do_wavemeter(config: &Config) {
     noptica::sample(&config.sample_command, |rising, _falling| {
         refpll.tick(rising & (1 << config.bit_ref) != 0);
         if refpll.locked() {
-            let position_opt;
             if rising & (1 << config.bit_meas) != 0 {
-                let position = position_tracker.edge(refpll.get_phase_unwrapped());
+                position = position_tracker.edge(refpll.get_phase_unwrapped());
                 min_max_monitor.input(position, |position_min, position_max| {
                     let amplitude = position_max - position_min;
                     let off_duty = ((amplitude as f64)*(1.0 - config.duty_cycle)) as i64;
@@ -186,14 +164,11 @@ fn do_wavemeter(config: &Config) {
                     duty_max = new_duty_max;
                 }
                 prev_position_above_middle = position_above_middle;
-                position_opt = Some(position);
+            }
-            } else {
+            let filtered_position = motion_filter.run(position as f64) as i64;
-                position_opt = None
-            };
-            motion_tracker.tick(position_opt);
 
             if rising & (1 << config.bit_input) != 0 {
-                let fringe_position = motion_tracker.extrapolated_position();
+                let fringe_position = filtered_position;
                 let in_duty = (duty_min < fringe_position) && (fringe_position < duty_max);
                 if in_duty & !prev_in_duty {
                     first_fringe = fringe_position;
@@ -213,6 +188,7 @@ fn do_wavemeter(config: &Config) {
                 prev_in_duty = in_duty;
             }
         } else {
+            position = 0;
             min_max_monitor.reset();
 
             prev_position_above_middle = false;

wavemeter.json

@@ -13,5 +13,7 @@
     "ref_wavelength": 632.991372e-9,
 
     "position_mon_time": 0.25,
-    "duty_cycle": 0.9
+    "duty_cycle": 0.9,
+
+    "motion_cutoff": 100e3
 }