refactor

dsp_lib.h

@@ -1,36 +1,60 @@
 #pragma once
 
 #include <cstdint>
+#include <complex>
+
+static uint32_t frequency_to_ftw(double f) {
+    return f*(double)UINT32_MAX;
+}
 
 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>
+template<typename T, unsigned int order>
 class Lowpass {
     private:
         double k = 0.0;
-        T s[N] = {};
+        T s[order] = {};
     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++) {
+            for(int i=0;i<order;i++) {
                 s[i] += (a - s[i])*k;
                 a = s[i];
             }
             return a;
         }
 };
+
+template<unsigned int order>
+class Lockin {
+    private:
+        double scale;
+        uint32_t phase = 0;
+        Lowpass<std::complex<double>, order> lpf;
+    public:
+        uint32_t ftw = 0;
+        void set_scale(double s) {
+            scale = s;
+        }
+        void set_bandwidth(double f) {
+            lpf.set_bandwidth(f);
+        }
+        std::complex<double> update(double x) {
+            std::complex<double> rotated;
+            rotated = x*std::polar(scale, phase*2.0*M_PI/(double)UINT32_MAX);
+            phase -= ftw;  // wraps on underflow
+            return lpf.update(rotated);
+        }
+};

sndlock.cpp

@@ -66,8 +66,10 @@ static void dsp_thread()
     int32_t buf_out[SND_BUFLEN*SND_PCHAN];
     size_t buf_out_offset = sizeof(buf_out);
 
-    uint32_t phase_in[SND_PCHAN] = { 0 };
-    Lowpass<std::complex<double>, 4> lpf[SND_PCHAN];
+    Lockin<4> lockin[SND_PCHAN];
+    for(int i=0;i<SND_PCHAN;i++)
+        // input channels are averaged together to reduce uncorrelated noise
+        lockin[i].set_scale(pow(0.5, SND_BITS-1)/SND_RCHAN);
     int lpf_count[SND_PCHAN] = { 0 };
 
     nfds_t nfds = sio_nfds(hdl);
@@ -79,8 +81,8 @@ static void dsp_thread()
 
         double lpf_k[SND_PCHAN];
         for(int i=0;i<SND_PCHAN;i++) {
-            dds[i].set_frequency(frequency[i]/SND_RATE);
-            lpf[i].set_bandwidth(lpf_bandwidth[i]/SND_RATE);
+            dds[i].ftw = lockin[i].ftw = frequency_to_ftw(frequency[i]/SND_RATE);
+            lockin[i].set_bandwidth(lpf_bandwidth[i]/SND_RATE);
         }
 
         if(revents & POLLOUT) {
@@ -98,18 +100,12 @@ static void dsp_thread()
         if(revents & POLLIN) {
             int32_t buf_in[SND_BUFLEN*SND_RCHAN];
             size_t read = sio_read(hdl, buf_in, sizeof(buf_in));
-            // input channels are averaged together to reduce uncorrelated noise
-            double scale = pow(0.5, SND_BITS-1)/SND_RCHAN;
             for(int i=0;i<SND_PCHAN;i++)
                 for(int j=0;j<read/(SND_RCHAN*sizeof(buf_in[0]));j++) {
                     double sample = 0.0;
                     for(int k=0;k<SND_RCHAN;k++)
                         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] -= dds[i].ftw;  // wraps on underflow
-
-                    double mag = std::abs(lpf[i].update(rotated));
+                    double mag = std::abs(lockin[i].update(sample));
 
                     lpf_count[i]++;
                     if(lpf_count[i] == 200) {