From 743e5278a324f08aea0b7e626eaad3341fe30de9 Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Mon, 4 Nov 2019 14:46:04 +0800
Subject: [PATCH] monitor DPLL lock

---
 src/noptica.rs    | 31 ++++++++++++++++++++++++++++-
 src/simple-dmi.rs | 10 ++++++----
 src/wavemeter.rs  | 50 +++++++++++++++++++++++++++--------------------
 3 files changed, 65 insertions(+), 26 deletions(-)

diff --git a/src/noptica.rs b/src/noptica.rs
index 188fc91..54f6e53 100644
--- a/src/noptica.rs
+++ b/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 {
diff --git a/src/simple-dmi.rs b/src/simple-dmi.rs
index c6c66b5..d19bad2 100644
--- a/src/simple-dmi.rs
+++ b/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);
+                }
             }
         }
     })
diff --git a/src/wavemeter.rs b/src/wavemeter.rs
index 85e16c3..dc2549a 100644
--- a/src/wavemeter.rs
+++ b/src/wavemeter.rs
@@ -59,20 +59,26 @@ 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;
+                }
             }
-            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 += 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();
+            }
+        } else {
             sample_count = 0;
             position_min = i64::max_value();
             position_max = i64::min_value();
@@ -121,15 +127,17 @@ fn do_wavemeter(config: &Config) {
 
     noptica::sample(&config.sample_command, |rising, _falling| {
         refpll.tick(rising & (1 << config.bit_ref) != 0);
-        let position = if rising & (1 << config.bit_meas) != 0 {
-            Some(position_tracker.edge(refpll.get_phase_unwrapped()))
-        } else {
-            None
-        };
-        motion_tracker.tick(position);
-        if rising & (1 << config.bit_input) != 0 {
-            let fringe_position = motion_tracker.extrapolated_position();
-            println!("{}", (fringe_position as f64)/(noptica::Dpll::TURN as f64));
+        if refpll.locked() {
+            let position = if rising & (1 << config.bit_meas) != 0 {
+                Some(position_tracker.edge(refpll.get_phase_unwrapped()))
+            } else {
+                None
+            };
+            motion_tracker.tick(position);
+            if rising & (1 << config.bit_input) != 0 {
+                let fringe_position = motion_tracker.extrapolated_position();
+                println!("{}", (fringe_position as f64)/(noptica::Dpll::TURN as f64));
+            }
         }
     })
 }