From c48a30bb9bd3a9e3f349b7594a462eeeaaec705c Mon Sep 17 00:00:00 2001 From: Sebastien Bourdeauducq Date: Sat, 16 Nov 2019 13:46:52 +0800 Subject: [PATCH] wavemeter: filter position --- Cargo.lock | 16 +++++++++++++ Cargo.toml | 1 + src/wavemeter.rs | 58 ++++++++++++++---------------------------------- wavemeter.json | 4 +++- 4 files changed, 37 insertions(+), 42 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index e39e191..6bf312d 100644 --- a/Cargo.lock +++ b/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" diff --git a/Cargo.toml b/Cargo.toml index 072e2df..4b2a24b 100644 --- a/Cargo.toml +++ b/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" diff --git a/src/wavemeter.rs b/src/wavemeter.rs index 462c29f..2d4e069 100644 --- a/src/wavemeter.rs +++ b/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>(path: P) -> Result> { @@ -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) { - 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::::from_params( + biquad::Type::LowPass, + biquad::frequency::Hertz::::from_hz(config.sample_rate).unwrap(), + biquad::frequency::Hertz::::from_hz(config.motion_cutoff).unwrap(), + biquad::Q_BUTTERWORTH_F64).unwrap(); + let mut motion_filter = biquad::DirectForm2Transposed::::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 { - position_opt = None - }; - motion_tracker.tick(position_opt); + } + let filtered_position = motion_filter.run(position as f64) as i64; 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; diff --git a/wavemeter.json b/wavemeter.json index 7f4745d..0ca983e 100644 --- a/wavemeter.json +++ b/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 }