improve lock algo, display locked state

dmi.py

@@ -19,11 +19,11 @@ def main():
         induction = InductionHeater("/dev/ttyUSB0", 350e3, 445e3)
         induction.start()
         try:
-            def stabilizer_cb(spectrum, peak_freq, tuning):
-                gui.update_beat_spectrum(spectrum, peak_freq)
+            def stabilizer_cb(spectrum, peak_freq, locked, tuning):
+                gui.update_beat_spectrum(spectrum, peak_freq, locked)
                 induction.set(tuning)
 
-            stabilizer = Stabilizer(block_size, 80.0, (1088.1e6 - freq_base)/freq_sample, 50.0, stabilizer_cb)
+            stabilizer = Stabilizer(block_size, (1088.1e6 - freq_base)/freq_sample, stabilizer_cb)
             position_tracker = PositionTracker(int(0.1*freq_sample/block_size))
 
             sdr = SoapySDR.Device()

gui.py

@@ -54,8 +54,8 @@ class GUI:
                                      os.path.join(os.path.dirname(os.path.abspath(__file__)), "gui_impl.py"),
                                      str(freq_sample), str(freq_base), str(block_size)])
 
-    def update_beat_spectrum(self, block, peak_freq):
-        obj = {"action": "update_beat_spectrum", "block": block, "peak_freq": peak_freq}
+    def update_beat_spectrum(self, block, peak_freq, locked):
+        obj = {"action": "update_beat_spectrum", "block": block, "peak_freq": peak_freq, "locked": locked}
         self.impl.write_pyon(obj)
 
     def close(self):

gui_impl.py

@@ -33,26 +33,34 @@ class SpectrogramItem(pg.ImageItem):
         self.setImage(self.img_array, autoLevels=True)
 
 
-class MainWindow(pg.GraphicsWindow):
+class MainWindow(pg.GraphicsLayoutWidget):
     def __init__(self, freq_sample, freq_base, block_size):
-        pg.GraphicsWindow.__init__(self)
+        pg.GraphicsLayoutWidget.__init__(self)
         self.setWindowTitle("NOPTICA Wavemeter")
 
         self.freq_sample = freq_sample
         self.freq_base = freq_base
 
-        self.text = pg.LabelItem()
-        self.addItem(self.text)
+        self.text_ref = pg.LabelItem(size="24pt")
+        self.addItem(self.text_ref, row=0, col=0)
+        self.text_locked = pg.LabelItem(size="24pt")
+        self.addItem(self.text_locked, row=0, col=1)
+        self.update_params(None, False)
 
-        p1 = self.addPlot(row=1, col=0)
+        p1 = self.addPlot(row=1, col=0, colspan=2)
         self.beat_spectrum = SpectrogramItem(freq_sample, freq_base, block_size)
         p1.addItem(self.beat_spectrum)
 
-    def update_params(self, peak_freq):
+    def update_params(self, peak_freq, locked):
         if peak_freq is None:
-            self.text.setText("REF: NO BEAT")
+            self.text_ref.setText("REF: NO SIGNAL")
         else:
-            self.text.setText("REF: {:.3f}MHz".format((self.freq_base + self.freq_sample*peak_freq)/1e6))
+            self.text_ref.setText("REF: {:.3f}MHz".format((self.freq_base + self.freq_sample*peak_freq)/1e6))
+        if locked:
+            self.text_locked.setText("REF LASER LOCKED", color="00FF00")
+        else:
+            self.text_locked.setText("REF LASER UNLOCKED", color="FF0000")
+
 
 
 class IPCClient(AsyncioChildComm):
@@ -70,7 +78,7 @@ class IPCClient(AsyncioChildComm):
                 action = obj["action"]
                 if action == "update_beat_spectrum":
                     main_window.beat_spectrum.add_block(obj["block"])
-                    main_window.update_params(obj["peak_freq"])
+                    main_window.update_params(obj["peak_freq"], obj["locked"])
                 if action == "terminate":
                     self.close_cb()
                     return

noptica.py

@@ -88,7 +88,7 @@ class InductionHeater:
             if amount is None:
                 break
 
-            amount = max(min(amount, 0.5), -0.5)
+            assert -0.5 <= amount <= 0.5
             freq = ((self.induction_min + self.induction_max)/2
                     + amount*(self.induction_max - self.induction_min))
 
@@ -106,25 +106,58 @@ class InductionHeater:
 
 
 class Stabilizer:
-    def __init__(self, fft_size, amp_threshold, freq_target, k, cb):
-        self.freqs = np.fft.fftfreq(fft_size)
-        self.amp_threshold = amp_threshold
+    def __init__(self, block_size, freq_target, cb):
+        self.freqs = np.fft.fftfreq(block_size)
         self.freq_target = freq_target
-        self.k = k
         self.cb = cb
 
+        self.lock_counter = 0
+        self.unlock_counter = 0
+        self.wiggle_direction = 1
+
+        self.amp_threshold = 80.0
+        self.k = 50.0
+        self.tolerance = 0.010
+        self.lock_counter_threshold = 60
+        self.unlock_counter_threshold = 600
+        self.wiggle = 0.1
+
     def input(self, samples):
         spectrum = np.abs(np.fft.fft(samples*blackmanharris(len(samples))))
         i = np.argmax(spectrum)
         amplitude = spectrum[i]
 
+        success = False
         if amplitude > self.amp_threshold:
             freq = self.freqs[i]
-            tuning = (freq - self.freq_target)*self.k
+            delta = freq - self.freq_target
+            tuning = delta*self.k
+            if abs(delta) < self.tolerance:
+                success = True
         else:
             freq = None
             tuning = 0.0
-        self.cb(spectrum, freq, tuning)
+        max_tuning_abs = 0.5 - self.wiggle
+        tuning = max(min(tuning, max_tuning_abs), -max_tuning_abs)
+
+        if success:
+            self.lock_counter += 1
+        else:
+            self.lock_counter = 0
+
+        if self.locked():
+            self.unlock_counter = 0
+        else:
+            self.unlock_counter += 1
+        if not success and (self.unlock_counter > self.unlock_counter_threshold):
+            print("wiggle")
+            self.wiggle_direction = -self.wiggle_direction
+            self.unlock_counter = 0
+
+        self.cb(spectrum, freq, self.locked(), tuning + self.wiggle_direction*self.wiggle)
+
+    def locked(self):
+        return self.lock_counter > self.lock_counter_threshold
 
 
 def continuous_unwrap(last_phase, last_phase_unwrapped, p):