Fixed some stuff

audio_sink/src/main.cpp

@@ -5,6 +5,7 @@
 #include <signal_path/sink.h>
 #include <portaudio.h>
 #include <dsp/audio.h>
+#include <dsp/processing.h>
 #include <spdlog/spdlog.h>
 
 #define CONCAT(a, b) ((std::string(a) + b).c_str())
@@ -35,6 +36,9 @@ public:
         monoRB.init(&s2m.out);
         stereoRB.init(_stream->sinkOut);
 
+        // monoPacker.init(&s2m.out, 240);
+        // stereoPacker.init(_stream->sinkOut, 240);
+
         // Initialize PortAudio
         devCount = Pa_GetDeviceCount();
         devId = Pa_GetDefaultOutputDevice();
@@ -156,12 +160,18 @@ private:
         if (dev->channels == 2) {
             stereoRB.data.setMaxLatency(bufferSize * 2);
             stereoRB.start();
+            // stereoPacker.setSampleCount(bufferSize);
+            // stereoPacker.start();
             err = Pa_OpenStream(&stream, NULL, &outputParams, sampleRate, paFramesPerBufferUnspecified, 0, _stereo_cb, this);
+            //err = Pa_OpenStream(&stream, NULL, &outputParams, sampleRate, bufferSize, 0, _stereo_cb, this);
         }
         else {
             monoRB.data.setMaxLatency(bufferSize * 2);
             monoRB.start();
+            // stereoPacker.setSampleCount(bufferSize);
+            // monoPacker.start();
             err = Pa_OpenStream(&stream, NULL, &outputParams, sampleRate, paFramesPerBufferUnspecified, 0, _mono_cb, this);
+            //err = Pa_OpenStream(&stream, NULL, &outputParams, sampleRate, bufferSize, 0, _mono_cb, this);
         }
 
         if (err != 0) {
@@ -182,12 +192,18 @@ private:
         s2m.stop();
         monoRB.stop();
         stereoRB.stop();
+        // monoPacker.stop();
+        // stereoPacker.stop();
         monoRB.data.stopReader();
         stereoRB.data.stopReader();
+        // monoPacker.out.stopReader();
+        // stereoPacker.out.stopReader();
         Pa_StopStream(stream);
         Pa_CloseStream(stream);
         monoRB.data.clearReadStop();
         stereoRB.data.clearReadStop();
+        // monoPacker.out.clearReadStop();
+        // stereoPacker.out.clearWriteStop();
     }
 
     static int _mono_cb(const void *input, void *output, unsigned long frameCount,
@@ -204,10 +220,33 @@ private:
         return 0;
     }
 
+    // static int _mono_cb(const void *input, void *output, unsigned long frameCount,
+    //     const PaStreamCallbackTimeInfo* timeInfo, PaStreamCallbackFlags statusFlags, void *userData) {
+    //     AudioSink* _this = (AudioSink*)userData;
+    //     if (_this->monoPacker.out.read() < 0) { return 0; }
+    //     memcpy((float*)output, _this->monoPacker.out.readBuf, frameCount * sizeof(float));
+    //     _this->monoPacker.out.flush();
+    //     return 0;
+    // }
+
+    // static int _stereo_cb(const void *input, void *output, unsigned long frameCount,
+    //     const PaStreamCallbackTimeInfo* timeInfo, PaStreamCallbackFlags statusFlags, void *userData) {
+    //     AudioSink* _this = (AudioSink*)userData;
+    //     if (_this->stereoPacker.out.read() < 0) { spdlog::warn("CB killed"); return 0; }
+    //     memcpy((dsp::stereo_t*)output, _this->stereoPacker.out.readBuf, frameCount * sizeof(dsp::stereo_t));
+    //     _this->stereoPacker.out.flush();
+    //     return 0;
+    // }
+    
+    
     SinkManager::Stream* _stream;
     dsp::StereoToMono s2m;
     dsp::RingBufferSink<float> monoRB;
     dsp::RingBufferSink<dsp::stereo_t> stereoRB;
+
+    // dsp::Packer<float> monoPacker;
+    // dsp::Packer<dsp::stereo_t> stereoPacker;
+
     std::string _streamName;
     PaStream *stream;
 

core/src/dsp/audio.h

@@ -47,6 +47,65 @@ namespace dsp {
 
     };
 
+    class ChannelsToStereo : public generic_block<ChannelsToStereo> {
+    public:
+        ChannelsToStereo() {}
+
+        ChannelsToStereo(stream<float>* in_left, stream<float>* in_right) { init(in_left, in_right); }
+
+        ~ChannelsToStereo() { generic_block<ChannelsToStereo>::stop(); }
+
+        void init(stream<float>* in_left, stream<float>* in_right) {
+            _in_left = in_left;
+            _in_right = in_right;
+            generic_block<ChannelsToStereo>::registerInput(_in_left);
+            generic_block<ChannelsToStereo>::registerInput(_in_right);
+            generic_block<ChannelsToStereo>::registerOutput(&out);
+        }
+
+        void setInput(stream<float>* in_left, stream<float>* in_right) {
+            std::lock_guard<std::mutex> lck(generic_block<ChannelsToStereo>::ctrlMtx);
+            generic_block<ChannelsToStereo>::tempStop();
+            generic_block<ChannelsToStereo>::unregisterInput(_in_left);
+            generic_block<ChannelsToStereo>::unregisterInput(_in_right);
+            _in_left = in_left;
+            _in_right = in_right;
+            generic_block<ChannelsToStereo>::registerInput(_in_left);
+            generic_block<ChannelsToStereo>::registerInput(_in_right);
+            generic_block<ChannelsToStereo>::tempStart();
+        }
+
+        int run() {
+            count_l = _in_left->read();
+            if (count_l < 0) { return -1; }
+            count_r = _in_right->read();
+            if (count_r < 0) { return -1; }
+
+            if (count_l != count_r) {
+                spdlog::warn("ChannelsToStereo block size missmatch");
+            }
+
+            for (int i = 0; i < count_l; i++) {
+                out.writeBuf[i].l = _in_left->readBuf[i];
+                out.writeBuf[i].r = _in_right->readBuf[i];
+            }
+
+            _in_left->flush();
+            _in_right->flush();
+            if (!out.swap(count_l)) { return -1; }
+            return count_l;
+        }
+
+        stream<stereo_t> out;
+
+    private:
+        int count_l;
+        int count_r;
+        stream<float>* _in_left;
+        stream<float>* _in_right;
+
+    };
+
     class StereoToMono : public generic_block<StereoToMono> {
     public:
         StereoToMono() {}
@@ -90,4 +149,52 @@ namespace dsp {
         stream<stereo_t>* _in;
 
     };
+
+    class StereoToChannels : public generic_block<StereoToChannels> {
+    public:
+        StereoToChannels() {}
+
+        StereoToChannels(stream<stereo_t>* in) { init(in); }
+
+        ~StereoToChannels() { generic_block<StereoToChannels>::stop(); }
+
+        void init(stream<stereo_t>* in) {
+            _in = in;
+            generic_block<StereoToChannels>::registerInput(_in);
+            generic_block<StereoToChannels>::registerOutput(&out_left);
+            generic_block<StereoToChannels>::registerOutput(&out_right);
+        }
+
+        void setInput(stream<stereo_t>* in) {
+            std::lock_guard<std::mutex> lck(generic_block<StereoToChannels>::ctrlMtx);
+            generic_block<StereoToChannels>::tempStop();
+            generic_block<StereoToChannels>::unregisterInput(_in);
+            _in = in;
+            generic_block<StereoToChannels>::registerInput(_in);
+            generic_block<StereoToChannels>::tempStart();
+        }
+
+        int run() {
+            count = _in->read();
+            if (count < 0) { return -1; }
+
+            for (int i = 0; i < count; i++) {
+                out_left.writeBuf[i] = _in->readBuf[i].l;
+                out_right.writeBuf[i] = _in->readBuf[i].r;
+            }
+
+            _in->flush();
+            if (!out_left.swap(count)) { return -1; }
+            if (!out_right.swap(count)) { return -1; }
+            return count;
+        }
+
+        stream<float> out_left;
+        stream<float> out_right;
+
+    private:
+        int count;
+        stream<stereo_t>* _in;
+
+    };
 }

core/src/dsp/demodulator.h

@@ -1,6 +1,8 @@
 #pragma once
 #include <dsp/block.h>
 #include <volk/volk.h>
+#include <dsp/filter.h>
+#include <dsp/processing.h>
 
 #include <spdlog/spdlog.h>
 
@@ -26,6 +28,86 @@ inline float fast_arctan2(float y, float x) {
 }
 
 namespace dsp {
+    class FloatFMDemod : public generic_block<FloatFMDemod> {
+    public:
+        FloatFMDemod() {}
+
+        FloatFMDemod(stream<complex_t>* in, float sampleRate, float deviation) { init(in, sampleRate, deviation); }
+
+        ~FloatFMDemod() { generic_block<FloatFMDemod>::stop(); }
+
+        void init(stream<complex_t>* in, float sampleRate, float deviation) {
+            _in = in;
+            _sampleRate = sampleRate;
+            _deviation = deviation;
+            phasorSpeed = (2 * FL_M_PI) / (_sampleRate / _deviation);
+            generic_block<FloatFMDemod>::registerInput(_in);
+            generic_block<FloatFMDemod>::registerOutput(&out);
+        }
+
+        void setInput(stream<complex_t>* in) {
+            std::lock_guard<std::mutex> lck(generic_block<FloatFMDemod>::ctrlMtx);
+            generic_block<FloatFMDemod>::tempStop();
+            generic_block<FloatFMDemod>::unregisterInput(_in);
+            _in = in;
+            generic_block<FloatFMDemod>::registerInput(_in);
+            generic_block<FloatFMDemod>::tempStart();
+        }
+
+        void setSampleRate(float sampleRate) {
+            std::lock_guard<std::mutex> lck(generic_block<FloatFMDemod>::ctrlMtx);
+            generic_block<FloatFMDemod>::tempStop();
+            _sampleRate = sampleRate;
+            phasorSpeed = (2 * FL_M_PI) / (_sampleRate / _deviation);
+            generic_block<FloatFMDemod>::tempStart();
+        }
+
+        float getSampleRate() {
+            return _sampleRate;
+        }
+
+        void setDeviation(float deviation) {
+            std::lock_guard<std::mutex> lck(generic_block<FloatFMDemod>::ctrlMtx);
+            generic_block<FloatFMDemod>::tempStop();
+            _deviation = deviation;
+            phasorSpeed = (2 * FL_M_PI) / (_sampleRate / _deviation);
+            generic_block<FloatFMDemod>::tempStart();
+        }
+
+        float getDeviation() {
+            return _deviation;
+        }
+
+        int run() {
+            count = _in->read();
+            if (count < 0) { return -1; }
+
+            // This is somehow faster than volk...
+
+            float diff, currentPhase;
+            for (int i = 0; i < count; i++) {
+                currentPhase = fast_arctan2(_in->readBuf[i].i, _in->readBuf[i].q);
+                diff = currentPhase - phase;
+                if (diff > 3.1415926535f)        { diff -= 2 * 3.1415926535f; }
+                else if (diff <= -3.1415926535f) { diff += 2 * 3.1415926535f; }
+                out.writeBuf[i] = diff / phasorSpeed;
+                phase = currentPhase;
+            }
+
+            _in->flush();
+            if (!out.swap(count)) { return -1; }
+            return count;
+        }
+
+        stream<float> out;
+
+    private:
+        int count;
+        float phase, phasorSpeed, _sampleRate, _deviation;
+        stream<complex_t>* _in;
+
+    };
+
     class FMDemod : public generic_block<FMDemod> {
     public:
         FMDemod() {}
@@ -88,7 +170,8 @@ namespace dsp {
                 diff = currentPhase - phase;
                 if (diff > 3.1415926535f)        { diff -= 2 * 3.1415926535f; }
                 else if (diff <= -3.1415926535f) { diff += 2 * 3.1415926535f; }
-                out.writeBuf[i] = diff / phasorSpeed;
+                out.writeBuf[i].l = diff / phasorSpeed;
+                out.writeBuf[i].r = diff / phasorSpeed;
                 phase = currentPhase;
             }
 
@@ -97,7 +180,7 @@ namespace dsp {
             return count;
         }
 
-        stream<float> out;
+        stream<stereo_t> out;
 
     private:
         int count;
@@ -106,6 +189,149 @@ namespace dsp {
 
     };
 
+    class StereoFMDemod : public generic_block<StereoFMDemod> {
+    public:
+        StereoFMDemod() {}
+
+        StereoFMDemod(stream<complex_t>* in, float sampleRate, float deviation) { init(in, sampleRate, deviation); }
+
+        ~StereoFMDemod() {
+            stop();
+            delete[] doubledPilot;
+            delete[] a_minus_b;
+            delete[] a_out;
+            delete[] b_out;
+        }
+
+        void init(stream<complex_t>* in, float sampleRate, float deviation) {
+            _sampleRate = sampleRate;
+
+            doubledPilot = new float[STREAM_BUFFER_SIZE];
+            a_minus_b = new float[STREAM_BUFFER_SIZE];
+            a_out = new float[STREAM_BUFFER_SIZE];
+            b_out = new float[STREAM_BUFFER_SIZE];
+
+            fmDemod.init(in, sampleRate, deviation);
+            split.init(&fmDemod.out);
+            split.bindStream(&filterInput);
+            split.bindStream(&decodeInput);
+
+            // Filter init
+            win.init(1000, 1000, 19000, sampleRate);
+            filter.init(&filterInput, &win);
+            agc.init(&filter.out, 20.0f, sampleRate);
+
+            generic_block<StereoFMDemod>::registerInput(&decodeInput);
+            generic_block<StereoFMDemod>::registerOutput(&out);
+        }
+
+        void setInput(stream<complex_t>* in) {
+            std::lock_guard<std::mutex> lck(generic_block<StereoFMDemod>::ctrlMtx);
+            generic_block<StereoFMDemod>::tempStop();
+            fmDemod.setInput(in);
+            generic_block<StereoFMDemod>::tempStart();
+        }
+
+        void setSampleRate(float sampleRate) {
+            std::lock_guard<std::mutex> lck(generic_block<StereoFMDemod>::ctrlMtx);
+            generic_block<StereoFMDemod>::tempStop();
+            _sampleRate = sampleRate;
+            fmDemod.setSampleRate(sampleRate);
+            win.setSampleRate(_sampleRate);
+            filter.updateWindow(&win);
+            generic_block<StereoFMDemod>::tempStart();
+        }
+
+        float getSampleRate() {
+            return _sampleRate;
+        }
+
+        void setDeviation(float deviation) {
+            std::lock_guard<std::mutex> lck(generic_block<StereoFMDemod>::ctrlMtx);
+            generic_block<StereoFMDemod>::tempStop();
+            fmDemod.setDeviation(deviation);
+            generic_block<StereoFMDemod>::tempStart();
+        }
+
+        float getDeviation() {
+            return fmDemod.getDeviation();
+        }
+
+        int run() {
+            count = decodeInput.read();
+            if (count < 0) { return -1; }
+            countFilter = agc.out.read();
+            if (countFilter < 0) { return -1; }
+
+            volk_32f_x2_multiply_32f(doubledPilot, agc.out.readBuf, agc.out.readBuf, count);
+
+            volk_32f_x2_multiply_32f(a_minus_b, decodeInput.readBuf, doubledPilot, count);
+
+            volk_32f_x2_add_32f(a_out, decodeInput.readBuf, a_minus_b, count);
+            volk_32f_x2_subtract_32f(b_out, decodeInput.readBuf, a_minus_b, count);
+
+            decodeInput.flush();
+            agc.out.flush();
+
+            volk_32f_x2_interleave_32fc((lv_32fc_t*)out.writeBuf, a_out, b_out, count);
+
+            if (!out.swap(count)) { return -1; }
+            return count;
+        }
+
+        void start() {
+            std::lock_guard<std::mutex> lck(generic_block<StereoFMDemod>::ctrlMtx);
+            if (generic_block<StereoFMDemod>::running) {
+                return;
+            }
+            generic_block<StereoFMDemod>::running = true;
+            generic_block<StereoFMDemod>::doStart();
+            fmDemod.start();
+            split.start();
+            filter.start();
+            agc.start();
+        }
+
+        void stop() {
+            std::lock_guard<std::mutex> lck(generic_block<StereoFMDemod>::ctrlMtx);
+            if (!generic_block<StereoFMDemod>::running) {
+                return;
+            }
+            fmDemod.stop();
+            split.stop();
+            filter.stop();
+            agc.stop();
+            generic_block<StereoFMDemod>::doStop();
+            generic_block<StereoFMDemod>::running = false;
+        }
+
+        stream<stereo_t> out;
+
+    private:
+        int count;
+        int countFilter;
+
+        float _sampleRate;
+
+        FloatFMDemod fmDemod;
+        Splitter<float> split;
+
+        // Pilot tone filtering
+        stream<float> filterInput;
+        FIR<float> filter;
+        filter_window::BlackmanBandpassWindow win;
+        AGC agc;
+
+        stream<float> decodeInput;
+
+        // Buffers
+        float* doubledPilot;
+        float* a_minus_b;
+        float* a_out;
+        float* b_out;
+
+    };
+
     class AMDemod : public generic_block<AMDemod> {
     public:
         AMDemod() {}

core/src/dsp/filter.h

@@ -92,11 +92,11 @@ namespace dsp {
     public:
         BFMDeemp() {}
 
-        BFMDeemp(stream<float>* in, float sampleRate, float tau) { init(in, sampleRate, tau); }
+        BFMDeemp(stream<stereo_t>* in, float sampleRate, float tau) { init(in, sampleRate, tau); }
 
         ~BFMDeemp() { generic_block<BFMDeemp>::stop(); }
 
-        void init(stream<float>* in, float sampleRate, float tau) {
+        void init(stream<stereo_t>* in, float sampleRate, float tau) {
             _in = in;
             _sampleRate = sampleRate;
             _tau = tau;
@@ -106,7 +106,7 @@ namespace dsp {
             generic_block<BFMDeemp>::registerOutput(&out);
         }
 
-        void setInput(stream<float>* in) {
+        void setInput(stream<stereo_t>* in) {
             std::lock_guard<std::mutex> lck(generic_block<BFMDeemp>::ctrlMtx);
             generic_block<BFMDeemp>::tempStop();
             generic_block<BFMDeemp>::unregisterInput(_in);
@@ -132,20 +132,26 @@ namespace dsp {
             if (count < 0) { return -1; }
 
             if (bypass) {
-                memcpy(out.writeBuf, _in->readBuf, count * sizeof(float));
+                memcpy(out.writeBuf, _in->readBuf, count * sizeof(stereo_t));
                 _in->flush();
                 if (!out.swap(count)) { return -1; }
                 return count;
             }
 
-            if (isnan(lastOut)) {
+            if (isnan(lastOutL)) {
-                lastOut = 0.0f;
+                lastOutL = 0.0f;
             }
-            out.writeBuf[0] = (alpha * _in->readBuf[0]) + ((1-alpha) * lastOut);
+            if (isnan(lastOutR)) {
+                lastOutR = 0.0f;
+            }
+            out.writeBuf[0].l = (alpha * _in->readBuf[0].l) + ((1-alpha) * lastOutL);
+            out.writeBuf[0].r = (alpha * _in->readBuf[0].r) + ((1-alpha) * lastOutR);
             for (int i = 1; i < count; i++) {
-                out.writeBuf[i] = (alpha * _in->readBuf[i]) + ((1 - alpha) * out.writeBuf[i - 1]);
+                out.writeBuf[i].l = (alpha * _in->readBuf[i].l) + ((1 - alpha) * out.writeBuf[i - 1].l);
+                out.writeBuf[i].r = (alpha * _in->readBuf[i].r) + ((1 - alpha) * out.writeBuf[i - 1].r);
             }
-            lastOut = out.writeBuf[count - 1];
+            lastOutL = out.writeBuf[count - 1].l;
+            lastOutR = out.writeBuf[count - 1].r;
 
             _in->flush();
             if (!out.swap(count)) { return -1; }
@@ -154,15 +160,16 @@ namespace dsp {
 
         bool bypass = false;
 
-        stream<float> out;
+        stream<stereo_t> out;
 
     private:
         int count;
-        float lastOut = 0.0f;
+        float lastOutL = 0.0f;
+        float lastOutR = 0.0f;
         float alpha;
         float _tau;
         float _sampleRate;
-        stream<float>* _in;
+        stream<stereo_t>* _in;
 
     };
 }

core/src/dsp/math.h

@@ -10,14 +10,14 @@ namespace dsp {
 
         Add(stream<T>* a, stream<T>* b) { init(a, b); }
 
-        ~Add() { generic_block<Add>::stop(); }
+        ~Add() { generic_block<Add<T>>::stop(); }
 
         void init(stream<T>* a, stream<T>* b) {
             _a = a;
             _b = b;
-            generic_block<Add>::registerInput(a);
+            generic_block<Add<T>>::registerInput(a);
-            generic_block<Add>::registerInput(b);
+            generic_block<Add<T>>::registerInput(b);
-            generic_block<Add>::registerOutput(&out);
+            generic_block<Add<T>>::registerOutput(&out);
         }
 
         int run() {
@@ -32,7 +32,57 @@ namespace dsp {
             }
 
             if constexpr (std::is_same_v<T, complex_t> || std::is_same_v<T, stereo_t>) {
-                volk_32fc_x2_add_32fc(out.writeBuf, _a->readBuf, _b->readBuf, a_count);
+                volk_32fc_x2_add_32fc((lv_32fc_t*)out.writeBuf, (lv_32fc_t*)_a->readBuf, (lv_32fc_t*)_b->readBuf, a_count);
+            }
+            else {
+                volk_32f_x2_add_32f(out.writeBuf, _a->readBuf, _b->readBuf, a_count);
+            }
+
+            _a->flush();
+            _b->flush();
+            if (!out.swap(a_count)) { return -1; }
+            return a_count;
+        }
+
+        stream<T> out;
+
+    private:
+        int a_count, b_count;
+        stream<T>* _a;
+        stream<T>* _b;
+
+    };
+
+    template <class T>
+    class Substract : public generic_block<Substract<T>> {
+    public:
+        Substract() {}
+
+        Substract(stream<T>* a, stream<T>* b) { init(a, b); }
+
+        ~Substract() { generic_block<Substract<T>>::stop(); }
+
+        void init(stream<T>* a, stream<T>* b) {
+            _a = a;
+            _b = b;
+            generic_block<Substract<T>>::registerInput(a);
+            generic_block<Substract<T>>::registerInput(b);
+            generic_block<Substract<T>>::registerOutput(&out);
+        }
+
+        int run() {
+            a_count = _a->read();
+            if (a_count < 0) { return -1; }
+            b_count = _b->read();
+            if (b_count < 0) { return -1; }
+            if (a_count != b_count) {
+                _a->flush();
+                _b->flush();
+                return 0;
+            }
+
+            if constexpr (std::is_same_v<T, complex_t> || std::is_same_v<T, stereo_t>) {
+                volk_32f_x2_subtract_32f((float*)out.writeBuf, (float*)_a->readBuf, (float*)_b->readBuf, a_count * 2);
             }
             else {
                 volk_32f_x2_add_32f(out.writeBuf, _a->readBuf, _b->readBuf, a_count);

core/src/dsp/measure.h

@@ -44,7 +44,7 @@ namespace dsp {
             _in->flush();
 
             float _lvlL = 10.0f * logf(maxL);
-            float _lvlR = 10.0f * logf(maxL);
+            float _lvlR = 10.0f * logf(maxR);
             
             // Update max values
             {

core/src/dsp/processing.h

@@ -297,4 +297,72 @@ namespace dsp {
         stream<complex_t>* _in;
 
     };
+
+    template <class T>
+    class Packer : public generic_block<Packer<T>> {
+    public:
+        Packer() {}
+
+        Packer(stream<T>* in, int count) { init(in, count); }
+
+        ~Packer() {
+            generic_block<Packer<T>>::stop();
+        }
+
+        void init(stream<T>* in, int count) {
+            _in = in;
+            samples = count;
+            generic_block<Packer<T>>::registerInput(_in);
+            generic_block<Packer<T>>::registerOutput(&out);
+        }
+
+        void setInput(stream<T>* in) {
+            std::lock_guard<std::mutex> lck(generic_block<Packer<T>>::ctrlMtx);
+            generic_block<Packer<T>>::tempStop();
+            generic_block<Packer<T>>::unregisterInput(_in);
+            _in = in;
+            generic_block<Packer<T>>::registerInput(_in);
+            generic_block<Packer<T>>::tempStart();
+        }
+
+        void setSampleCount(int count) {
+            std::lock_guard<std::mutex> lck(generic_block<Packer<T>>::ctrlMtx);
+            generic_block<Packer<T>>::tempStop();
+            samples = count;
+            generic_block<Packer<T>>::tempStart();
+        }
+
+        int run() {
+            count = _in->read();
+            if (count < 0) {
+                read = 0;
+                return -1;
+            }
+
+            for (int i = 0; i < count; i++) {
+                out.writeBuf[read++] = _in->readBuf[i];
+                if (read >= samples) {
+                    read = 0;
+                    if (!out.swap(samples)) {
+                        _in->flush();
+                        read = 0;
+                        return -1;
+                    }
+                }
+            }
+
+            _in->flush();
+            
+            return count;
+        }
+
+        stream<T> out;
+
+    private:
+        int count;
+        int samples = 1;
+        int read = 0;
+        stream<T>* _in;
+
+    };
 }

core/src/dsp/resampling.h

@@ -117,7 +117,7 @@ namespace dsp {
                     outIndex++;
                 }
             }
-            if constexpr (std::is_same_v<T, complex_t>) {
+            if constexpr (std::is_same_v<T, complex_t> || std::is_same_v<T, stereo_t>) {
                 for (int i = 0; outIndex < outCount; i += _decim) {
                     int phase = i % _interp;
                     volk_32fc_32f_dot_prod_32fc((lv_32fc_t*)&out.writeBuf[outIndex], (lv_32fc_t*)&buffer[(i / _interp)], tapPhases[phase], tapsPerPhase);

core/src/dsp/window.h

@@ -1,5 +1,6 @@
 #pragma once
 #include <dsp/block.h>
+#include <dsp/types.h>
 
 namespace dsp {
     namespace filter_window {
@@ -9,6 +10,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() {}
@@ -72,5 +79,145 @@ namespace dsp {
             float _cutoff, _transWidth, _sampleRate;
 
         };
+
+        class BlackmanBandpassWindow : public filter_window::generic_window {
+        public:
+            BlackmanBandpassWindow() {}
+            BlackmanBandpassWindow(float cutoff, float transWidth, float offset, float sampleRate) { init(cutoff, transWidth, offset, sampleRate); }
+
+            void init(float cutoff, float transWidth, float offset, float sampleRate) {
+                _cutoff = cutoff;
+                _transWidth = transWidth;
+                _offset = offset;
+                _sampleRate = sampleRate;
+            }
+
+            void setSampleRate(float sampleRate) {
+                _sampleRate = sampleRate;
+            }
+
+            void setCutoff(float cutoff) {
+                _cutoff = cutoff;
+            }
+            
+            void setTransWidth(float transWidth) {
+                _transWidth = transWidth;
+            }
+
+            void setOffset(float offset) {
+                _offset = offset;
+            }
+
+            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(float* taps, int tapCount, float factor = 1.0f) {
+                float fc = _cutoff / _sampleRate;
+                if (fc > 1.0f) {
+                    fc = 1.0f;
+                }
+                float tc = tapCount;
+                float sum = 0.0f;
+                float val;
+                for (int i = 0; i < tapCount; i++) {
+                    val = (sin(2.0f * FL_M_PI * fc * ((float)i - (tc / 2))) / ((float)i - (tc / 2))) * 
+                        (0.42f - (0.5f * cos(2.0f * FL_M_PI / tc)) + (0.8f * cos(4.0f * FL_M_PI / tc)));
+                    taps[i] = val; // tapCount - i - 1
+                    sum += val;
+                }
+                for (int i = 0; i < tapCount; i++) {
+                    taps[i] *= cos(2.0f * (_offset / _sampleRate) * FL_M_PI * (float)i);
+                    taps[i] *= factor;
+                    taps[i] /= sum;
+                }
+            }
+
+        private:
+            float _cutoff, _transWidth, _sampleRate, _offset;
+
+        };
+
+        class BlackmanBandpassWindow : public filter_window::generic_complex_window {
+        public:
+            BlackmanBandpassWindow() {}
+            BlackmanBandpassWindow(float cutoff, float transWidth, float offset, float sampleRate) { init(cutoff, transWidth, offset, sampleRate); }
+
+            void init(float cutoff, float transWidth, float offset, float sampleRate) {
+                _cutoff = cutoff;
+                _transWidth = transWidth;
+                _offset = offset;
+                _sampleRate = sampleRate;
+            }
+
+            void setSampleRate(float sampleRate) {
+                _sampleRate = sampleRate;
+            }
+
+            void setCutoff(float cutoff) {
+                _cutoff = cutoff;
+            }
+            
+            void setTransWidth(float transWidth) {
+                _transWidth = transWidth;
+            }
+
+            void setOffset(float offset) {
+                _offset = offset;
+            }
+
+            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) {
+                float fc = _cutoff / _sampleRate;
+                if (fc > 1.0f) {
+                    fc = 1.0f;
+                }
+                float tc = tapCount;
+                float sum = 0.0f;
+                float val;
+                for (int i = 0; i < tapCount; i++) {
+                    val = (sin(2.0f * FL_M_PI * fc * ((float)i - (tc / 2))) / ((float)i - (tc / 2))) * 
+                        (0.42f - (0.5f * cos(2.0f * FL_M_PI / tc)) + (0.8f * cos(4.0f * FL_M_PI / tc)));
+                    taps[i] = val; // tapCount - i - 1
+                    sum += val;
+                }
+                for (int i = 0; i < tapCount; i++) {
+                    taps[i] *= cos(2.0f * (_offset / _sampleRate) * FL_M_PI * (float)i);
+                    taps[i] *= factor;
+                    taps[i] /= sum;
+                }
+            }
+
+        private:
+            float _cutoff, _transWidth, _sampleRate, _offset;
+
+        };
     }
 }

core/src/gui/widgets/waterfall.cpp

@@ -61,29 +61,30 @@ double findBestRange(double bandwidth, int maxSteps) {
 }
 
 void printAndScale(double freq, char* buf) {
-    if (freq < 1000) {
+    double freqAbs = fabs(freq);
-        sprintf(buf, "%.3lf", freq);
+    if (freqAbs < 1000) {
+        sprintf(buf, "%.6g", freq);
     }
-    else if (freq < 1000000) {
+    else if (freqAbs < 1000000) {
-        sprintf(buf, "%.3lfK", freq / 1000.0);
+        sprintf(buf, "%.6lgK", freq / 1000.0);
     }
-    else if (freq < 1000000000) {
+    else if (freqAbs < 1000000000) {
-        sprintf(buf, "%.3lfM", freq / 1000000.0);
+        sprintf(buf, "%.6lgM", freq / 1000000.0);
     }
-    else if (freq < 1000000000000) {
+    else if (freqAbs < 1000000000000) {
-        sprintf(buf, "%.3lfG", freq / 1000000000.0);
+        sprintf(buf, "%.6lgG", freq / 1000000000.0);
-    }
-    for (int i = strlen(buf) - 2; i >= 0; i--) {
-        if (buf[i] != '0') {
-            if (buf[i] == '.') {
-                i--;
-            }
-            char scale = buf[strlen(buf) - 1];
-            buf[i + 1] = scale;
-            buf[i + 2] = 0;
-            return;
-        }
     }
+    // for (int i = strlen(buf) - 2; i >= 0; i--) {
+    //     if (buf[i] != '0') {
+    //         if (buf[i] == '.') {
+    //             i--;
+    //         }
+    //         char scale = buf[strlen(buf) - 1];
+    //         buf[i + 1] = scale;
+    //         buf[i + 2] = 0;
+    //         return;
+    //     }
+    // }
 }
 
 namespace ImGui {

core/src/module.cpp

@@ -17,7 +17,7 @@ ModuleManager::Module_t ModuleManager::loadModule(std::string path) {
 #ifdef _WIN32
     mod.handle = LoadLibraryA(path.c_str());
     if (mod.handle == NULL) {
-        spdlog::error("Couldn't load {0}.", path);
+        spdlog::error("Couldn't load {0}. Error code: {1}", path, GetLastError());
         mod.handle = NULL;
         return mod;
     }

radio/src/cw_demod.h

@@ -170,7 +170,7 @@ private:
 
     const float bwMax = 500;
     const float bwMin = 100;
-    const float bbSampRate = 6000;
+    const float bbSampRate = 500;
 
     std::string uiPrefix;
     float snapInterval = 10;

radio/src/fm_demod.h

@@ -48,15 +48,12 @@ public:
         resamp.init(&demod.out, &win, bbSampRate, audioSampRate);
         win.setSampleRate(bbSampRate * resamp.getInterpolation());
         resamp.updateWindow(&win);
-
-        m2s.init(&resamp.out);
     }
 
     void start() {
         squelch.start();
         demod.start();
         resamp.start();
-        m2s.start();
         running = true;
     }
 
@@ -64,7 +61,6 @@ public:
         squelch.stop();
         demod.stop();
         resamp.stop();
-        m2s.stop();
         running = false;
     }
     
@@ -108,7 +104,7 @@ public:
     }
 
     dsp::stream<dsp::stereo_t>* getOutput() {
-        return &m2s.out;
+        return &resamp.out;
     }
     
     void showMenu() {
@@ -172,8 +168,7 @@ private:
     dsp::Squelch squelch;
     dsp::FMDemod demod;
     dsp::filter_window::BlackmanWindow win;
-    dsp::PolyphaseResampler<float> resamp;
+    dsp::PolyphaseResampler<dsp::stereo_t> resamp;
-    dsp::MonoToStereo m2s;
 
     ConfigManager* _config;
 

radio/src/wfm_demod.h

@@ -57,7 +57,6 @@ public:
 
         deemp.init(&resamp.out, audioSampRate, tau);
 
-        m2s.init(&deemp.out);
     }
 
     void start() {
@@ -65,7 +64,6 @@ public:
         demod.start();
         resamp.start();
         deemp.start();
-        m2s.start();
         running = true;
     }
 
@@ -74,7 +72,6 @@ public:
         demod.stop();
         resamp.stop();
         deemp.stop();
-        m2s.stop();
         running = false;
     }
     
@@ -121,7 +118,7 @@ public:
     }
 
     dsp::stream<dsp::stereo_t>* getOutput() {
-        return &m2s.out;
+        return &deemp.out;
     }
     
     void showMenu() {
@@ -209,9 +206,8 @@ private:
     dsp::Squelch squelch;
     dsp::FMDemod demod;
     dsp::filter_window::BlackmanWindow win;
-    dsp::PolyphaseResampler<float> resamp;
+    dsp::PolyphaseResampler<dsp::stereo_t> resamp;
     dsp::BFMDeemp deemp;
-    dsp::MonoToStereo m2s;
 
     ConfigManager* _config;
 

sdrplay_source/src/main.cpp

@@ -34,7 +34,7 @@ public:
 
         sdrplay_api_Open();
 
-        sampleRate = 10000000.0;
+        sampleRate = 8000000.0;
 
         handler.ctx = this;
         handler.selectHandler = menuSelected;
@@ -185,12 +185,14 @@ private:
         // Do start procedure here
         sdrplay_api_ErrT err;
 
-        _this->openDevParams->devParams->samplesPerPkt = 8000000 / 200;
+        _this->bufferIndex = 0;
+        _this->bufferSize = 8000000 / 200;
+
         _this->openDevParams->devParams->fsFreq.fsHz = 8000000;
         _this->openDevParams->rxChannelA->tunerParams.bwType = sdrplay_api_BW_8_000;
         _this->openDevParams->rxChannelA->tunerParams.rfFreq.rfHz = _this->freq;
-        _this->openDevParams->rxChannelA->tunerParams.gain.gRdB = 30;
+        _this->openDevParams->rxChannelA->tunerParams.gain.gRdB = 59;
-        _this->openDevParams->rxChannelA->tunerParams.gain.LNAstate = 0;
+        _this->openDevParams->rxChannelA->tunerParams.gain.LNAstate = 9;
         _this->openDevParams->rxChannelA->ctrlParams.agc.enable = sdrplay_api_AGC_DISABLE;
         //_this->openDevParams->devParams->
 
@@ -275,7 +277,27 @@ private:
     }
 
     void RSP1AMenu(float menuWidth) {
+        ImGui::PushItemWidth(menuWidth - ImGui::CalcTextSize("LNA Gain").x - 10);
+        ImGui::Text("LNA Gain");
+        ImGui::SameLine();
+        float pos = ImGui::GetCursorPosX();
+        if (ImGui::SliderInt(CONCAT("##sdrplay_lna_gain", name), &gainTest, 9, 0, "")) {
+            openDevParams->rxChannelA->tunerParams.gain.LNAstate = gainTest;
+            sdrplay_api_Update(openDev.dev, openDev.tuner, sdrplay_api_Update_Tuner_Gr, sdrplay_api_Update_Ext1_None);
+        }
 
+        ImGui::Text("IF Gain");
+        ImGui::SameLine();
+        ImGui::SetCursorPosX(pos);
+        if (ImGui::SliderInt(CONCAT("##sdrplay_gain", name), &gainTest2, 59, 20, "")) {
+            openDevParams->rxChannelA->tunerParams.gain.gRdB = gainTest2;
+            sdrplay_api_Update(openDev.dev, openDev.tuner, sdrplay_api_Update_Tuner_Gr, sdrplay_api_Update_Ext1_None);
+        }
+        ImGui::PopItemWidth();
+
+        bool test = false;
+        ImGui::Checkbox("FM Notch", &test);
+        ImGui::Checkbox("DAB Notch", &test);
     }
 
     void RSP2Menu(float menuWidth) {
@@ -299,10 +321,15 @@ private:
         SDRPlaySourceModule* _this = (SDRPlaySourceModule*)cbContext;
         if (!_this->running) { return; }
         for (int i = 0; i < numSamples; i++) {
-            _this->stream.writeBuf[i].i = (float)xq[i] / 32768.0f;
+            int id = _this->bufferIndex++;
-            _this->stream.writeBuf[i].q = (float)xi[i] / 32768.0f;
+            _this->stream.writeBuf[id].i = (float)xq[i] / 32768.0f;
+            _this->stream.writeBuf[id].q = (float)xi[i] / 32768.0f;
+
+            if (_this->bufferIndex >= _this->bufferSize) {
+                _this->stream.swap(_this->bufferSize);
+                _this->bufferIndex = 0;
+            }
         }
-        _this->stream.swap(numSamples);
     }
 
     static void eventCB(sdrplay_api_EventT eventId, sdrplay_api_TunerSelectT tuner,
@@ -327,7 +354,11 @@ private:
     int devId = 0;
     int srId = 0;
 
-    int gainTest = 0;
+    int gainTest = 9;
+    int gainTest2 = 59;
+
+    int bufferSize = 0;
+    int bufferIndex = 0;
 
     std::vector<sdrplay_api_DeviceT> devList;
     std::string devListTxt;