More work

core/src/dsp/demod/am.h

@@ -2,10 +2,14 @@
 #include "../processor.h"
 #include "../loop/agc.h"
 #include "../correction/dc_blocker.h"
+#include "../convert/mono_to_stereo.h"
+#include "../filter/fir.h"
+#include "../taps/low_pass.h"
 
 namespace dsp::demod {
-    class AM : public Processor<dsp::complex_t, float> {
-        using base_type = Processor<dsp::complex_t, float>;
+    template <class T>
+    class AM : public Processor<dsp::complex_t, T> {
+        using base_type = Processor<dsp::complex_t, T>;
     public:
         enum AGCMode {
             CARRIER,
@@ -14,17 +18,30 @@ namespace dsp::demod {
 
         AM() {}
 
-        AM(stream<complex_t>* in, AGCMode agcMode, double agcAttack, double agcDecay, double dcBlockRate) { init(in, agcMode, agcAttack, agcDecay, dcBlockRate); }
+        AM(stream<complex_t>* in, AGCMode agcMode, double bandwidth, double agcAttack, double agcDecay, double dcBlockRate, double samplerate) { init(in, agcMode, bandwidth, agcAttack, agcDecay, dcBlockRate, samplerate); }
 
-        void init(stream<complex_t>* in, AGCMode agcMode, double agcAttack, double agcDecay, double dcBlockRate) {
+        ~AM() {
+            if (!base_type::_block_init) { return; }
+            base_type::stop();
+            taps::free(lpfTaps);
+        }
+
+        void init(stream<complex_t>* in, AGCMode agcMode, double bandwidth, double agcAttack, double agcDecay, double dcBlockRate, double samplerate) {
             _agcMode = agcMode;
+            _bandwidth = bandwidth;
+            _samplerate = samplerate;
 
             carrierAgc.init(NULL, 1.0, agcAttack, agcDecay, 10e6, 10.0);
             audioAgc.init(NULL, 1.0, agcAttack, agcDecay, 10e6, 10.0);
             dcBlock.init(NULL, dcBlockRate);
+            lpfTaps = taps::lowPass(bandwidth / 2.0, (bandwidth / 2.0) * 0.1, samplerate);
+            lpf.init(NULL, lpfTaps);
 
-            audioAgc.out.free();
+            if constexpr (std::is_same_v<T, float>) {
+                audioAgc.out.free();
+            }
             dcBlock.out.free();
+            lpf.out.free();
             
             base_type::init(in);
         }
@@ -38,6 +55,17 @@ namespace dsp::demod {
             base_type::tempStart();
         }
 
+        void setBandwidth(double bandwidth) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            if (bandwidth == _bandwidth) { return; }
+            _bandwidth = bandwidth;
+            std::lock_guard<std::mutex> lck2(lpfMtx);
+            taps::free(lpfTaps);
+            lpfTaps = taps::lowPass(_bandwidth / 2.0, (_bandwidth / 2.0) * 0.1, _samplerate);
+            lpf.setTaps(lpfTaps);
+        }
+
         void setAGCAttack(double attack) {
             assert(base_type::_block_init);
             std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
@@ -58,6 +86,8 @@ namespace dsp::demod {
             dcBlock.setRate(rate);
         }
 
+        // TODO: Implement setSamplerate
+
         void reset() {
             assert(base_type::_block_init);
             std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
@@ -68,20 +98,35 @@ namespace dsp::demod {
             base_type::tempStart();
         }
 
-        int process(int count, complex_t* in, float* out) {
+        int process(int count, complex_t* in, T* out) {
             // Apply carrier AGC if needed
             if (_agcMode == AGCMode::CARRIER) {
                 carrierAgc.process(count, in, carrierAgc.out.writeBuf);
                 in = carrierAgc.out.writeBuf;
             }
 
-            // Get magnitude of each sample and remove DC (TODO: use block instead)
-            volk_32fc_magnitude_32f(out, (lv_32fc_t*)in, count);
-            dcBlock.process(count, out, out);
-
-            // Apply audio AGC if needed
-            if (_agcMode == AGCMode::AUDIO) {
-                audioAgc.process(count, out, out);
+            if constexpr (std::is_same_v<T, float>) {
+                volk_32fc_magnitude_32f(out, (lv_32fc_t*)in, count);
+                dcBlock.process(count, out, out);
+                if (_agcMode == AGCMode::AUDIO) {
+                    audioAgc.process(count, out, out);
+                }
+                {
+                    std::lock_guard<std::mutex> lck(lpfMtx);
+                    lpf.process(count, out, out);
+                }
+            }
+            if constexpr (std::is_same_v<T, stereo_t>) {
+                volk_32fc_magnitude_32f(audioAgc.out.writeBuf, (lv_32fc_t*)in, count);
+                dcBlock.process(count, audioAgc.out.writeBuf, audioAgc.out.writeBuf);
+                if (_agcMode == AGCMode::AUDIO) {
+                    audioAgc.process(count, audioAgc.out.writeBuf, audioAgc.out.writeBuf);
+                }
+                {
+                    std::lock_guard<std::mutex> lck(lpfMtx);
+                    lpf.process(count, audioAgc.out.writeBuf, audioAgc.out.writeBuf);
+                }
+                convert::MonoToStereo::process(count, audioAgc.out.writeBuf, out);
             }
 
             return count;
@@ -101,9 +146,15 @@ namespace dsp::demod {
     protected:
         AGCMode _agcMode;
 
+        double _samplerate;
+        double _bandwidth;
+
         loop::AGC<complex_t> carrierAgc;
         loop::AGC<float> audioAgc;
         correction::DCBlocker<float> dcBlock;
+        tap<float> lpfTaps;
+        filter::FIR<float, float> lpf;
+        std::mutex lpfMtx;
 
     };
 }

core/src/dsp/demod/broadcast_fm.h

@@ -1,5 +1,5 @@
 #pragma once
-#include "fm.h"
+#include "quadrature.h"
 #include "../taps/low_pass.h"
 #include "../taps/band_pass.h"
 #include "../filter/fir.h"
@@ -193,7 +193,7 @@ namespace dsp::demod {
         bool _stereo;
         bool _lowPass = true;
 
-        FM demod;
+        Quadrature demod;
         tap<complex_t> pilotFirTaps;
         filter::FIR<complex_t, complex_t> pilotFir;
         convert::RealToComplex rtoc;

core/src/dsp/demod/cw.h

@@ -0,0 +1,87 @@
+#pragma once
+#include "../processor.h"
+#include "../channel/frequency_xlator.h"
+#include "../convert/complex_to_real.h"
+#include "../loop/agc.h"
+#include "../convert/mono_to_stereo.h"
+
+namespace dsp::demod {
+    template <class T>
+    class CW : public Processor<complex_t, T> {
+        using base_type = Processor<complex_t, T>;
+    public:
+        CW() {}
+        
+        CW(stream<complex_t>* in, double tone, double agcAttack, double agcDecay, double samplerate) { init(in, tone, agcAttack, agcDecay, samplerate); }
+
+        void init(stream<complex_t>* in, double tone, double agcAttack, double agcDecay, double samplerate) {
+            _tone = tone;
+            
+            xlator.init(NULL, tone, samplerate);
+            agc.init(NULL, 1.0, agcAttack, agcDecay, 10e6, 10.0);
+
+            if constexpr (std::is_same_v<T, float>) {
+                agc.out.free();
+            }
+
+            base_type::init(in);
+        }
+
+        void setTone(double tone) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _tone = tone;
+            xlator.setOffset(_tone);
+        }
+
+        void setAGCAttack(double attack) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            agc.setAttack(attack);
+        }
+
+        void setAGCDecay(double decay) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            agc.setDecay(decay);
+        }
+
+        void setSamplerate(double samplerate) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            xlator.setOffset(_tone, samplerate);
+        }
+
+        inline int process(int count, const complex_t* in, T* out) {
+            xlator.process(count, in, xlator.out.writeBuf);
+            if constexpr (std::is_same_v<T, float>) {
+                dsp::convert::ComplexToReal::process(count, xlator.out.writeBuf, out);
+                agc.process(count, out, out);
+            }
+            if constexpr (std::is_same_v<T, stereo_t>) {
+                dsp::convert::ComplexToReal::process(count, xlator.out.writeBuf, agc.out.writeBuf);
+                agc.process(count, agc.out.writeBuf, agc.out.writeBuf);
+                convert::MonoToStereo::process(count, agc.out.writeBuf, out);
+            }
+            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;
+        }
+
+    private:
+        double _tone;
+
+        dsp::channel::FrequencyXlator xlator;
+        dsp::loop::AGC<float> agc;
+
+    };
+}

core/src/dsp/demod/fm.h

@@ -1,54 +1,98 @@
 #pragma once
 #include "../processor.h"
-#include "../math/fast_atan2.h"
-#include "../math/freq_to_omega.h"
-#include "../math/norm_phase_diff.h"
+#include "quadrature.h"
+#include "../filter/fir.h"
+#include "../taps/low_pass.h"
+#include "../convert/mono_to_stereo.h"
 
 namespace dsp::demod {
-    class FM : public Processor<complex_t, float> {
-        using base_type = Processor<complex_t, float>;
+    template <class T>
+    class FM : public dsp::Processor<dsp::complex_t, T> {
+        using base_type = dsp::Processor<dsp::complex_t, T>;
     public:
         FM() {}
 
-        FM(stream<complex_t>* in, double deviation) { init(in, deviation); }
+        FM(dsp::stream<dsp::complex_t>* in, double samplerate, double bandwidth, bool lowPass) { init(in, samplerate, bandwidth, lowPass); }
 
-        FM(stream<complex_t>* in, double deviation, double samplerate) { init(in, deviation, samplerate); }
-
-        
-        virtual void init(stream<complex_t>* in, double deviation) {
-            _invDeviation = 1.0 / deviation;
-            base_type::init(in);
+        ~FM() {
+            if (!base_type::_block_init) { return; }
+            base_type::stop();
+            dsp::taps::free(lpfTaps);
         }
 
-        virtual void init(stream<complex_t>* in, double deviation, double samplerate) {
-            init(in, math::freqToOmega(deviation, samplerate));
-        }
+        void init(dsp::stream<dsp::complex_t>* in, double samplerate, double bandwidth, bool lowPass) {
+            _samplerate = samplerate;
+            _bandwidth = bandwidth;
+            _lowPass = lowPass;
 
-        void setDeviation(double deviation) {
-            assert(base_type::_block_init);
-            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
-            _invDeviation = 1.0 / deviation;
-        }
+            demod.init(NULL, bandwidth / 2.0, _samplerate);
+            lpfTaps = dsp::taps::lowPass(_bandwidth / 2.0, (_bandwidth / 2.0) * 0.1, _samplerate);
+            lpf.init(NULL, lpfTaps);
 
-        void setDeviation(double deviation, double samplerate) {
-            assert(base_type::_block_init);
-            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
-            _invDeviation = 1.0 / math::freqToOmega(deviation, samplerate);
-        }
-
-        inline int process(int count, complex_t* in, float* out) {
-            for (int i = 0; i < count; i++) {
-                float cphase = in[i].phase();
-                out[i] = math::normPhaseDiff(cphase - phase) * _invDeviation;
-                phase = cphase;
+            if constexpr (std::is_same_v<T, float>) {
+                demod.out.free();
             }
-            return count;
+            lpf.out.free();
+        }
+
+        void setSamplerate(double samplerate) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            _samplerate = samplerate;
+            demod.setDeviation(_bandwidth / 2.0, _samplerate);
+            dsp::taps::free(lpfTaps);
+            lpfTaps = dsp::taps::lowPass(_bandwidth / 2.0, (_bandwidth / 2.0) * 0.1, _samplerate);
+            lpf.setTaps(lpfTaps);
+            base_type::tempStart();
+        }
+
+        void setBandwidth(double bandwidth) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            if (bandwidth == _bandwidth) { return; }
+            _bandwidth = bandwidth;
+            std::lock_guard<std::mutex> lck2(lpfMtx);
+            demod.setDeviation(_bandwidth / 2.0, _samplerate);
+            dsp::taps::free(lpfTaps);
+            lpfTaps = dsp::taps::lowPass(_bandwidth / 2, (_bandwidth / 2) * 0.1, _samplerate);
+            lpf.setTaps(lpfTaps);
+        }
+
+        void setLowPass(bool lowPass) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            std::lock_guard<std::mutex> lck2(lpfMtx);
+            _lowPass = lowPass;
+            lpf.reset();
         }
 
         void reset() {
             assert(base_type::_block_init);
             std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
-            phase = 0.0f;
+            base_type::tempStop();
+            demod.reset();
+            lpf.reset();
+            base_type::tempStart();
+        }
+
+        inline int process(int count, dsp::complex_t* in, T* out) {
+            if constexpr (std::is_same_v<T, float>) {
+                demod.process(count, in, out);
+                if (_lowPass) {
+                    std::lock_guard<std::mutex> lck(lpfMtx);
+                    lpf.process(count, out, out);
+                }
+            }
+            if constexpr (std::is_same_v<T, stereo_t>) {
+                demod.process(count, in, demod.out.writeBuf);
+                if (_lowPass) {
+                    std::lock_guard<std::mutex> lck(lpfMtx);
+                    lpf.process(count, demod.out.writeBuf, demod.out.writeBuf);
+                }
+                convert::MonoToStereo::process(count, demod.out.writeBuf, out);
+            }
+            return count;
         }
 
         int run() {
@@ -62,8 +106,14 @@ namespace dsp::demod {
             return count;
         }
 
-    protected:
-        float _invDeviation;
-        float phase = 0.0f;
+    private:
+        double _samplerate;
+        double _bandwidth;
+        bool _lowPass;
+
+        Quadrature demod;
+        tap<float> lpfTaps;
+        filter::FIR<float, float> lpf;
+        std::mutex lpfMtx;
     };
 }

core/src/dsp/demod/narrow_fm.h

@@ -0,0 +1,114 @@
+#pragma once
+#include "../processor.h"
+#include "fm.h"
+#include "../filter/fir.h"
+#include "../taps/low_pass.h"
+#include "../convert/mono_to_stereo.h"
+
+namespace dsp::demod {
+    template <class T>
+    class NarrowFM : public dsp::Processor<dsp::complex_t, T> {
+        using base_type = dsp::Processor<dsp::complex_t, T>;
+    public:
+        NarrowFM() {}
+
+        NarrowFM(dsp::stream<dsp::complex_t>* in, double samplerate, double bandwidth, bool lowPass) { init(in, samplerate, bandwidth, lowPass); }
+
+        ~NarrowFM() {
+            if (!base_type::_block_init) { return; }
+            base_type::stop();
+            dsp::taps::free(lpfTaps);
+        }
+
+        void init(dsp::stream<dsp::complex_t>* in, double samplerate, double bandwidth, bool lowPass) {
+            _samplerate = samplerate;
+            _bandwidth = bandwidth;
+            _lowPass = lowPass;
+
+            demod.init(NULL, bandwidth / 2.0, _samplerate);
+            lpfTaps = dsp::taps::lowPass(_bandwidth / 2, (_bandwidth / 2) * 0.1, _samplerate);
+            lpf.init(NULL, lpfTaps);
+
+            if constexpr (std::is_same_v<T, float>) {
+                demod.out.free();
+            }
+            lpf.out.free();
+        }
+
+        void setSamplerate(double samplerate) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            _samplerate = samplerate;
+            demod.setDeviation(_bandwidth / 2.0, _samplerate);
+            dsp::taps::free(lpfTaps);
+            lpfTaps = dsp::taps::lowPass(_bandwidth / 2, (_bandwidth / 2) * 0.1, _samplerate);
+            lpf.setTaps(lpfTaps);
+            base_type::tempStart();
+        }
+
+        void setBandwidth(double bandwidth) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            if (bandwidth == _bandwidth) { return; }
+            _bandwidth = bandwidth;
+            demod.setDeviation(_bandwidth / 2.0, _samplerate);
+            dsp::taps::free(lpfTaps);
+            lpfTaps = dsp::taps::lowPass(_bandwidth / 2, (_bandwidth / 2) * 0.1, _samplerate);
+            lpf.setTaps(lpfTaps);
+        }
+
+        void setLowPass(bool lowPass) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _lowPass = lowPass;
+            lpf.reset();
+        }
+
+        void reset() {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            demod.reset();
+            lpf.reset();
+            base_type::tempStart();
+        }
+
+        inline int process(int count, dsp::complex_t* in, float* out) {
+            if constexpr (std::is_same_v<T, float>) {
+                demod.process(count, in, out);
+                if (_lowPass) {
+                    lpf.process(count, out, out);
+                }
+            }
+            if constexpr (std::is_same_v<T, stereo_t>) {
+                demod.process(count, in, demod.out.writeBuf);
+                if (_lowPass) {
+                    lpf.process(count, demod.out.writeBuf, demod.out.writeBuf);
+                }
+                convert::MonoToStereo::process(count, demod.out.writeBuf, out);
+            }
+            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;
+        }
+
+    private:
+        double _samplerate;
+        double _bandwidth;
+        bool _lowPass;
+
+        dsp::demod::FM demod;
+        dsp::tap<float> lpfTaps;
+        dsp::filter::FIR<float, float> lpf;
+    };
+}

core/src/dsp/demod/quadrature.h

@@ -0,0 +1,69 @@
+#pragma once
+#include "../processor.h"
+#include "../math/fast_atan2.h"
+#include "../math/freq_to_omega.h"
+#include "../math/norm_phase_diff.h"
+
+namespace dsp::demod {
+    class Quadrature : public Processor<complex_t, float> {
+        using base_type = Processor<complex_t, float>;
+    public:
+        Quadrature() {}
+
+        Quadrature(stream<complex_t>* in, double deviation) { init(in, deviation); }
+
+        Quadrature(stream<complex_t>* in, double deviation, double samplerate) { init(in, deviation, samplerate); }
+
+        
+        virtual void init(stream<complex_t>* in, double deviation) {
+            _invDeviation = 1.0 / deviation;
+            base_type::init(in);
+        }
+
+        virtual void init(stream<complex_t>* in, double deviation, double samplerate) {
+            init(in, math::freqToOmega(deviation, samplerate));
+        }
+
+        void setDeviation(double deviation) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _invDeviation = 1.0 / deviation;
+        }
+
+        void setDeviation(double deviation, double samplerate) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _invDeviation = 1.0 / math::freqToOmega(deviation, samplerate);
+        }
+
+        inline int process(int count, complex_t* in, float* out) {
+            for (int i = 0; i < count; i++) {
+                float cphase = in[i].phase();
+                out[i] = math::normPhaseDiff(cphase - phase) * _invDeviation;
+                phase = cphase;
+            }
+            return count;
+        }
+
+        void reset() {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            phase = 0.0f;
+        }
+
+        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 _invDeviation;
+        float phase = 0.0f;
+    };
+}

core/src/dsp/demod/ssb.h

@@ -3,10 +3,12 @@
 #include "../channel/frequency_xlator.h" 
 #include "../convert/complex_to_real.h"
 #include "../loop/agc.h"
+#include "../convert/mono_to_stereo.h"
 
 namespace dsp::demod {
-    class SSB : public Processor<complex_t, float> {
-        using base_type = Processor<complex_t, float>;
+    template <class T>
+    class SSB : public Processor<complex_t, T> {
+        using base_type = Processor<complex_t, T>;
     public:
         enum Mode {
             USB,
@@ -26,7 +28,9 @@ namespace dsp::demod {
             xlator.init(NULL, getTranslation(), _samplerate);
             agc.init(NULL, 1.0, agcAttack, agcDecay, 10e6, 10.0);
 
-            agc.out.free();
+            if constexpr (std::is_same_v<T, float>) {
+                agc.out.free();
+            }
 
             base_type::init(in);
         }
@@ -70,15 +74,19 @@ namespace dsp::demod {
             agc.setDecay(decay);
         }
 
-        int process(int count, const complex_t* in, float* out) {
+        int process(int count, const complex_t* in, T* out) {
             // Move back sideband
             xlator.process(count, in, xlator.out.writeBuf);
 
-            // Extract the real component
-            convert::ComplexToReal::process(count, xlator.out.writeBuf, out);
-
-            // Apply AGC
-            agc.process(count, out, out);
+            if constexpr (std::is_same_v<T, float>) {
+                convert::ComplexToReal::process(count, xlator.out.writeBuf, out);
+                agc.process(count, out, out);
+            }
+            if constexpr (std::is_same_v<T, stereo_t>) {
+                convert::ComplexToReal::process(count, xlator.out.writeBuf, agc.out.writeBuf);
+                agc.process(count, agc.out.writeBuf, agc.out.writeBuf);
+                convert::MonoToStereo::process(count, agc.out.writeBuf, out);
+            }
 
             return count;
         }

core/src/dsp/sink/ring_buffer.h

@@ -17,7 +17,7 @@ namespace dsp::sink {
             int count = base_type::_in->read();
             if (count < 0) { return -1; }
 
-            if (data.write(_in->readBuf, count) < 0) { return -1; }
+            if (data.write(base_type::_in->readBuf, count) < 0) { return -1; }
 
             base_type::_in->flush();
             return count;