From 1d8c9ca2241204e4cfe437710459bfd6610f280a Mon Sep 17 00:00:00 2001
From: David Lenfesty <lenfesty@ualberta.ca>
Date: Sat, 27 May 2023 09:53:29 -0600
Subject: [PATCH] gw: Start testing peak detector, and fix a bug!

---
 gateware/litex_main.py            |  2 +
 gateware/sampler/peak_detector.py | 80 ++++++++++++++++++++++++++++++-
 2 files changed, 80 insertions(+), 2 deletions(-)

diff --git a/gateware/litex_main.py b/gateware/litex_main.py
index 52b13e9..e39b5bb 100755
--- a/gateware/litex_main.py
+++ b/gateware/litex_main.py
@@ -178,10 +178,12 @@ def main():
     if args.test:
         from sampler import circular_buffer
         from sampler import controller
+        from sampler import peak_detector
 
         results = []
         results.append(run_test("CircularBuffer", circular_buffer.testbench))
         results.append(run_test("SamplerController", controller.test_bus_access))
+        results.append(run_test("PeakDetector", peak_detector.test_simple_waveform))
 
         passed = sum((1 for result in results if result.result == TestResult.PASS))
         failed = sum((1 for result in results if result.result == TestResult.FAIL))
diff --git a/gateware/sampler/peak_detector.py b/gateware/sampler/peak_detector.py
index bc42b33..ec556ac 100644
--- a/gateware/sampler/peak_detector.py
+++ b/gateware/sampler/peak_detector.py
@@ -90,9 +90,85 @@ class PeakDetector(Module):
                 If(decay_counter >= self.decay_period,
                    decay_counter.eq(0),
 
-                   # Only apply decay if the values would not overlap
-                   If(diff >= (self.decay_value << 1),
+                   # Only apply decay if the values would not overlap, and we use the decay
+                   If((diff >= (self.decay_value << 1)) & (self.decay_value > 0),
                       max_val.eq(max_val - self.decay_value),
                       min_val.eq(min_val + self.decay_value)))
             )
         )
+
+
+from typing import Tuple
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+def create_waveform(dc_bias: int = 0, scale: float = 1) -> Tuple[np.ndarray[float], np.ndarray[int]]:
+    """
+    Create a simple 40kHz sine wave in integer values that can be used by peak detector
+    """
+    assert scale <= 1.0, "Scale factor must be some ratio of full range"
+
+    # Constants
+    f_s = 10e6              # Sample rate (Hz)
+    f = 40e3                # Signal Frequency (Hz)
+    t = 0.002               # Sample period (s)
+    n = int(f_s * 0.002)    # Number of samples
+
+    # Create time from 0ms to 2ms
+    x = np.linspace(0, t, n)
+
+    # Create signal!
+    y = np.sin(x * 2*np.pi*f)
+
+    # Scale according to user inputs
+    y = y * scale
+
+    # Convert to positive integer at provided bias
+    signal = np.ndarray((len(y)), dtype=np.uint16)
+    # "unsafe" casting because numpy doesn't know we are staying under 10 bit values
+    np.copyto(signal, y * 512 + 512 + dc_bias, casting='unsafe')
+
+    #plt.plot(x, signal)
+    #plt.show()
+
+    return x, signal
+
+
+def set_settings(dut: PeakDetector, thresh_value: int, thresh_time: int, decay_value: int, decay_period: int) -> None:
+    """Set peak detector settings simply"""
+    (yield dut.thresh_value.eq(thresh_value))
+    (yield dut.thresh_time.eq(thresh_time))
+    (yield dut.decay_value.eq(decay_value))
+    (yield dut.decay_period.eq(decay_period))
+
+    # Load in values with a new clock
+    yield
+
+
+def test_simple_waveform():
+    (_, signal) = create_waveform()
+    dut = PeakDetector(10)
+
+    def test_fn():
+        # First set settings to be simple, we want to trigger pretty much immediately
+        yield from set_settings(dut, 800, 10, 0, 0)
+
+        # Enable device
+        (yield dut.enable.eq(1))
+        yield
+
+        # Load data in until we trigger
+        for i, val in enumerate(signal):
+            if (yield dut.triggered) == 1:
+                # Test passed!
+                return
+
+            # Load in data, set valid
+            (yield dut.data.eq(int(val)))
+            (yield dut.data_valid.eq(1))
+            yield # Tick
+
+        assert False, "No trigger, test has failed..."
+
+    run_simulation(dut, test_fn(), vcd_name="peak_detector.vcd")