FM stereo prototype

2021-07-23 06:29:16 +02:00 · 2021-07-23 06:29:16 +02:00 · 8454b40d54
commit 8454b40d54
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -155,6 +155,7 @@ if (OPT_BUILD_RIGCTL_SERVER)
 add_subdirectory("rigctl_server")
 endif (OPT_BUILD_RIGCTL_SERVER)

+
 add_executable(sdrpp "src/main.cpp" "win32/resources.rc")
 target_link_libraries(sdrpp PRIVATE sdrpp_core)

--- a/core/src/core.cpp
+++ b/core/src/core.cpp
@ -20,6 +20,7 @@
 #include <duktape/duk_console.h>
 #include <filesystem>
 #include <gui/menus/theme.h>
+#include <server.h>

 #define STB_IMAGE_RESIZE_IMPLEMENTATION
 #include <stb_image_resize.h>
@ -258,6 +259,8 @@ int sdrpp_main(int argc, char *argv[]) {

    core::configManager.release(true);

+    if (options::opts.serverMode) { return server_main(); }
+
    // Setup window
    glfwSetErrorCallback(glfw_error_callback);
    if (!glfwInit()) {
--- a/core/src/dsp/demodulator.h
+++ b/core/src/dsp/demodulator.h
@ -10,6 +10,7 @@
 #include <dsp/math.h>
 #include <dsp/convertion.h>
 #include <dsp/audio.h>
+#include <dsp/stereo_fm.h>

 #define FAST_ATAN2_COEF1    FL_M_PI / 4.0f
 #define FAST_ATAN2_COEF2    3.0f * FAST_ATAN2_COEF1
@ -670,67 +671,32 @@ namespace dsp {
    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);
+        StereoFMDemod(stream<complex_t>* input, float sampleRate, float deviation) {
+            init(input, sampleRate, deviation);
        }

-        void init(stream<float>* input, float sampleRate) {
+        void init(stream<complex_t>* input, float sampleRate, float deviation) {
            _sampleRate = sampleRate;

-            r2c.init(input);
-            split.init(&r2c.out);
+            PilotFirWin.init(18750, 19250, 3000, _sampleRate);

-            split.bindStream(&APlusBIn);
-            split.bindStream(&AMinusBIn);
-            split.bindStream(&PilotIn);
+            demod.init(input, _sampleRate, deviation);

-            APlusBWin.init(0, 17000, 2500, _sampleRate);
-            AMinusBWin.init(38000, 38000 + 17000, 2500, _sampleRate);
-            PilotWin.init(18500, 19500, 1500, _sampleRate);
+            r2c.init(&demod.out);

-            APlusBFir.init(&APlusBIn, &APlusBWin);
-            AMinusBFir.init(&AMinusBIn, &AMinusBWin);
-            PilotFir.init(&PilotIn, &PilotWin);
+            pilotFilter.init(&r2c.out, &PilotFirWin);

-            pll.init(&PilotFir.out, 0.1f);
+            demux.init(&pilotFilter.dataOut, &pilotFilter.pilotOut, 1.0f);

-            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;
+            recon.init(&demux.AplusBOut, &demux.AminusBOut);

+            out = &recon.out;
+            
+            generic_hier_block<StereoFMDemod>::registerBlock(&demod);
            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>::registerBlock(&pilotFilter);
+            generic_hier_block<StereoFMDemod>::registerBlock(&demux);
+            generic_hier_block<StereoFMDemod>::registerBlock(&recon);
            generic_hier_block<StereoFMDemod>::_block_init = true;
        }

@ -739,44 +705,24 @@ namespace dsp {
            r2c.setInput(input);
        }

+        void setDeviation(float deviation) {
+            demod.setDeviation(deviation);
+        }
+
        stream<stereo_t>* out = NULL;

    private:
-        filter_window::BandPassBlackmanWindow APlusBWin;
-        filter_window::BandPassBlackmanWindow AMinusBWin;
-        filter_window::BandPassBlackmanWindow PilotWin;
+        filter_window::BandPassBlackmanWindow PilotFirWin;
+
+        FloatFMDemod demod;

        RealToComplex r2c;
-        Splitter<complex_t> split;

-        stream<complex_t> APlusBIn;
-        stream<complex_t> AMinusBIn;
-        stream<complex_t> PilotIn;
-        ComplexFIR APlusBFir;
-        ComplexFIR AMinusBFir;
-        ComplexFIR PilotFir;
+        FMStereoDemuxPilotFilter pilotFilter;

-        PLL pll;
+        FMStereoDemux demux;

-        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;
+        FMStereoReconstruct recon;

        float _sampleRate;
    };
--- a/core/src/dsp/pll.h
+++ b/core/src/dsp/pll.h
@ -279,11 +279,6 @@ namespace dsp {
            float error;

            for (int i = 0; i < count; i++) {
-
-                // Mix the VFO with the input to create the output value
-                outVal.re = (lastVCO.re*_in->readBuf[i].re) - ((-lastVCO.im)*_in->readBuf[i].im);
-                outVal.im = ((-lastVCO.im)*_in->readBuf[i].re) + (lastVCO.re*_in->readBuf[i].im);
-
                out.writeBuf[i] = lastVCO;

                // Calculate the phase error estimation
@ -292,9 +287,6 @@ namespace dsp {
                if (error > 3.1415926535f)        { error -= 2.0f * 3.1415926535f; }
                else if (error <= -3.1415926535f) { error += 2.0f * 3.1415926535f; }
                
-                // if (error > 1.0f) { error = 1.0f; }
-                // else if (error < -1.0f) { error = -1.0f; }
-                
                // Integrate frequency and clamp it
                vcoFrequency += _beta * error;
                if (vcoFrequency > 1.0f) { vcoFrequency = 1.0f; }
--- a/core/src/dsp/stereo_fm.h
+++ b/core/src/dsp/stereo_fm.h
@ -0,0 +1,281 @@
+#pragma once
+#include <dsp/block.h>
+#include <dsp/stream.h>
+#include <dsp/types.h>
+
+namespace dsp {
+
+    class FMStereoDemuxPilotFilter : public generic_block<FMStereoDemuxPilotFilter> {
+    public:
+        FMStereoDemuxPilotFilter() {}
+
+        FMStereoDemuxPilotFilter(stream<complex_t>* in, dsp::filter_window::generic_complex_window* window) { init(in, window); }
+
+        ~FMStereoDemuxPilotFilter() {
+            if (!generic_block<FMStereoDemuxPilotFilter>::_block_init) { return; }
+            generic_block<FMStereoDemuxPilotFilter>::stop();
+            volk_free(buffer);
+            volk_free(taps);
+            generic_block<FMStereoDemuxPilotFilter>::_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<FMStereoDemuxPilotFilter>::registerInput(_in);
+            generic_block<FMStereoDemuxPilotFilter>::registerOutput(&dataOut);
+            generic_block<FMStereoDemuxPilotFilter>::registerOutput(&pilotOut);
+            generic_block<FMStereoDemuxPilotFilter>::_block_init = true;
+        }
+
+        void setInput(stream<complex_t>* in) {
+            assert(generic_block<FMStereoDemuxPilotFilter>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<FMStereoDemuxPilotFilter>::ctrlMtx);
+            generic_block<FMStereoDemuxPilotFilter>::tempStop();
+            generic_block<FMStereoDemuxPilotFilter>::unregisterInput(_in);
+            _in = in;
+            generic_block<FMStereoDemuxPilotFilter>::registerInput(_in);
+            generic_block<FMStereoDemuxPilotFilter>::tempStart();
+        }
+
+        void updateWindow(dsp::filter_window::generic_complex_window* window) {
+            assert(generic_block<FMStereoDemuxPilotFilter>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<FMStereoDemuxPilotFilter>::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<FMStereoDemuxPilotFilter>::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*)&pilotOut.writeBuf[i], (lv_32fc_t*)&buffer[i+1], (lv_32fc_t*)taps, tapCount);
+            }
+
+            memcpy(dataOut.writeBuf, &buffer[tapCount - ((tapCount-1)/2)], count * sizeof(complex_t));
+
+            if (!dataOut.swap(count)) { return -1; }
+            if (!pilotOut.swap(count)) { return -1; }
+
+            memmove(buffer, &buffer[count], tapCount * sizeof(complex_t));
+
+            generic_block<FMStereoDemuxPilotFilter>::ctrlMtx.unlock();
+
+            return count;
+        }
+
+        stream<complex_t> dataOut;
+        stream<complex_t> pilotOut;
+        
+
+    private:
+        stream<complex_t>* _in;
+
+        dsp::filter_window::generic_complex_window* _window;
+
+        complex_t* bufStart;
+        complex_t* buffer;
+        int tapCount;
+        complex_t* taps;
+
+    };
+
+    class FMStereoDemux: public generic_block<FMStereoDemux> {
+    public:
+        FMStereoDemux() {}
+
+        FMStereoDemux(stream<complex_t>* data, stream<complex_t>* pilot, float loopBandwidth) { init(data, pilot, loopBandwidth); }
+
+        void init(stream<complex_t>* data, stream<complex_t>* pilot, float loopBandwidth) {
+            _data = data;
+            _pilot = pilot;
+            lastVCO.re = 1.0f;
+            lastVCO.im = 0.0f;
+            _loopBandwidth = loopBandwidth;
+
+            float dampningFactor = sqrtf(2.0f) / 2.0f;
+            float denominator = (1.0 + 2.0 * dampningFactor * _loopBandwidth + _loopBandwidth * _loopBandwidth);
+            _alpha = (4 * dampningFactor * _loopBandwidth) / denominator;
+            _beta = (4 * _loopBandwidth * _loopBandwidth) / denominator;
+
+            generic_block<FMStereoDemux>::registerInput(_data);
+            generic_block<FMStereoDemux>::registerInput(_pilot);
+            generic_block<FMStereoDemux>::registerOutput(&AplusBOut);
+            generic_block<FMStereoDemux>::registerOutput(&AminusBOut);
+            generic_block<FMStereoDemux>::_block_init = true;
+        }
+
+        void setInput(stream<complex_t>* data, stream<complex_t>* pilot) {
+            assert(generic_block<FMStereoDemux>::_block_init);
+            generic_block<FMStereoDemux>::tempStop();
+            generic_block<FMStereoDemux>::unregisterInput(_data);
+            generic_block<FMStereoDemux>::unregisterInput(_pilot);
+            _data = data;
+            _pilot = pilot;
+            generic_block<FMStereoDemux>::registerInput(_data);
+            generic_block<FMStereoDemux>::registerInput(_pilot);
+            generic_block<FMStereoDemux>::tempStart();
+        }
+
+        void setLoopBandwidth(float loopBandwidth) {
+            assert(generic_block<FMStereoDemux>::_block_init);
+            generic_block<FMStereoDemux>::tempStop();
+            _loopBandwidth = loopBandwidth;
+            float dampningFactor = sqrtf(2.0f) / 2.0f;
+            float denominator = (1.0 + 2.0 * dampningFactor * _loopBandwidth + _loopBandwidth * _loopBandwidth);
+            _alpha = (4 * dampningFactor * _loopBandwidth) / denominator;
+            _beta = (4 * _loopBandwidth * _loopBandwidth) / denominator;
+            generic_block<FMStereoDemux>::tempStart();
+        }
+
+        int run() {
+            int count = _data->read();
+            if (count < 0) { return -1; }
+            int pCount = _pilot->read();
+            if (pCount < 0) { return -1; }
+
+            complex_t doubledVCO;
+            float error;
+
+            volk_32fc_deinterleave_real_32f(AplusBOut.writeBuf, (lv_32fc_t*)_data->readBuf, count);
+
+            for (int i = 0; i < count; i++) {
+                // Double the VCO, then mix it with the input data.
+                doubledVCO = lastVCO*lastVCO;
+                AminusBOut.writeBuf[i] = (_data->readBuf[i].re * doubledVCO.re) + (_data->readBuf[i].im * doubledVCO.im);
+
+                // Calculate the phase error estimation
+                error = _pilot->readBuf[i].phase() - vcoPhase;
+                if (error > 3.1415926535f)        { error -= 2.0f * 3.1415926535f; }
+                else if (error <= -3.1415926535f) { error += 2.0f * 3.1415926535f; }
+                
+                // Integrate frequency and clamp it
+                vcoFrequency += _beta * error;
+                if (vcoFrequency > upperLimit) { vcoFrequency = upperLimit; }
+                else if (vcoFrequency < lowerLimit) { vcoFrequency = lowerLimit; }
+
+                // Calculate new phase and wrap it
+                vcoPhase += vcoFrequency + (_alpha * error);
+                while (vcoPhase > (2.0f * FL_M_PI)) { vcoPhase -= (2.0f * FL_M_PI); }
+                while (vcoPhase < (-2.0f * FL_M_PI)) { vcoPhase += (2.0f * FL_M_PI); }
+
+                // Calculate output
+                lastVCO.re = cosf(vcoPhase);
+                lastVCO.im = sinf(vcoPhase);
+            }
+            
+            _data->flush();
+            _pilot->flush();
+
+            if (!AplusBOut.swap(count)) { return -1; }
+            if (!AminusBOut.swap(count)) { return -1; }
+            return count;
+        }
+
+        stream<float> AplusBOut;
+        stream<float> AminusBOut;
+
+    private:
+        float _loopBandwidth = 0.01f;
+
+        const float expectedFreq = ((19000.0f / 250000.0f) * 2.0f * FL_M_PI);
+        const float upperLimit = ((19200.0f / 250000.0f) * 2.0f * FL_M_PI);
+        const float lowerLimit = ((18800.0f / 250000.0f) * 2.0f * FL_M_PI);
+
+        float _alpha; // Integral coefficient
+        float _beta; // Proportional coefficient
+        float vcoFrequency = expectedFreq;
+        float vcoPhase = 0.0f;
+        complex_t lastVCO;
+
+        stream<complex_t>* _data;
+        stream<complex_t>* _pilot;
+    };
+
+    class FMStereoReconstruct : public generic_block<FMStereoReconstruct> {
+    public:
+        FMStereoReconstruct() {}
+
+        FMStereoReconstruct(stream<float>* a, stream<float>* b) { init(a, b); }
+
+        ~FMStereoReconstruct() {
+            generic_block<FMStereoReconstruct>::stop();
+            delete[] leftBuf;
+            delete[] rightBuf;
+        }
+
+        void init(stream<float>* aplusb, stream<float>* aminusb) {
+            _aplusb = aplusb;
+            _aminusb = aminusb;
+
+            leftBuf = new float[STREAM_BUFFER_SIZE];
+            rightBuf = new float[STREAM_BUFFER_SIZE];
+
+            generic_block<FMStereoReconstruct>::registerInput(aplusb);
+            generic_block<FMStereoReconstruct>::registerInput(aminusb);
+            generic_block<FMStereoReconstruct>::registerOutput(&out);
+            generic_block<FMStereoReconstruct>::_block_init = true;
+        }
+
+        void setInputs(stream<float>* aplusb, stream<float>* aminusb) {
+            assert(generic_block<FMStereoReconstruct>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<FMStereoReconstruct>::ctrlMtx);
+            generic_block<FMStereoReconstruct>::tempStop();
+            generic_block<FMStereoReconstruct>::unregisterInput(_aplusb);
+            generic_block<FMStereoReconstruct>::unregisterInput(_aminusb);
+            _aplusb = aplusb;
+            _aminusb = aminusb;
+            generic_block<FMStereoReconstruct>::registerInput(_aplusb);
+            generic_block<FMStereoReconstruct>::registerInput(_aminusb);
+            generic_block<FMStereoReconstruct>::tempStart();
+        }
+
+        int run() {
+            int a_count = _aplusb->read();
+            if (a_count < 0) { return -1; }
+            int b_count = _aminusb->read();
+            if (b_count < 0) { return -1; }
+            if (a_count != b_count) {
+                _aplusb->flush();
+                _aminusb->flush();
+                return 0;
+            }
+
+            volk_32f_x2_add_32f(leftBuf, _aplusb->readBuf, _aminusb->readBuf, a_count);
+            volk_32f_x2_subtract_32f(rightBuf, _aplusb->readBuf, _aminusb->readBuf, a_count);
+            _aplusb->flush();
+            _aminusb->flush();
+
+            volk_32f_x2_interleave_32fc((lv_32fc_t*)out.writeBuf, leftBuf, rightBuf, a_count);
+
+            if (!out.swap(a_count)) { return -1; }
+            return a_count;
+        }
+
+        stream<stereo_t> out;
+
+    private:
+        stream<float>* _aplusb;
+        stream<float>* _aminusb;
+
+        float* leftBuf;
+        float* rightBuf;
+
+    };
+}
--- a/core/src/options.cpp
+++ b/core/src/options.cpp
@ -25,6 +25,9 @@ namespace options {
            else if (!strcmp(arg, "-s") || !strcmp(arg, "--show-console")) {
                opts.showConsole = true;
            }
+            else if (!strcmp(arg, "--server")) {
+                opts.serverMode = true;
+            }
            else {
                spdlog::error("Invalid command line option: {0}", arg);
                return false;
--- a/core/src/options.h
+++ b/core/src/options.h
@ -6,6 +6,7 @@ namespace options {
    struct CMDLineOptions {
        std::string root;
        bool showConsole;
+        bool serverMode;
    };

    SDRPP_EXPORT CMDLineOptions opts;
--- a/core/src/server.cpp
+++ b/core/src/server.cpp
@ -0,0 +1,7 @@
+#include <server.h>
+#include <spdlog/spdlog.h>
+
+int server_main() {
+    spdlog::error("Server mode is not implemented yet.");
+    return 0;
+}
--- a/core/src/server.h
+++ b/core/src/server.h
@ -0,0 +1,3 @@
+#pragma once
+
+int server_main();
--- a/radio/src/wfm_demod.h
+++ b/radio/src/wfm_demod.h
@ -48,6 +48,7 @@ public:
        squelch.init(_vfo->output, squelchLevel);
        
        demod.init(&squelch.out, bbSampRate, bw / 2.0f);
+        demodStereo.init(&squelch.out, bbSampRate, bw / 2.0f);

        float audioBW = std::min<float>(audioSampleRate / 2.0f, 16000.0f);
        win.init(audioBW, audioBW, bbSampRate);
@ -63,7 +64,12 @@ public:

    void start() {
        squelch.start();
-        demod.start();
+        if (stereo) {
+            demodStereo.start();
+        }
+        else {
+            demod.start();
+        }
        resamp.start();
        deemp.start();
        running = true;
@ -71,7 +77,12 @@ public:

    void stop() {
        squelch.stop();
-        demod.stop();
+        if (stereo) {
+            demodStereo.stop();
+        }
+        else {
+            demod.stop();
+        }
        resamp.stop();
        deemp.stop();
        running = false;
@ -176,6 +187,10 @@ public:
            _config->conf[uiPrefix]["WFM"]["squelchLevel"] = squelchLevel;
            _config->release(true);
        }
+
+        if (ImGui::Checkbox("Stereo##radio_wfm_demod", &stereo)) {
+            setStereo(stereo);
+        }
    }

    void setDeempIndex(int id) {
@ -197,6 +212,23 @@ public:
        bw = bandWidth;
        _vfo->setBandwidth(bw, updateWaterfall);
        demod.setDeviation(bw / 2.0f);
+        demodStereo.setDeviation(bw / 2.0f);
+    }
+
+    void setStereo(bool enabled) {
+        if (running) {
+            demod.stop();
+            demodStereo.stop();
+        }
+
+        if (enabled) {
+            resamp.setInput(demodStereo.out);
+            if (running) { demodStereo.start(); }
+        }
+        else {
+            resamp.setInput(&demod.out);
+            if (running) { demod.start(); }
+        }
    }

 private:
@ -212,13 +244,17 @@ private:
    float audioSampRate = 48000;
    float squelchLevel = -100.0f;
    float bw = 200000;
+    bool stereo = false;
    int deempId = 0;
    float tau = 50e-6;
    bool running = false;

    VFOManager::VFO* _vfo;
    dsp::Squelch squelch;
+
    dsp::FMDemod demod;
+    dsp::StereoFMDemod demodStereo;
+
    dsp::filter_window::BlackmanWindow win;
    dsp::PolyphaseResampler<dsp::stereo_t> resamp;
    dsp::BFMDeemp deemp;