wavemeter: filter position

This commit is contained in:
Sebastien Bourdeauducq 2019-11-16 13:46:52 +08:00
parent 1cfb90760a
commit c48a30bb9b
4 changed files with 37 additions and 42 deletions

16
Cargo.lock generated
View File

@ -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"

View File

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

View File

@ -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 {
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;

View File

@ -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
}