wavemeter: filter position
This commit is contained in:
parent
1cfb90760a
commit
c48a30bb9b
16
Cargo.lock
generated
16
Cargo.lock
generated
@ -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"
|
||||
|
||||
@ -18,3 +18,4 @@ serde = "1.0"
|
||||
serde_derive = "1.0"
|
||||
serde_json = "1.0"
|
||||
argparse = "0.2.2"
|
||||
biquad = "0.3.0"
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user