refactor, higher-order LPF

dsp_lib.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <cstdint>
+
+class DDS {
+    private:
+        uint32_t phase = 0;
+    public:
+        uint32_t ftw = 0;
+        void set_frequency(double f) {
+            ftw = f*(double)UINT32_MAX;
+        }
+        double get() {
+            phase += ftw;  // wraps on overflow
+            return sin(phase*2.0*M_PI/(double)UINT32_MAX);
+        }
+};
+
+template<typename T, unsigned int N>
+class Lowpass {
+    private:
+        double k = 0.0;
+        T s[N] = {};
+    public:
+        void set_bandwidth(double f) {
+            k = 2.0*M_PI*f;
+        }
+        T update(T x) {
+            T a = x;
+            for(int i=0;i<N;i++) {
+                s[i] += (a - s[i])*k;
+                a = s[i];
+            }
+            return a;
+        }
+};

sndlock.cpp

@@ -12,6 +12,8 @@
 #include "imgui_impl_glfw.h"
 #include "imgui_impl_opengl3.h"
 
+#include "dsp_lib.h"
+
 #define SND_BITS 24
 #define SND_PCHAN 2
 #define SND_RCHAN 2
@@ -60,12 +62,12 @@ static void dsp_thread()
         return;
     }
 
-    uint32_t phase_out[SND_PCHAN] = { 0 };
+    DDS dds[SND_PCHAN];
     int32_t buf_out[SND_BUFLEN*SND_PCHAN];
     size_t buf_out_offset = sizeof(buf_out);
 
     uint32_t phase_in[SND_PCHAN] = { 0 };
-    std::complex<double> lpf_y[SND_PCHAN];
+    Lowpass<std::complex<double>, 4> lpf[SND_PCHAN];
     int lpf_count[SND_PCHAN] = { 0 };
 
     nfds_t nfds = sio_nfds(hdl);
@@ -75,21 +77,18 @@ static void dsp_thread()
         poll(pfd, nfds, INFTIM);
         int revents = sio_revents(hdl, pfd);
 
-        uint32_t ftw[SND_PCHAN];
         double lpf_k[SND_PCHAN];
         for(int i=0;i<SND_PCHAN;i++) {
-            ftw[i] = frequency[i]*(double)UINT32_MAX/SND_RATE;
+            dds[i].set_frequency(frequency[i]/SND_RATE);
-            lpf_k[i] = 2.0*M_PI*lpf_bandwidth[i]/SND_RATE;
+            lpf[i].set_bandwidth(lpf_bandwidth[i]/SND_RATE);
         }
 
         if(revents & POLLOUT) {
             double scale = pow(2.0, SND_BITS-1) - 1.0;
             if(buf_out_offset == sizeof(buf_out)) {
                 for(int i=0;i<SND_PCHAN;i++)
-                    for(int j=0;j<SND_BUFLEN;j++) {
+                    for(int j=0;j<SND_BUFLEN;j++)
-                        buf_out[SND_PCHAN*j+i] = scale*sin(phase_out[i]*2.0*M_PI/(double)UINT32_MAX);
+                        buf_out[SND_PCHAN*j+i] = scale*dds[i].get();
-                        phase_out[i] += ftw[i];  // wraps on overflow
-                    }
                 buf_out_offset = 0;
             }
             size_t written = sio_write(hdl, (const char *)buf_out + buf_out_offset, sizeof(buf_out) - buf_out_offset);
@@ -108,10 +107,9 @@ static void dsp_thread()
                         sample += (double)buf_in[SND_RCHAN*j+k];
                     std::complex<double> rotated;
                     rotated = sample*std::polar(scale, phase_in[i]*2.0*M_PI/(double)UINT32_MAX);
-                    phase_in[i] -= ftw[i];  // wraps on underflow
+                    phase_in[i] -= dds[i].ftw;  // wraps on underflow
 
-                    lpf_y[i] += (rotated - lpf_y[i])*lpf_k[i];
+                    double mag = std::abs(lpf[i].update(rotated));
-                    double mag = std::abs(lpf_y[i]);
 
                     lpf_count[i]++;
                     if(lpf_count[i] == 200) {