Finished stereo FM demod

src/dsp/convert/complex_to_real.h

@@ -0,0 +1,31 @@
+#pragma once
+#include "../processor.h"
+
+namespace dsp::convert {
+    class ComplexToReal : public Processor<complex_t, float> {
+        using base_type = Processor<complex_t, float>;
+    public:
+        ComplexToReal() {}
+
+        ComplexToReal(stream<complex_t>* in) { init(in); }
+
+        void init(stream<complex_t>* in) { base_type::init(in); }
+
+        inline static int process(int count, const complex_t* in, float* out) {
+            volk_32fc_deinterleave_real_32f(out, (lv_32fc_t*)in, count);
+            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;
+        }
+
+    };
+}

src/dsp/convert/l_r_to_stereo.h

@@ -17,7 +17,7 @@ namespace dsp::convert {
 
         void setInputR(stream<float>* r) { base_type::setInputB(r); }
 
-        inline int process(int count, const float* l, const float* r, stereo_t* out) {
+        static inline int process(int count, const float* l, const float* r, stereo_t* out) {
             volk_32f_x2_interleave_32fc((lv_32fc_t*)out, l, r, count);
             return count;
         }

src/dsp/convert/real_to_complex.h

@@ -16,7 +16,7 @@ namespace dsp::convert {
 
         void init(stream<float>* in) {
             nullBuf = allocBuffer<float>(STREAM_BUFFER_SIZE);
-            // TODO: Null out only needed part
+            clearBuffer(nullBuf, STREAM_BUFFER_SIZE);
             base_type::init(in);
         }
 

src/dsp/demod/broadcast_fm.h

@@ -1,39 +1,119 @@
 #pragma once
 #include "fm.h"
+#include "../taps/band_pass.h"
 #include "../filter/fir.h"
 #include "../loop/pll.h"
+#include "../convert/l_r_to_stereo.h"
 #include "../convert/real_to_complex.h"
+#include "../convert/complex_to_real.h"
+#include "../math/conjugate.h"
+#include "../math/delay.h"
+#include "../math/multiply.h"
+#include "../math/add.h"
+#include "../math/subtract.h"
 
 namespace dsp::demod {
-    class BroadcastFM : public Processor<complex_t, float> {
-        using base_type = Processor<complex_t, float>;
+    class BroadcastFM : public Processor<complex_t, stereo_t> {
+        using base_type = Processor<complex_t, stereo_t>;
     public:
         BroadcastFM() {}
 
-        BroadcastFM(stream<complex_t>* in) { init(in); }
+        BroadcastFM(stream<complex_t>* in, double deviation, double samplerate, bool stereo = true) { init(in, deviation, samplerate, stereo); }
 
-        virtual void init(stream<complex_t>* in) {
+        BroadcastFM() {
+            if (!base_type::_block_init) { return; }
+            base_type::stop();
+            freeBuffer(lmr);
+            freeBuffer(l);
+            freeBuffer(r);
+        }
+
+        virtual void init(stream<complex_t>* in, double deviation, double samplerate, bool stereo = true) {
+            _deviation = deviation;
+            _samplerate = samplerate;
+            _stereo = stereo;
+
+            demod.init(NULL, _deviation, _samplerate);
+            pilotFirTaps = taps::bandPass<complex_t>(18750.0, 19250.0, 3000.0, _samplerate);
+            pilotFir.init(NULL, pilotFirTaps);
+            rtoc.init(NULL);
+            pilotPLL.init(NULL, 0.1/*TODO*/, 0.0, math::freqToOmega(19000.0, _samplerate), math::freqToOmega(18750.0, _samplerate), math::freqToOmega(19250.0, _samplerate));
+            delay.init(NULL, pilotFirTaps.size / 2.0);
+
+            lmr = allocBuffer<float>(STREAM_BUFFER_SIZE);
+            l = allocBuffer<float>(STREAM_BUFFER_SIZE);
+            r = allocBuffer<float>(STREAM_BUFFER_SIZE);
 
             base_type::init(in);
         }
 
-        inline int process(int count, complex_t* in, float* out) {
-            // Demodulate and convert to complex
-            demod.process(count, in, demod.out.writeBuf);
-            rtoc.process(count, demod.out.writeBuf, rtoc.out.writeBuf);
+        void setDeviation(double deviation) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _deviation = deviation;
+            demod.setDeviation(_deviation, _samplerate);
+        }
 
-            // Filter out pilot and run through PLL
-            pilotFir.process(count, rtoc.out.writeBuf, pilotFir.out.writeBuf);
-            pilotPLL.process(count, pilotFir.out.writeBuf, pilotPLL.out.writeBuf);
+        void setSamplerate(double samplerate) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            _samplerate = samplerate;
 
-            return count;
+            demod.setDeviation(_deviation, _samplerate);
+            taps::free(pilotFirTaps);
+            pilotFirTaps = taps::bandPass<complex_t>(18750.0, 19250.0, 3000.0, samplerate);
+            pilotFir.setTaps(pilotFirTaps);
+            pilotPLL.setFrequencyLimits(math::freqToOmega(18750.0, _samplerate), math::freqToOmega(19250.0, _samplerate));
+            pilotPLL.setInitialFreq(math::freqToOmega(19000.0, _samplerate));
+            delay.setDelay(pilotFirTaps.size / 2);
+
+            reset();
+            base_type::tempStart();
         }
 
         void reset() {
             assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
             demod.reset();
             pilotFir.reset();
             pilotPLL.reset();
+            delay.reset();
+            base_type::tempStart();
+        }
+
+        inline int process(int count, complex_t* in, stereo_t* out) {
+            // Demodulate
+            demod.process(count, in, demod.out.writeBuf);
+            if (_stereo) {
+                // Convert to complex
+                rtoc.process(count, demod.out.writeBuf, rtoc.out.writeBuf);
+
+                // Filter out pilot and run through PLL
+                pilotFir.process(count, rtoc.out.writeBuf, pilotFir.out.writeBuf);
+                pilotPLL.process(count, pilotFir.out.writeBuf, pilotPLL.out.writeBuf);
+
+                // Conjugate PLL output to down convert the L-R signal
+                math::Conjugate::process(count, pilotPLL.out.writeBuf, pilotPLL.out.writeBuf);
+                math::Multiply<dsp::complex_t>::process(count, rtoc.out.writeBuf, pilotPLL.out.writeBuf, rtoc.out.writeBuf);
+
+                // Convert output back to real for further processing
+                convert::ComplexToReal::process(count, rtoc.out.writeBuf, lmr);
+
+                // Do L = (L+R) + (L-R), R = (L+R) - (L-R)
+                math::Add<float>::process(count, demod.out.writeBuf, lmr, l);
+                math::Subtract<float>::process(count, demod.out.writeBuf, lmr, r);
+
+                // Interleave into stereo
+                convert::LRToStereo::process(count, l, r, out);
+            }
+            else {
+                // Interleave raw MPX to stereo
+                convert::LRToStereo::process(count, demod.out.writeBuf, demod.out.writeBuf, out);
+            }
+
+            return count;
         }
 
         int run() {
@@ -48,11 +128,20 @@ namespace dsp::demod {
         }
 
     protected:
+        double _deviation;
+        double _samplerate;
+        bool _stereo;
+
         FM demod;
-        filter::FIR<complex_t, complex_t> pilotFir;
         tap<complex_t> pilotFirTaps;
+        filter::FIR<complex_t, complex_t> pilotFir;
         convert::RealToComplex rtoc;
         loop::PLL pilotPLL;
+        math::Delay<float> delay;
+
+        float* lmr;
+        float* l;
+        float* r;
 
     };
 }

src/dsp/math/conjugate.h

@@ -0,0 +1,28 @@
+#pragma once
+#include "../processor.h"
+
+namespace dsp::math {
+    class Conjugate : public Processor<complex_t, complex_t> {
+        using base_type = Processor<complex_t, complex_t>;
+    public:
+        Conjugate() {}
+
+        Conjugate(stream<complex_t>* in) { base_type::init(in); }
+
+        inline static int process(int count, const complex_t* in, complex_t* out) {
+            volk_32fc_conjugate_32fc((lv_32fc_t*)out, (lv_32fc_t*)in, count);
+            return count;
+        }
+
+        virtual 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;
+        }
+    };
+}

src/dsp/math/delay.h

@@ -12,15 +12,60 @@ namespace dsp::math {
 
         ~Delay() {
             if (!base_type::_block_init) { return; }
+            base_type::stop();
             freeBuffer(buffer);
         }
 
         void init(stream<T>* in, int delay) {
-
+            buffer = allocBuffer(STREAM_BUFFER_SIZE + 64000);
+            setDelay(delay);
             base_type::init(in);
         }
 
+        void setDelay(int delay) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            _delay = delay;
+            bufStart = &buffer[_delay];
+            reset();
+            base_type::tempStart();
+        }
+
+        void reset() {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            clearBuffer(buffer, _delay);
+            base_type::tempStart();
+        }
+
+        inline int process(int count, const T* in, T* out) {
+            // Copy data into delay buffer
+            memcpy(bufStart, in, count * sizeof(T));
+
+            // Copy data out of the delay buffer
+            memcpy(out, buffer);
+
+            // Move end of the delay buffer to the front
+            memmove(buffer, &buffer[count], _delay * sizeof(T));
+
+            return count;
+        }
+
+        virtual 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:
+        int _delay;
         T* buffer;
         T* bufStart;
     };

src/dsp/math/multiply.h

@@ -9,7 +9,7 @@ namespace dsp::math {
 
         Multiply(stream<T>* a, stream<T>* b) { base_type::init(a, b); }
 
-        inline int process(int count, const T* a, const T* b, T* out) {
+        inline static int process(int count, const T* a, const T* b, T* out) {
             if constexpr (std::is_same_v<T, complex_t>) {
                 volk_32fc_x2_multiply_32fc((lv_32fc_t*)out, (lv_32fc_t*)a, (lv_32fc_t*)b, count);
             }

src/dsp/math/subtract.h

@@ -9,7 +9,7 @@ namespace dsp::math {
 
         Subtract(stream<T>* a, stream<T>* b) { init(a, b); }
 
-        inline int process(int count, const T* a, const T* b, T* out) {
+        inline static int process(int count, const T* a, const T* b, T* out) {
             if constexpr (std::is_same_v<T, complex_t> || std::is_same_v<T, stereo_t>) {
                 volk_32f_x2_subtract_32f((float*)out, (float*)a, (float*)b, count * 2);
             }