diff --git a/dsp_lib.h b/dsp_lib.h index 3cfe7b9..b8d7553 100644 --- a/dsp_lib.h +++ b/dsp_lib.h @@ -1,36 +1,60 @@ #pragma once #include +#include + +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 +template 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 +class Lockin { + private: + double scale; + uint32_t phase = 0; + Lowpass, 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 update(double x) { + std::complex rotated; + rotated = x*std::polar(scale, phase*2.0*M_PI/(double)UINT32_MAX); + phase -= ftw; // wraps on underflow + return lpf.update(rotated); + } +}; diff --git a/sndlock.cpp b/sndlock.cpp index e2a803b..46d434f 100644 --- a/sndlock.cpp +++ b/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, 4> lpf[SND_PCHAN]; + Lockin<4> lockin[SND_PCHAN]; + for(int i=0;i 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) {