bugfix + preparations for stereo FM

core/src/dsp/audio.h

@@ -52,6 +52,8 @@ namespace dsp {
         void init(stream<float>* in_left, stream<float>* in_right) {
             _in_left = in_left;
             _in_right = in_right;
+            nullbuf = new float[STREAM_BUFFER_SIZE];
+            for (int i = 0; i < STREAM_BUFFER_SIZE; i++) { nullbuf[i] = 0; }
             generic_block<ChannelsToStereo>::registerInput(_in_left);
             generic_block<ChannelsToStereo>::registerInput(_in_right);
             generic_block<ChannelsToStereo>::registerOutput(&out);
@@ -95,6 +97,8 @@ namespace dsp {
         stream<float>* _in_left;
         stream<float>* _in_right;
 
+        float* nullbuf;
+
     };
 
     class StereoToMono : public generic_block<StereoToMono> {

core/src/dsp/demodulator.h

@@ -8,6 +8,8 @@
 #include <dsp/pll.h>
 #include <dsp/clock_recovery.h>
 #include <dsp/math.h>
+#include <dsp/convertion.h>
+#include <dsp/audio.h>
 
 #define FAST_ATAN2_COEF1    FL_M_PI / 4.0f
 #define FAST_ATAN2_COEF2    3.0f * FAST_ATAN2_COEF1
@@ -663,4 +665,119 @@ namespace dsp {
         float _muGain;
         float _omegaRelLimit;
     };
+
+    class StereoFMDemod : public generic_hier_block<StereoFMDemod> {
+    public:
+        StereoFMDemod() {}
+
+        StereoFMDemod(stream<float>* input, float sampleRate, float baudRate, float agcRate = 0.02e-3f, float pllLoopBandwidth = (0.06f*0.06f) / 4.0f, int rrcTapCount = 31, float rrcAlpha = 0.6f, float omegaGain = (0.01*0.01) / 4, float muGain = 0.01f, float omegaRelLimit = 0.005f) {
+            init(input, sampleRate);
+        }
+
+        void init(stream<float>* input, float sampleRate) {
+            _sampleRate = sampleRate;
+
+            r2c.init(input);
+            split.init(&r2c.out);
+
+            split.bindStream(&APlusBIn);
+            split.bindStream(&AMinusBIn);
+            split.bindStream(&PilotIn);
+
+            APlusBWin.init(0, 17000, 2500, _sampleRate);
+            AMinusBWin.init(38000, 38000 + 17000, 2500, _sampleRate);
+            PilotWin.init(18500, 19500, 1500, _sampleRate);
+
+            APlusBFir.init(&APlusBIn, &APlusBWin);
+            AMinusBFir.init(&AMinusBIn, &AMinusBWin);
+            PilotFir.init(&PilotIn, &PilotWin);
+
+            pll.init(&PilotFir.out, 0.1f);
+
+            p2s.init(&pll.out);
+
+            mixer.init(&AMinusBFir.out, &p2s.out);
+
+            c2rAPlusB.init(&APlusBFir.out);
+            c2rAMinusB.init(&mixer.out);
+
+            APlusBSplit.init(&c2rAPlusB.out);
+            AMinusBSplit.init(&c2rAMinusB.out);
+
+            APlusBSplit.bindStream(&AdderAPlusB);
+            APlusBSplit.bindStream(&SubtractorAPlusB);
+            AMinusBSplit.bindStream(&AdderAMinusB);
+            AMinusBSplit.bindStream(&SubtractorAMinusB);
+
+            Adder.init(&AdderAPlusB, &AdderAMinusB);
+            Subtractor.init(&SubtractorAPlusB, &SubtractorAMinusB);
+
+            c2s.init(&Adder.out, &Subtractor.out);
+
+            out = &c2s.out;
+
+            generic_hier_block<StereoFMDemod>::registerBlock(&r2c);
+            generic_hier_block<StereoFMDemod>::registerBlock(&split);
+            generic_hier_block<StereoFMDemod>::registerBlock(&APlusBFir);
+            generic_hier_block<StereoFMDemod>::registerBlock(&AMinusBFir);
+            generic_hier_block<StereoFMDemod>::registerBlock(&PilotFir);
+            generic_hier_block<StereoFMDemod>::registerBlock(&pll);
+            generic_hier_block<StereoFMDemod>::registerBlock(&p2s);
+            generic_hier_block<StereoFMDemod>::registerBlock(&mixer);
+            generic_hier_block<StereoFMDemod>::registerBlock(&c2rAPlusB);
+            generic_hier_block<StereoFMDemod>::registerBlock(&c2rAMinusB);
+            generic_hier_block<StereoFMDemod>::registerBlock(&APlusBSplit);
+            generic_hier_block<StereoFMDemod>::registerBlock(&AMinusBSplit);
+            generic_hier_block<StereoFMDemod>::registerBlock(&Adder);
+            generic_hier_block<StereoFMDemod>::registerBlock(&Subtractor);
+            generic_hier_block<StereoFMDemod>::registerBlock(&c2s);
+            generic_hier_block<StereoFMDemod>::_block_init = true;
+        }
+
+        void setInput(stream<float>* input) {
+            assert(generic_hier_block<StereoFMDemod>::_block_init);
+            r2c.setInput(input);
+        }
+
+        stream<stereo_t>* out = NULL;
+
+    private:
+        filter_window::BandPassBlackmanWindow APlusBWin;
+        filter_window::BandPassBlackmanWindow AMinusBWin;
+        filter_window::BandPassBlackmanWindow PilotWin;
+
+        RealToComplex r2c;
+        Splitter<complex_t> split;
+
+        stream<complex_t> APlusBIn;
+        stream<complex_t> AMinusBIn;
+        stream<complex_t> PilotIn;
+        ComplexFIR APlusBFir;
+        ComplexFIR AMinusBFir;
+        ComplexFIR PilotFir;
+
+        PLL pll;
+
+        BFMPilotToStereo p2s;
+
+        Multiply<complex_t> mixer;
+
+        ComplexToReal c2rAPlusB;
+        ComplexToReal c2rAMinusB;
+
+        Splitter<float> APlusBSplit;
+        Splitter<float> AMinusBSplit;
+
+        stream<float> AdderAPlusB;
+        stream<float> AdderAMinusB;
+        stream<float> SubtractorAPlusB;
+        stream<float> SubtractorAMinusB;
+
+        Add<float> Adder;
+        Add<float> Subtractor;
+
+        ChannelsToStereo c2s;
+
+        float _sampleRate;
+    };
 }

core/src/dsp/filter.h

@@ -98,6 +98,91 @@ namespace dsp {
 
     };
 
+    class ComplexFIR : public generic_block<ComplexFIR> {
+    public:
+        ComplexFIR() {}
+
+        ComplexFIR(stream<complex_t>* in, dsp::filter_window::generic_complex_window* window) { init(in, window); }
+
+        ~ComplexFIR() {
+            if (!generic_block<ComplexFIR>::_block_init) { return; }
+            generic_block<ComplexFIR>::stop();
+            volk_free(buffer);
+            volk_free(taps);
+            generic_block<ComplexFIR>::_block_init = false;
+        }
+
+        void init(stream<complex_t>* in, dsp::filter_window::generic_complex_window* window) {
+            _in = in;
+
+            tapCount = window->getTapCount();
+            taps = (complex_t*)volk_malloc(tapCount * sizeof(complex_t), volk_get_alignment());
+            window->createTaps(taps, tapCount);
+
+            buffer = (complex_t*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(complex_t) * 2, volk_get_alignment());
+            bufStart = &buffer[tapCount];
+            generic_block<ComplexFIR>::registerInput(_in);
+            generic_block<ComplexFIR>::registerOutput(&out);
+            generic_block<ComplexFIR>::_block_init = true;
+        }
+
+        void setInput(stream<complex_t>* in) {
+            assert(generic_block<ComplexFIR>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<ComplexFIR>::ctrlMtx);
+            generic_block<ComplexFIR>::tempStop();
+            generic_block<ComplexFIR>::unregisterInput(_in);
+            _in = in;
+            generic_block<ComplexFIR>::registerInput(_in);
+            generic_block<ComplexFIR>::tempStart();
+        }
+
+        void updateWindow(dsp::filter_window::generic_complex_window* window) {
+            assert(generic_block<ComplexFIR>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<ComplexFIR>::ctrlMtx);
+            _window = window;
+            volk_free(taps);
+            tapCount = window->getTapCount();
+            taps = (complex_t*)volk_malloc(tapCount * sizeof(complex_t), volk_get_alignment());
+            bufStart = &buffer[tapCount];
+            window->createTaps(taps, tapCount);
+        }
+
+        int run() {
+            int count = _in->read();
+            if (count < 0) { return -1; }
+
+            generic_block<ComplexFIR>::ctrlMtx.lock();
+
+            memcpy(bufStart, _in->readBuf, count * sizeof(complex_t));
+            _in->flush();
+
+            for (int i = 0; i < count; i++) {
+                volk_32fc_x2_dot_prod_32fc((lv_32fc_t*)&out.writeBuf[i], (lv_32fc_t*)&buffer[i+1], (lv_32fc_t*)taps, tapCount);
+            }
+
+            if (!out.swap(count)) { return -1; }
+
+            memmove(buffer, &buffer[count], tapCount * sizeof(complex_t));
+
+            generic_block<ComplexFIR>::ctrlMtx.unlock();
+
+            return count;
+        }
+
+        stream<complex_t> out;
+
+    private:
+        stream<complex_t>* _in;
+
+        dsp::filter_window::generic_complex_window* _window;
+
+        complex_t* bufStart;
+        complex_t* buffer;
+        int tapCount;
+        complex_t* taps;
+
+    };
+
     class BFMDeemp : public generic_block<BFMDeemp> {
     public:
         BFMDeemp() {}

core/src/dsp/math.h

@@ -175,10 +175,10 @@ namespace dsp {
         void init(stream<T>* a, stream<T>* b) {
             _a = a;
             _b = b;
-            generic_block<Multiply>::registerInput(a);
+            generic_block<Multiply<T>>::registerInput(a);
-            generic_block<Multiply>::registerInput(b);
+            generic_block<Multiply<T>>::registerInput(b);
-            generic_block<Multiply>::registerOutput(&out);
+            generic_block<Multiply<T>>::registerOutput(&out);
-            generic_block<Multiply>::_block_init = true;
+            generic_block<Multiply<T>>::_block_init = true;
         }
 
         void setInputs(stream<T>* a, stream<T>* b) {

core/src/dsp/pll.h

@@ -323,7 +323,7 @@ namespace dsp {
 
         float _alpha; // Integral coefficient
         float _beta; // Proportional coefficient
-        float vcoFrequency = 0.0f;
+        float vcoFrequency = ((19000.0f / 250000.0f) * 2.0f * FL_M_PI);
         float vcoPhase = 0.0f;
         complex_t lastVCO;
 

core/src/dsp/processing.h

@@ -559,4 +559,57 @@ namespace dsp {
         stream<float>* _in;
 
     };
+
+    class BFMPilotToStereo : public generic_block<BFMPilotToStereo> {
+    public:
+        BFMPilotToStereo() {}
+
+        BFMPilotToStereo(stream<complex_t>* in) { init(in); }
+
+        ~BFMPilotToStereo() {
+            generic_block<BFMPilotToStereo>::stop();
+            delete[] buffer;
+        }
+
+        void init(stream<complex_t>* in) {
+            _in = in;
+
+            buffer = new complex_t[STREAM_BUFFER_SIZE];
+
+            generic_block<BFMPilotToStereo>::registerInput(_in);
+            generic_block<BFMPilotToStereo>::registerOutput(&out);
+            generic_block<BFMPilotToStereo>::_block_init = true;
+        }
+
+        void setInputs(stream<complex_t>* in) {
+            assert(generic_block<BFMPilotToStereo>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<BFMPilotToStereo>::ctrlMtx);
+            generic_block<BFMPilotToStereo>::tempStop();
+            generic_block<BFMPilotToStereo>::unregisterInput(_in);
+            _in = in;
+            generic_block<BFMPilotToStereo>::registerInput(_in);
+            generic_block<BFMPilotToStereo>::tempStart();
+        }
+
+        int run() {
+            int count = _in->read();
+            if (count < 0) { return -1; }
+
+            volk_32fc_x2_multiply_32fc((lv_32fc_t*)buffer, (lv_32fc_t*)_in->readBuf, (lv_32fc_t*)_in->readBuf, count);
+            _in->flush();
+
+            volk_32fc_conjugate_32fc((lv_32fc_t*)out.writeBuf, (lv_32fc_t*)buffer, count);
+
+            if (!out.swap(count)) { return -1; }
+            return count;
+        }
+
+        stream<complex_t> out;
+
+    private:
+        stream<complex_t>* _in;
+
+        complex_t* buffer;
+
+    };
 }

core/src/dsp/window.h

@@ -11,6 +11,12 @@ namespace dsp {
             virtual void createTaps(float* taps, int tapCount, float factor = 1.0f) {}
         };
 
+        class generic_complex_window {
+        public:
+            virtual int getTapCount() { return -1; }
+            virtual void createTaps(dsp::complex_t* taps, int tapCount, float factor = 1.0f) {}
+        };
+
         class BlackmanWindow : public filter_window::generic_window {
         public:
             BlackmanWindow() {}
@@ -76,6 +82,108 @@ namespace dsp {
             float _cutoff, _transWidth, _sampleRate;
 
         };
+
+        class BandPassBlackmanWindow : public filter_window::generic_complex_window {
+        public:
+            BandPassBlackmanWindow() {}
+            BandPassBlackmanWindow(float lowCutoff, float highCutoff, float transWidth, float sampleRate) { init(lowCutoff, highCutoff, transWidth, sampleRate); }
+
+            void init(float lowCutoff, float highCutoff, float transWidth, float sampleRate) {
+                assert(lowCutoff <= highCutoff);
+                _lowCutoff = lowCutoff;
+                _highCutoff = highCutoff;
+                _transWidth = transWidth;
+                _sampleRate = sampleRate;
+
+                // Calculate other values
+                _offset = (_lowCutoff + _highCutoff) / 2.0f;
+                _cutoff = fabs((_highCutoff - _lowCutoff) / 2.0f);
+            }
+
+            void setSampleRate(float sampleRate) {
+                _sampleRate = sampleRate;
+            }
+
+            void setCutoffs(float lowCutoff, float highCutoff) {
+                assert(lowCutoff <= highCutoff);
+                _lowCutoff = lowCutoff;
+                _highCutoff = highCutoff;
+
+                // Calculate other values
+                _offset = (_lowCutoff + _highCutoff) / 2.0f;
+                _cutoff = fabs((_highCutoff - _lowCutoff) / 2.0f);
+            }
+
+            void setLowCutoff(float lowCutoff) {
+                assert(lowCutoff <= _highCutoff);
+                _lowCutoff = lowCutoff;
+
+                // Calculate other values
+                _offset = (_lowCutoff + _highCutoff) / 2.0f;
+                _cutoff = fabs((_highCutoff - _lowCutoff) / 2.0f);
+            }
+
+            void setHighCutoff(float highCutoff) {
+                assert(_lowCutoff <= highCutoff);
+                _highCutoff = highCutoff;
+
+                // Calculate other values
+                _offset = (_lowCutoff + _highCutoff) / 2.0f;
+                _cutoff = fabs((_highCutoff - _lowCutoff) / 2.0f);
+            }
+            
+            void setTransWidth(float transWidth) {
+                _transWidth = transWidth;
+            }
+
+            int getTapCount() {
+                float fc = _cutoff / _sampleRate;
+                if (fc > 1.0f) {
+                    fc = 1.0f;
+                }
+
+                int _M = 4.0f / (_transWidth / _sampleRate);
+                if (_M < 4) {
+                    _M = 4;
+                }
+
+                if (_M % 2 == 0) { _M++; }
+
+                return _M;
+            }
+
+            void createTaps(dsp::complex_t* taps, int tapCount, float factor = 1.0f) {
+                // Calculate cuttoff frequency
+                float omega = 2.0f * FL_M_PI * (_cutoff / _sampleRate);
+                if (omega > FL_M_PI) { omega = FL_M_PI; }
+
+                // Generate taps
+                float val;
+                float sum = 0.0f;
+                float tc = tapCount;
+                for (int i = 0; i < tapCount; i++) {
+                    val = math::sinc(omega, (float)i - (tc/2), FL_M_PI) * window_function::blackman(i, tc - 1);
+                    taps[i].re = val;
+                    taps[i].im = 0;
+                    sum += val;
+                }
+
+                // Normalize taps and multiply by supplied factor
+                for (int i = 0; i < tapCount; i++) {
+                    taps[i] = taps[i] * factor;
+                    taps[i] = taps[i] / sum;
+                }
+
+                // Add offset
+                lv_32fc_t phase = lv_cmake(1.0f, 0.0f);
+                lv_32fc_t phaseDelta = lv_cmake(std::cos((-_offset / _sampleRate) * 2.0f * FL_M_PI), std::sin((-_offset / _sampleRate) * 2.0f * FL_M_PI));
+                volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)taps, (lv_32fc_t*)taps, phaseDelta, &phase, tapCount);
+            }
+
+        private:
+            float _lowCutoff, _highCutoff;
+            float _cutoff, _transWidth, _sampleRate, _offset;
+        };
     }
 
     class RRCTaps : public filter_window::generic_window {

file_source/src/main.cpp

@@ -69,16 +69,16 @@ private:
         tuner::tune(tuner::TUNER_MODE_IQ_ONLY, "", _this->centerFreq);
         sigpath::signalPath.setBuffering(false);
         gui::waterfall.centerFrequencyLocked = true;
-        gui::freqSelect.minFreq = _this->centerFreq - (_this->sampleRate/2);
+        //gui::freqSelect.minFreq = _this->centerFreq - (_this->sampleRate/2);
-        gui::freqSelect.maxFreq = _this->centerFreq + (_this->sampleRate/2);
+        //gui::freqSelect.maxFreq = _this->centerFreq + (_this->sampleRate/2);
-        gui::freqSelect.limitFreq = true;
+        //gui::freqSelect.limitFreq = true;
         spdlog::info("FileSourceModule '{0}': Menu Select!", _this->name);
     }
 
     static void menuDeselected(void* ctx) {
         FileSourceModule* _this = (FileSourceModule*)ctx;
         sigpath::signalPath.setBuffering(true);
-        gui::waterfall.centerFrequencyLocked = false;
+        //gui::waterfall.centerFrequencyLocked = false;
         spdlog::info("FileSourceModule '{0}': Menu Deselect!", _this->name);
     }
     
@@ -124,9 +124,9 @@ private:
                     std::string filename = std::filesystem::path(_this->fileSelect.path).filename().string();
                     _this->centerFreq = _this->getFrequency(filename);
                     tuner::tune(tuner::TUNER_MODE_IQ_ONLY, "", _this->centerFreq);
-                    gui::freqSelect.minFreq = _this->centerFreq - (_this->sampleRate/2);
+                    //gui::freqSelect.minFreq = _this->centerFreq - (_this->sampleRate/2);
-                    gui::freqSelect.maxFreq = _this->centerFreq + (_this->sampleRate/2);
+                    //gui::freqSelect.maxFreq = _this->centerFreq + (_this->sampleRate/2);
-                    gui::freqSelect.limitFreq = true;
+                    //gui::freqSelect.limitFreq = true;
                 }
                 catch (std::exception e) {
                     spdlog::error("Error: {0}", e.what());
@@ -186,10 +186,10 @@ private:
     WavReader* reader = NULL;
     bool running = false;
     bool enabled = true;
-    float sampleRate = 48000;
+    float sampleRate = 1000000;
     std::thread workerThread;
 
-    double centerFreq = 0;
+    double centerFreq = 100000000;
 
     bool float32Mode = false;
 };