wavemeter: new algo

src/noptica.rs

@@ -12,6 +12,9 @@ pub struct Dpll {
 
     phase: i64,
     phase_unwrapped: i64,
+
+    was_locked: bool,
+    wait_lock: Option<u32>
 }
 
 impl Dpll {
@@ -30,7 +33,9 @@ impl Dpll {
             ftw: init_ftw,
             integrator: init_ftw,
             phase: 0,
-            phase_unwrapped: 0
+            phase_unwrapped: 0,
+            was_locked: false,
+            wait_lock: Some(0)
         }
     }
 
@@ -47,12 +52,36 @@ impl Dpll {
                 self.ftw_min, self.ftw_max);
             self.ftw = clamp(self.integrator + (pe*self.kp >> 32),
                 self.ftw_min, self.ftw_max);
+
+            if pe.abs() <= (self.ftw + self.ftw/3) {
+                if let Some(wait_lock) = self.wait_lock {
+                    if wait_lock < 1000000 {
+                        self.wait_lock = Some(wait_lock + 1);
+                    } else {
+                        self.wait_lock = None;
+                    }
+                }
+            } else {
+                self.wait_lock = Some(0);
+            }
+
+            if self.locked() & !self.was_locked {
+                eprintln!("DPLL locked");
+            }
+            if !self.locked() & self.was_locked {
+                eprintln!("DPLL lost lock");
+            }
+            self.was_locked = self.locked();
         }
     }
 
     pub fn get_phase_unwrapped(&self) -> i64 {
         self.phase_unwrapped
     }
+
+    pub fn locked(&self) -> bool {
+        self.wait_lock.is_none()
+    }
 }
 
 pub struct PositionTracker {
@@ -76,27 +105,32 @@ impl PositionTracker {
     }
 }
 
-pub struct Decimator {
-    accumulator: i64,
+pub struct Decimator<T> {
+    accumulator: T,
     current_count: u32,
     max_count: u32
 }
 
-impl Decimator {
-    pub fn new(max_count: u32) -> Decimator {
+impl<T> Decimator<T> {
+    pub fn new(max_count: u32) -> Decimator<T> where T: std::convert::From<u32> {
         Decimator {
-            accumulator: 0,
+            accumulator: T::from(0),
             current_count: 0,
             max_count: max_count
         }
     }
 
-    pub fn input(&mut self, data: i64) -> Option<i64> {
+    pub fn input(&mut self, data: T) -> Option<T>
+            where T: Copy +
+                std::convert::From<u32> +
+                std::convert::From<<T as std::ops::Div>::Output> +
+                std::ops::AddAssign +
+                std::ops::Div {
         self.accumulator += data;
         self.current_count += 1;
         if self.current_count == self.max_count {
-            let average = self.accumulator/(self.current_count as i64);
-            self.accumulator = 0;
+            let average = T::from(self.accumulator/T::from(self.current_count));
+            self.accumulator = T::from(0);
             self.current_count = 0;
             Some(average)
         } else {

src/simple-dmi.rs

@@ -42,10 +42,12 @@ fn main() {
     let mut decimator = noptica::Decimator::new(config.decimation);
     noptica::sample(&config.sample_command, |rising, _falling| {
         refpll.tick(rising & (1 << config.bit_ref) != 0);
-        if rising & (1 << config.bit_meas) != 0 {
-            let position = position_tracker.edge(refpll.get_phase_unwrapped());
-            if let Some(position_avg) = decimator.input(position) {
-                println!("{}", position_avg);
+        if refpll.locked() {
+            if rising & (1 << config.bit_meas) != 0 {
+                let position = position_tracker.edge(refpll.get_phase_unwrapped());
+                if let Some(position_avg) = decimator.input(position) {
+                    println!("{}", position_avg);
+                }
             }
         }
     })

src/wavemeter.rs

@@ -35,13 +35,9 @@ struct Config {
     refpll_kp: i64,         // Proportionality constant of the DPLL loop filter.
 
     ref_wavelength: f64,    // Wavelength of the reference laser in m.
-    scan_displacement: f64, // Optical path difference across one scan cycle in m (use -c).
-    scan_decimation: u32,   // Decimation factor for the scan positions.
-    scan_frequency: f64,    // Frequency of the scan in Hz.
-    scan_speed_tol: f64,    // Fraction of the nominal scan speed that is accepted as deviation
-                            // from "smooth" motion.
-    scan_blanking: f64,     // Fraction of the scan period that is blanked at the beginning
-                            // and end of each slope (4 times per scan period).
+
+    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
 }
 
 fn read_config_from_file<P: AsRef<Path>>(path: P) -> Result<Config, Box<dyn Error>> {
@@ -53,7 +49,8 @@ fn read_config_from_file<P: AsRef<Path>>(path: P) -> Result<Config, Box<dyn Erro
 
 fn do_calibrate(config: &Config) {
     let mut sample_count = 0;
-    let max_sample_count = (config.sample_rate/4.0) as u32;
+    let avg_ref = (config.ref_min + config.ref_max)/2.0;
+    let max_sample_count = (avg_ref*config.position_mon_time) as u32;
     let mut position_min = i64::max_value();
     let mut position_max = i64::min_value();
 
@@ -66,20 +63,25 @@ fn do_calibrate(config: &Config) {
 
     noptica::sample(&config.sample_command, |rising, _falling| {
         refpll.tick(rising & (1 << config.bit_ref) != 0);
-        if rising & (1 << config.bit_meas) != 0 {
-            let position = position_tracker.edge(refpll.get_phase_unwrapped());
-            if position > position_max {
-                position_max = position;
+        if refpll.locked() {
+            if rising & (1 << config.bit_meas) != 0 {
+                let position = position_tracker.edge(refpll.get_phase_unwrapped());
+                if position > position_max {
+                    position_max = position;
+                }
+                if position < position_min {
+                    position_min = position;
+                }
+                sample_count += 1;
+                if sample_count == max_sample_count {
+                    let displacement = ((position_max-position_min) as f64)/(noptica::Dpll::TURN as f64)*config.ref_wavelength;
+                    println!("{} um", 1.0e6*displacement);
+                    sample_count = 0;
+                    position_min = i64::max_value();
+                    position_max = i64::min_value();
+                }
             }
-            if position < position_min {
-                position_min = position;
-            }
-        }
-
-        sample_count += 1;
-        if sample_count == max_sample_count {
-            let displacement = ((position_max-position_min) as f64)/(noptica::Dpll::TURN as f64)*config.ref_wavelength;
-            println!("{} um", 1.0e6*displacement);
+        } else {
             sample_count = 0;
             position_min = i64::max_value();
             position_max = i64::min_value();
@@ -87,141 +89,111 @@ fn do_calibrate(config: &Config) {
     })
 }
 
-enum MotionTrackerState {
-    BlankingIn(u32),
-    Smooth(u32),
-    BlankingOut
-}
 
 pub struct MotionTracker {
-    abs_speed_min: i64,
-    abs_speed_max: i64,
-    len_blanking_in: u32,
-    len_smooth: u32,
-
-    state: MotionTrackerState,
     last_position: i64,
     last_position_ago: u32,
     speed: i64
 }
 
 impl MotionTracker {
-    pub fn new(abs_speed_min: i64, abs_speed_max: i64, len_blanking_in: u32, len_smooth: u32) -> MotionTracker {
+    pub fn new() -> MotionTracker {
         MotionTracker {
-            abs_speed_min: abs_speed_min,
-            abs_speed_max: abs_speed_max,
-            len_blanking_in: len_blanking_in,
-            len_smooth: len_smooth,
-            state: MotionTrackerState::BlankingIn(0),
             last_position: 0,
             last_position_ago: 0,
             speed: 0
         }
     }
 
-    fn speed_ok(&self) -> bool {
-        let abs_speed = self.speed.abs();
-        (self.abs_speed_min < abs_speed) && (abs_speed < self.abs_speed_max)
-    }
-
     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>, mut callback: impl FnMut(bool)) {
+    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;
         }
-
-        match self.state {
-            MotionTrackerState::BlankingIn(count) => {
-                let count = if self.speed_ok() { count + 1 } else { 0 };
-                if count == self.len_blanking_in {
-                    self.state = MotionTrackerState::Smooth(0);
-                    callback(true);
-                }  else {
-                    self.state = MotionTrackerState::BlankingIn(count);
-                }
-            },
-            MotionTrackerState::Smooth(count) => {
-                if self.speed_ok() {
-                    let count = count + 1;
-                    if count == self.len_smooth {
-                        self.state = MotionTrackerState::BlankingOut;
-                        callback(false);
-                    } else {
-                        self.state = MotionTrackerState::Smooth(count);
-                    }
-                } else {
-                    // abort
-                    self.state = MotionTrackerState::BlankingIn(0);
-                }
-            },
-            MotionTrackerState::BlankingOut => {
-                if !self.speed_ok() {
-                    self.state = MotionTrackerState::BlankingIn(0);
-                }
-            }
-        }
     }
 }
 
 fn do_wavemeter(config: &Config) {
-    let mut first_fringe_position: i64 = 0;
-    let mut last_fringe_position: i64 = 0;
-    let mut fringe_count = 0;
-    let mut counting_fringes = false;
-
     let mut refpll = noptica::Dpll::new(
         noptica::Dpll::frequency_to_ftw(config.ref_min, config.sample_rate),
         noptica::Dpll::frequency_to_ftw(config.ref_max, config.sample_rate),
         config.refpll_ki,
         config.refpll_kp);
     let mut position_tracker = noptica::PositionTracker::new();
-    let mut position_decimator = noptica::Decimator::new(config.scan_decimation);
+    let mut motion_tracker = MotionTracker::new();
 
-    let abs_speed_nominal = 2.0*config.scan_displacement*config.scan_frequency;
-    let abs_speed_min = abs_speed_nominal*(1.0 - config.scan_speed_tol);
-    let abs_speed_max = abs_speed_nominal*(1.0 + config.scan_speed_tol);
-    let len_blanking_in = 2.0*config.scan_blanking/config.scan_frequency;
-    let len_smooth = (1.0 - 4.0*config.scan_blanking)/(2.0*config.scan_frequency);
-    let mut motion_tracker = MotionTracker::new(
-        (abs_speed_min/config.ref_wavelength*(noptica::Dpll::TURN as f64)) as i64,
-        (abs_speed_max/config.ref_wavelength*(noptica::Dpll::TURN as f64)) as i64,
-        (len_blanking_in*config.sample_rate) as u32,
-        (len_smooth*config.sample_rate) as u32);
+    let mut position_min = i64::max_value();
+    let mut position_max = i64::min_value();
+    let avg_ref = (config.ref_min + config.ref_max)/2.0;
+    let max_position_sample_count = (avg_ref*config.position_mon_time) as u32;
+    let mut position_sample_count = 0;
+    let mut duty_min = i64::max_value();
+    let mut duty_max = i64::min_value();
+    let mut prev_in_duty = false;
+
+    let mut first_fringe = 0;
+    let mut fringe_count = 0;
 
     noptica::sample(&config.sample_command, |rising, _falling| {
         refpll.tick(rising & (1 << config.bit_ref) != 0);
-        let position = if rising & (1 << config.bit_meas) != 0 {
-            position_decimator.input(position_tracker.edge(refpll.get_phase_unwrapped()))
-        } else {
-            None
-        };
-        motion_tracker.tick(position,
-            |enter| {
-                if enter {
-                    counting_fringes = true;
-                    fringe_count = 0;
-                } else {
-                    counting_fringes = false;
-                    if fringe_count > 1 {
-                        let wavelength = (last_fringe_position - first_fringe_position).abs()/(fringe_count - 1);
-                        let wavelength_m = (wavelength as f64)/(noptica::Dpll::TURN as f64)*config.ref_wavelength;
-                        println!("{:.3} nm", 1.0e9*wavelength_m);
-                    }
+        if refpll.locked() {
+            let position_opt;
+            if rising & (1 << config.bit_meas) != 0 {
+                let position = position_tracker.edge(refpll.get_phase_unwrapped());
+                if position > position_max {
+                    position_max = position;
                 }
-            });
-        if counting_fringes && (rising & (1 << config.bit_input) != 0) {
-            let fringe_position = motion_tracker.extrapolated_position();
-            if fringe_count == 0 {
-                first_fringe_position = fringe_position;
+                if position < position_min {
+                    position_min = position;
+                }
+                position_sample_count += 1;
+                if position_sample_count == max_position_sample_count {
+                    let amplitude = position_max - position_min;
+                    let off_duty = ((amplitude as f64)*(1.0 - config.duty_cycle)) as i64;
+                    duty_min = position_min + off_duty/2;
+                    duty_max = position_max - off_duty/2;
+                    position_sample_count = 0;
+                }
+                position_opt = Some(position);
+            } else {
+                position_opt = None
+            };
+            motion_tracker.tick(position_opt);
+            if rising & (1 << config.bit_input) != 0 {
+                let fringe_position = motion_tracker.extrapolated_position();
+                let in_duty = (duty_min < fringe_position) && (fringe_position < duty_max);
+                if in_duty & !prev_in_duty {
+                    first_fringe = fringe_position;
+                    fringe_count = 0;
+                }
+                if !in_duty & prev_in_duty {
+                    let wavelength = (fringe_position - first_fringe).abs()/fringe_count;
+                    let max_displacement = ((position_max-position_min) as f64)/(noptica::Dpll::TURN as f64)*config.ref_wavelength;
+                    let displacement = ((fringe_position - first_fringe).abs() as f64)/(noptica::Dpll::TURN as f64)*config.ref_wavelength;
+                    println!("{:.4} {} {:.2} {} {:.1}",
+                        (wavelength as f64)/(noptica::Dpll::TURN as f64)*1.0e9*config.ref_wavelength,
+                        fringe_count,
+                        1.0e6*max_displacement,
+                        if fringe_position > first_fringe { "UP  " } else { "DOWN" },
+                        1.0e9*displacement);
+                    fringe_count = 0;
+                }
+                fringe_count += 1;
+                prev_in_duty = in_duty;
             }
-            last_fringe_position = fringe_position;
-            fringe_count += 1;
+        } else {
+            position_min = i64::max_value();
+            position_max = i64::min_value();
+            position_sample_count = 0;
+            duty_min = i64::max_value();
+            duty_max = i64::min_value();
+            prev_in_duty = false;
         }
     })
 }

wavemeter.json

@@ -11,9 +11,7 @@
     "refpll_kp": 85899345,
 
     "ref_wavelength": 632.991372e-9,
-    "scan_displacement": 40e-6,
-    "scan_decimation": 30,
-    "scan_frequency": 50.0,
-    "scan_speed_tol": 0.05,
-    "scan_blanking": 0.05
+
+    "position_mon_time": 0.25,
+    "duty_cycle": 0.5
 }