From 6cefac3e2fd4b72f8b5c6997035f320a4c125a31 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Bourdeauducq?= <sb@m-labs.hk>
Date: Fri, 27 Dec 2024 18:56:31 +0800
Subject: [PATCH] lockin demodulation

---
 sndlock.cpp | 61 +++++++++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 52 insertions(+), 9 deletions(-)

diff --git a/sndlock.cpp b/sndlock.cpp
index 162762b..bc6fddf 100644
--- a/sndlock.cpp
+++ b/sndlock.cpp
@@ -1,6 +1,7 @@
 #include <iostream>
 #include <thread>
 #include <cstdint>
+#include <complex>
 
 #include <GLFW/glfw3.h>
 #include <sndio.h>
@@ -18,6 +19,10 @@
 
 static std::atomic<bool> shutdown_threads;
 static std::atomic<float> frequency[SND_PCHAN];
+static std::atomic<float> lpf_bandwidth[SND_PCHAN];
+
+static std::mutex lpf_hist_mutex;
+static float lpf_hist[SND_PCHAN][512];
 
 static void dsp_thread()
 {
@@ -55,20 +60,26 @@ static void dsp_thread()
     }
 
     uint32_t phase_out[SND_PCHAN] = { 0 };
-
     int16_t buf_out[SND_BUFLEN*SND_PCHAN];
     size_t buf_out_idx = SND_BUFLEN*SND_PCHAN;
+
+    uint32_t phase_in[SND_PCHAN] = { 0 };
+    std::complex<float> lpf_y[SND_PCHAN];
+    int lpf_count[SND_PCHAN] = { 0 };
+
     nfds_t nfds = sio_nfds(hdl);
     struct pollfd pfd[nfds];
-
     while(!shutdown_threads) {
         sio_pollfd(hdl, pfd, POLLOUT|POLLIN);
         poll(pfd, nfds, INFTIM);
         int revents = sio_revents(hdl, pfd);
 
         uint32_t ftw[SND_PCHAN];
-        for(int i=0;i<SND_PCHAN;i++)
+        float lpf_k[SND_PCHAN];
+        for(int i=0;i<SND_PCHAN;i++) {
             ftw[i] = frequency[i]*(float)UINT32_MAX/SND_RATE;
+            lpf_k[i] = 2.0f*(float)M_PI*lpf_bandwidth[i]/SND_RATE;
+        }
 
         if(revents & POLLOUT) {
             float scale = powf(2.0f, SND_BITS-1) - 1.0f;
@@ -86,10 +97,25 @@ static void dsp_thread()
 
         if(revents & POLLIN) {
             int16_t buf_in[SND_BUFLEN];
-            sio_read(hdl, buf_in, sizeof(buf_in));
-            for(int i=0;i<10;i++)
-              std::cout << buf_in[i] << " ";
-            std::cout << std::endl;
+            size_t read = sio_read(hdl, buf_in, sizeof(buf_in));
+            float scale = powf(0.5f, SND_BITS-1);
+            for(int i=0;i<SND_PCHAN;i++)
+                for(int j=0;j<read;j++) {
+                    std::complex<float> rotated;
+                    rotated = (float)buf_in[j]*std::polar(scale, phase_in[i]*2.0f*(float)M_PI/(float)UINT32_MAX);
+                    phase_in[i] += ftw[i];  // wraps on underflow
+
+                    lpf_y[i] += (rotated - lpf_y[i])*lpf_k[i];
+                    float mag = std::abs(lpf_y[i]);
+
+                    lpf_count[i]++;
+                    if(lpf_count[i] == 800) {
+                        lpf_count[i] = 0;
+                        std::lock_guard<std::mutex> guard(lpf_hist_mutex);
+                        std::memmove(&lpf_hist[i][0], &lpf_hist[i][1], 511*sizeof(float));
+                        lpf_hist[i][511] = mag;
+                    }
+                }
         }
 
         if(sio_eof(hdl)) {
@@ -138,8 +164,10 @@ int main(int argc, char* argv[])
     ImGui_ImplOpenGL3_Init("#version 130");
     std::atexit(ImGui_ImplOpenGL3_Shutdown);
 
-    for(int i=0;i<SND_PCHAN;i++)
-        frequency[i] = 441.0 + 202.0*i;
+    for(int i=0;i<SND_PCHAN;i++) {
+        frequency[i] = 441.0f + 202.0f*i;
+        lpf_bandwidth[i] = 10.0f;
+    }
 
     static std::thread dsp_thread_h = std::thread(dsp_thread);
     static auto JoinDSP = []() {
@@ -177,6 +205,21 @@ int main(int argc, char* argv[])
                 frequency[i] = frequency_l;
             }
         }
+        if(ImGui::CollapsingHeader("Demodulation", ImGuiTreeNodeFlags_DefaultOpen)) {
+            for(int i=0;i<SND_PCHAN;i++) {
+                char str[64];
+                sprintf(str, "LPF bandwidth ch%d", i);
+                float lpf_bandwidth_l = lpf_bandwidth[i];
+                ImGui::SliderFloat(str, &lpf_bandwidth_l, 0.5f, 200.0f);
+                lpf_bandwidth[i] = lpf_bandwidth_l;
+
+                sprintf(str, "Output ch%d", i);
+                {
+                    std::lock_guard<std::mutex> guard(lpf_hist_mutex);
+                    ImGui::PlotLines(str, lpf_hist[i], 512, 0, 0, -0.0f, 0.01f, ImVec2(0, 200.0f));
+                }
+            }
+        }
         if(ImGui::Button("Exit"))
             exit = true;