even more stuff

core/src/dsp/loop/agc.h

@@ -0,0 +1,81 @@
+#pragma once
+#include "../processor.h"
+
+namespace dsp::loop {
+    template <class T>
+    class AGC : public Processor<T, T> {
+        using base_type = Processor<T, T>;
+    public:
+        AGC() {}
+
+        AGC(stream<T>* in) { init(in); }
+
+        void init(stream<T>* in, double setPoint, double rate, double initGain = 1.0) {
+            _setPoint = setPoint;
+            _rate = rate;
+            _initGain = initGain;
+            gain = _initGain;
+            base_type::init(in);
+        }
+
+        void setSetPoint(double setPoint) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _setPoint = setPoint;
+        }
+
+        void setRate(double rate) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _rate = rate;
+        }
+
+        void setInitialGain(double initGain) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _initGain = initGain;
+        }
+
+        void reset() {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            gain = _initGain;
+        }
+
+        inline int process(int count, T* in, T* out) {
+            for (int i = 0; i < count; i++) {
+                // Scale output by gain
+                out[i] = in[i] * gain;
+
+                // Update gain according to setpoint and rate
+                if constexpr (std::is_same_v<T, complex_t>) {
+                    gain += (_setPoint - out[i].amplitude()) * _rate;
+                }
+                if constexpr (std::is_same_v<T, float>) {
+                    gain += (_setPoint - fabsf(out[i])) * _rate;
+                }
+            }
+            printf("%f\n", gain);
+            return count;
+        }
+
+        int run() {
+            int count = base_type::_in->read();
+            if (count < 0) { return -1; }
+
+            process(count, base_type::_in->readBuf, base_type::out.writeBuf);
+
+            base_type::_in->flush();
+            if (!base_type::out.swap(count)) { return -1; }
+            return count;
+        }
+
+    protected:
+        float _setPoint;
+        float _rate;
+        float _initGain;
+
+        float gain;
+
+    };
+}

core/src/dsp/loop/phase_control_loop.h

@@ -0,0 +1,85 @@
+#pragma once
+#include <math.h>
+#include <assert.h>
+#include "../types.h"
+
+namespace dsp::loop {
+    template<class T>
+    class PhaseControlLoop {
+    public:
+        PhaseControlLoop() {}
+
+        PhaseControlLoop(T alpha, T beta, T phase, T minPhase, T maxPhase, T freq, T minFreq, T maxFreq) {
+            init(alpha, beta, phase, minPhase, maxPhase, freq, minFreq, maxFreq);
+        }
+
+        void init(T alpha, T beta, T phase, T minPhase, T maxPhase, T freq, T minFreq, T maxFreq) {
+            assert(maxPhase > minPhase);
+            assert(maxFreq > minFreq);
+            _alpha = alpha;
+            _beta = beta;
+            this->phase = phase;
+            _minPhase = minPhase;
+            _maxPhase = maxPhase;
+            this->freq = freq;
+            _minFreq = minFreq;
+            _maxFreq = maxFreq;
+
+            phaseDelta = _maxPhase - _minPhase;
+        }
+
+        static inline void criticallyDamped(T bandwidth, T& alpha, T& beta) {
+            T dampningFactor = sqrt(2.0) / 2.0;
+            T denominator = (1.0 + 2.0*dampningFactor*bandwidth + bandwidth*bandwidth);
+            alpha = (4 * dampningFactor * bandwidth) / denominator;
+            beta = (4 * bandwidth * bandwidth) / denominator;
+        }
+
+        void setPhaseLimits(T minPhase, T maxPhase) {
+            assert(maxPhase > minPhase);
+            _minPhase = minPhase;
+            _maxPhase = maxPhase;
+            phaseDelta = _maxPhase - _minPhase;
+            clampPhase();
+        }
+
+        void setFreqLimits(T minFreq, T maxFreq) {
+            assert(maxFreq > minFreq);
+            _minFreq = minFreq;
+            _maxFreq = maxFreq;
+            clampFreq();
+        }
+
+        inline void advance(T error) {
+            // Increment and clamp frequency
+            freq += _beta * error;
+            clampFreq();
+
+            // Increment and clamp phase
+            phase += freq + (_alpha * error);
+            clampPhase();
+        }
+
+        T freq;
+        T phase;
+
+    protected:
+        inline void clampFreq() {
+            if (freq > _maxFreq) { freq = _maxFreq; }
+            else if (freq < _minFreq) { freq = _minFreq; }
+        }
+
+        inline void clampPhase() {
+            while (phase > _maxPhase) { phase -= phaseDelta; }
+            while (phase < _minPhase) { phase += phaseDelta; }
+        }
+
+        T _alpha;
+        T _beta;
+        T _minPhase;
+        T _maxPhase;
+        T _minFreq;
+        T _maxFreq;
+        T phaseDelta;
+    };
+}

core/src/dsp/loop/pll.h

@@ -0,0 +1,80 @@
+#pragma once
+#include "../processor.h"
+#include "../math/norm_phase_diff.h"
+#include "../math/phasor.h"
+#include "phase_control_loop.h"
+
+namespace dsp::loop {
+    class PLL : public Processor<complex_t, complex_t> {
+        using base_type = Processor<complex_t, complex_t>;
+    public:
+        PLL() {}
+
+        PLL(stream<complex_t>* in, double bandwidth, double initPhase = 0.0, double initFreq = 0.0, double minFreq = -FL_M_PI, double maxFreq = FL_M_PI) { init(in, bandwidth, initFreq, initPhase, minFreq, maxFreq); }
+
+        void init(stream<complex_t>* in, double bandwidth, double initPhase = 0.0, double initFreq = 0.0, double minFreq = -FL_M_PI, double maxFreq = FL_M_PI) {
+            _initPhase = initPhase;
+            _initFreq = initFreq;
+
+            // Init phase control loop
+            float alpha, beta;
+            PhaseControlLoop<float>::criticallyDamped(bandwidth, alpha, beta);
+            pcl.init(alpha, beta, initPhase, -FL_M_PI, FL_M_PI, initFreq, minFreq, maxFreq);
+            
+            base_type::init(in);
+        }
+
+        void setInitialPhase(double initPhase) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _initPhase = initPhase;
+        }
+
+        void setInitialFreq(double initFreq) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _initFreq = initFreq;
+        }
+
+        void setFrequencyLimits(double minFreq, double maxFreq) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            pcl.setFreqLimits(minFreq, maxFreq);
+        }
+
+        void reset() {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            pcl.phase = _initPhase;
+            pcl.freq = _initFreq;
+            base_type::tempStart();
+        }
+
+        inline int process(int count, complex_t* in, complex_t* out) {
+            for (int i = 0; i < count; i++) {
+                out[i] = math::phasor(pcl.phase);
+                pcl.advance(math::normPhaseDiff(in[i].phase() - pcl.phase));
+            }
+            return count;
+        }
+
+        int run() {
+            int count = base_type::_in->read();
+            if (count < 0) { return -1; }
+
+            process(count, base_type::_in->readBuf, base_type::out.writeBuf);
+
+            base_type::_in->flush();
+            if (!base_type::out.swap(count)) { return -1; }
+            return count;
+        }
+
+    protected:
+        PhaseControlLoop<float> pcl;
+        float _initPhase;
+        float _initFreq;
+        complex_t lastVCO = { 1.0f, 0.0f };
+
+    };
+}