new algo with better diagnostics

shell.nix

@@ -22,4 +22,7 @@ in
       (pkgs.python3.withPackages(ps: [ps.quamash ps.pyqt5 pyqtgraph-qt5]))
       pkgs.rustc pkgs.cargo
     ];
+
+    # Hack: shut up rustc complaint "#![feature] may not be used on the stable release channel"
+    RUSTC_BOOTSTRAP = "1";
   }

src/wavemeter.rs

@@ -1,3 +1,5 @@
+#![feature(generators, generator_trait)]
+
 extern crate argparse;
 extern crate num_traits;
 extern crate serde_derive;
@@ -12,6 +14,9 @@ use std::error::Error;
 use std::fs::File;
 use std::io::BufReader;
 use std::path::Path;
+use std::ops::{Generator, GeneratorState};
+use std::pin::Pin;
+use std::cell::Cell;
 
 mod noptica;
 
@@ -42,6 +47,8 @@ struct Config {
 
     debug: bool,            // Enable debug output of wavelength determination code
     motion_cutoff: f64,     // Cut-off frequency of the motion filter
+    min_fringes: u32,       // Minimum number of fringes to count
+    fringe_jitter_tol: f64, // Tolerance for fringe distance jitter
     decimation: u32,        // Decimation/averaging factor for the final wavelength output
 }
 
@@ -116,6 +123,121 @@ fn do_calibrate(config: &Config) {
     })
 }
 
+#[derive(Clone, Copy, PartialEq, Debug)]
+enum Quadrant {
+    BelowMin,
+    Up,
+    AboveMax,
+    Down
+}
+
+#[derive(Clone)]
+struct QuadrantTracker {
+    prev_state: Quadrant,
+    state: Quadrant,
+    min: i64,
+    max: i64,
+    new_min: i64,
+    new_max: i64,
+    prev_above_middle: bool,
+    middle: i64,
+}
+
+impl QuadrantTracker {
+    pub fn new() -> QuadrantTracker {
+        QuadrantTracker {
+            prev_state: Quadrant::BelowMin,
+            state: Quadrant::BelowMin,
+            min: i64::max_value(),
+            max: i64::min_value(),
+            new_min: i64::max_value(),
+            new_max: i64::min_value(),
+            prev_above_middle: false,
+            middle: i64::max_value(),
+        }
+    }
+
+    pub fn reset(&mut self) {
+        *self = QuadrantTracker::new();
+    }
+
+    pub fn input(&mut self, position: i64) {
+        let above_min = position > self.min;  // always false before init
+        let below_max = position < self.max;  // always false before init
+        let next_state;
+        if above_min && below_max {
+            next_state = match self.state {
+                Quadrant::BelowMin => Quadrant::Up,
+                Quadrant::Up => Quadrant::Up,
+                Quadrant::AboveMax => Quadrant::Down,
+                Quadrant::Down => Quadrant::Down
+            }
+        } else {
+            if above_min {
+                next_state = Quadrant::AboveMax;
+            } else {
+                next_state = Quadrant::BelowMin;  // stays here before init
+            }
+        }
+
+        self.prev_state = self.state;
+        if self.state != next_state {
+            match (self.state, next_state) {
+                (Quadrant::BelowMin, Quadrant::Up) => (),
+                (Quadrant::Up, Quadrant::AboveMax) => (),
+                (Quadrant::AboveMax, Quadrant::Down) => (),
+                (Quadrant::Down, Quadrant::BelowMin) => (),
+                _ => eprintln!("invalid quadrant transition: {:?} -> {:?}",
+                    self.state, next_state)
+            }
+            self.state = next_state;
+        }
+
+
+        // Update min and max when the position is near the middle
+        // to avoid glitches.
+        let above_middle = position > self.middle;  // always false before init
+        if above_middle && !self.prev_above_middle {
+            self.min = self.new_min;
+            self.max = self.new_max;
+        }
+        self.prev_above_middle = above_middle;
+    }
+
+    pub fn update_limits(&mut self, min: i64, max: i64) {
+        self.new_min = min;
+        self.new_max = max;
+        self.middle = (min + max)/2;
+    }
+
+    pub fn up_start(&self) -> bool {
+        self.prev_state == Quadrant::BelowMin && self.state == Quadrant::Up
+    }
+
+    pub fn up_end(&self) -> bool {
+        self.prev_state == Quadrant::Up && self.state == Quadrant::AboveMax
+    }
+
+    pub fn down_start(&self) -> bool {
+        self.prev_state == Quadrant::AboveMax && self.state == Quadrant::Down
+    }
+
+    pub fn down_end(&self) -> bool {
+        self.prev_state == Quadrant::Down && self.state == Quadrant::BelowMin
+    }
+}
+
+#[derive(Debug, Clone, Copy)]
+enum FringeCounterEvent {
+    Start,
+    Fringe(i64),
+    End,
+}
+
+macro_rules! generator_input {
+    ($e:expr) => ({ yield (); $e.get() })
+}
+
 fn do_wavemeter(config: &Config) {
     let mut refpll = noptica::Dpll::new(
         noptica::Dpll::frequency_to_ftw(config.ref_min, config.sample_rate),
@@ -124,90 +246,148 @@ fn do_wavemeter(config: &Config) {
         config.refpll_kp);
     let mut position_tracker = noptica::PositionTracker::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);
+    let mut min_max_monitor = MinMaxMonitor::new((config.sample_rate*config.position_mon_time) as u32);
+    let mut quadrant_tracker = QuadrantTracker::new();
 
-    // Update duty_min and duty_max when the position is near the middle
-    // to avoid glitches.
-    let mut prev_position_above_middle = false;
-    // Trick: position > position_middle is always false before the first monitor cycle.
-    let mut position_middle = i64::max_value();
-    let mut new_duty_min = i64::max_value();
-    let mut new_duty_max = i64::min_value();
+    let fringe_counter_input = Cell::new(FringeCounterEvent::Start);
+    let mut fringe_counter = || {
+        'outer: loop {
+            loop {
+                if let FringeCounterEvent::Start = generator_input!(fringe_counter_input) {
+                    break;
+                }
+            }
 
-    let mut duty_min = i64::max_value();
-    let mut duty_max = i64::min_value();
-    let mut prev_in_duty = false;
+            let mut boundary_fringes = [0i64; 4];
+            for i in 0..4 {
+                if let FringeCounterEvent::Fringe(position) = generator_input!(fringe_counter_input) {
+                    boundary_fringes[i] = position;
+                } else {
+                    eprintln!("unexpected event (boundary fringe acquisition)");
+                    continue 'outer;
+                }
+            }
 
-    let mut first_fringe = 0;
-    let mut fringe_count = 0;
-    let mut decimator = noptica::Decimator::new(config.decimation);
+            let mut fringes_between_boundary = 0;
+            loop {
+                match generator_input!(fringe_counter_input) {
+                    FringeCounterEvent::Start => {
+                        eprintln!("unexpected event (initial fringe counting)");
+                        continue 'outer;
+                    },
+                    FringeCounterEvent::Fringe(position) => {
+                        boundary_fringes[2] = boundary_fringes[3];
+                        boundary_fringes[3] = position;
+                        fringes_between_boundary += 1;
+                    },
+                    FringeCounterEvent::End => break,
+                }
+            }
+
+            if fringes_between_boundary < config.min_fringes {
+                eprintln!("insufficient fringes between boundary ({})", fringes_between_boundary);
+                continue 'outer;
+            }
+
+            let nominal_distance = boundary_fringes[1] - boundary_fringes[0];
+            let jitter_tol = ((nominal_distance as f64)*config.fringe_jitter_tol) as i64;
+
+            let limit1 = (boundary_fringes[0] + boundary_fringes[1])/2;
+            let limit2 = (boundary_fringes[2] + boundary_fringes[3])/2;
+            let expected_fringes = fringes_between_boundary + 2;
+            let mut f1_acc = boundary_fringes[1];
+            let mut f2_acc = boundary_fringes[2];
+
+            for _ in 0..config.decimation-1 {
+                loop {
+                    if let FringeCounterEvent::Start = generator_input!(fringe_counter_input) {
+                        break;
+                    }
+                }
+                let mut last_fringe: Option<i64> = None;
+                let mut count: u32 = 0;
+                loop {
+                    match generator_input!(fringe_counter_input) {
+                        FringeCounterEvent::Start => {
+                            eprintln!("unexpected event (secondary fringe counting)");
+                            continue 'outer;
+                        },
+                        FringeCounterEvent::Fringe(position) => {
+                            if (position > limit1) && (position < limit2)
+                                    || (position > limit2) && (position < limit1) {
+                                if let Some(last_fringe) = last_fringe {
+                                    let distance = position - last_fringe;
+                                    if (distance - nominal_distance).abs() > jitter_tol {
+                                        eprintln!("distance between fringes above tolerance (got {}, nominal {})",
+                                            distance, nominal_distance);
+                                        continue 'outer;
+                                    }
+                                }
+                                last_fringe = Some(position);
+                                count += 1;
+                                if count == 1 {
+                                    f1_acc += position;
+                                }
+                            }
+                        },
+                        FringeCounterEvent::End => break,
+                    }
+                }
+                if count == expected_fringes {
+                    f2_acc += last_fringe.unwrap();
+                } else {
+                    eprintln!("unexpected fringe count (got {}, expected {})", count, expected_fringes);
+                    continue 'outer;
+                }
+            }
+
+            let f1_avg = f1_acc/(config.decimation as i64);
+            let f2_avg = f2_acc/(config.decimation as i64);
+            let wavelength = (f2_avg - f1_avg).abs()/(expected_fringes as i64 - 1);
+            let wavelength_nm = (wavelength as f64)*config.ref_wavelength/(noptica::Dpll::TURN as f64);
+            println!("{:.4}", wavelength_nm*1.0e9);
+        }
+
+    };
+    let mut fringe_counter_event = |event: FringeCounterEvent| {
+        fringe_counter_input.set(event);
+        Pin::new(&mut fringe_counter).resume();
+    };
 
     noptica::sample(&config.sample_command, |rising, _falling| {
         refpll.tick(rising & (1 << config.bit_ref) != 0);
         if refpll.locked() {
             if rising & (1 << config.bit_meas) != 0 {
                 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;
-                    new_duty_min = position_min + off_duty/2;
-                    new_duty_max = position_max - off_duty/2;
-                    position_middle = (position_max + position_min)/2;
-                });
-                let position_above_middle = position > position_middle;
-                if !position_above_middle && prev_position_above_middle {
-                    duty_min = new_duty_min;
-                    duty_max = new_duty_max;
-                }
-                prev_position_above_middle = position_above_middle;
             }
-            let filtered_position = motion_filter.run(position as f64) as i64;
-
+            let f_position = motion_filter.run(position as f64) as i64;
+            min_max_monitor.input(f_position, |position_min, position_max| {
+                let amplitude = position_max - position_min;
+                let off_duty = ((amplitude as f64)*(1.0 - config.duty_cycle)) as i64;
+                quadrant_tracker.update_limits(
+                    position_min + off_duty/2,
+                    position_max - off_duty/2);
+            });
+            quadrant_tracker.input(f_position);
+            if quadrant_tracker.up_start() {
+                fringe_counter_event(FringeCounterEvent::Start);
+            }
+            if quadrant_tracker.up_end() {
+                fringe_counter_event(FringeCounterEvent::End);
+            }
             if rising & (1 << config.bit_input) != 0 {
-                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;
-                    fringe_count = 0;
-                }
-                if !in_duty & prev_in_duty {
-                    let wavelength = (fringe_position - first_fringe).abs()/fringe_count;
-                    let wavelength_nm = (wavelength as f64)/(noptica::Dpll::TURN as f64)*1.0e9*config.ref_wavelength;
-                    if config.debug {
-                        let displacement = ((fringe_position - first_fringe).abs() as f64)/(noptica::Dpll::TURN as f64)*config.ref_wavelength;
-                        println!("DEBUG: {:.4} {} {} {:.1}",
-                            wavelength_nm,
-                            fringe_count,
-                            if fringe_position > first_fringe { "UP  " } else { "DOWN" },
-                            1.0e9*displacement);
-                    }
-                    fringe_count = 0;
-                    if let Some(wavelength_nm) = decimator.input(wavelength_nm) {
-                        println!("{:.4} nm", wavelength_nm);
-                    }
-                }
-                fringe_count += 1;
-                prev_in_duty = in_duty;
+                fringe_counter_event(FringeCounterEvent::Fringe(position));
             }
         } else {
             position = 0;
             min_max_monitor.reset();
-
-            prev_position_above_middle = false;
-            position_middle = i64::max_value();
-            new_duty_min = i64::max_value();
-            new_duty_max = i64::min_value();
-
-            duty_min = i64::max_value();
-            duty_max = i64::min_value();
-            prev_in_duty = false;
+            quadrant_tracker.reset();
         }
     })
 }

wavemeter.json

@@ -17,5 +17,7 @@
 
     "debug": false,
     "motion_cutoff": 100e3,
+    "min_fringes": 1000,
+    "fringe_jitter_tol": 0.05,
     "decimation": 50
 }