Fixed IF reduction not working with multivfo + Added beginning of IF notch code

core/src/dsp/noise_reduction.h

@@ -384,4 +384,85 @@ namespace dsp {
         stream<complex_t>* _in;
 
     };
+
+    class NotchFilter : public generic_block<NotchFilter> {
+    public:
+        NotchFilter() {}
+
+        NotchFilter(stream<complex_t>* in, float rate, float offset, float sampleRate) { init(in, rate, offset, sampleRate); }
+
+        void init(stream<complex_t>* in, float rate, float offset, float sampleRate) {
+            _in = in;
+            correctionRate = rate;
+            _offset = offset;
+            _sampleRate = sampleRate;
+
+            phaseDelta = lv_cmake(std::cos((-_offset / _sampleRate) * 2.0f * FL_M_PI), std::sin((-_offset / _sampleRate) * 2.0f * FL_M_PI));
+            phaseDeltaConj = {phaseDelta.real(), -phaseDelta.imag()};
+
+            generic_block<NotchFilter>::registerInput(_in);
+            generic_block<NotchFilter>::registerOutput(&out);
+            generic_block<NotchFilter>::_block_init = true;
+        }
+
+        void setInput(stream<complex_t>* in) {
+            assert(generic_block<NotchFilter>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<NotchFilter>::ctrlMtx);
+            generic_block<NotchFilter>::tempStop();
+            generic_block<NotchFilter>::unregisterInput(_in);
+            _in = in;
+            generic_block<NotchFilter>::registerInput(_in);
+            generic_block<NotchFilter>::tempStart();
+        }
+
+        void setCorrectionRate(float rate) {
+            correctionRate = rate;
+        }
+
+        void setOffset(float offset) {
+            _offset = offset;
+            phaseDelta = lv_cmake(std::cos((-_offset / _sampleRate) * 2.0f * FL_M_PI), std::sin((-_offset / _sampleRate) * 2.0f * FL_M_PI));
+            phaseDeltaConj = {phaseDelta.real(), -phaseDelta.imag()};
+        }
+
+        void setSampleRate(float sampleRate) {
+            _sampleRate = sampleRate;
+            phaseDelta = lv_cmake(std::cos((-_offset / _sampleRate) * 2.0f * FL_M_PI), std::sin((-_offset / _sampleRate) * 2.0f * FL_M_PI));
+            phaseDeltaConj = {phaseDelta.real(), -phaseDelta.imag()};
+        }
+
+        int run() {
+            int count = _in->read();
+            if (count < 0) { return -1; }
+
+            volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)_in->readBuf, (lv_32fc_t*)_in->readBuf, phaseDelta, &inPhase, count);
+
+            for (int i = 0; i < count; i++) {
+                out.writeBuf[i] = _in->readBuf[i] - offset;
+                offset = offset + (out.writeBuf[i] * correctionRate);
+            }
+
+            volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)out.writeBuf, (lv_32fc_t*)out.writeBuf, phaseDeltaConj, &outPhase, count);
+
+            _in->flush();
+
+            if (!out.swap(count)) { return -1; }
+
+            return count;
+        }
+
+        stream<complex_t> out;
+
+    private:
+        stream<complex_t>* _in;
+        complex_t offset = {0, 0};
+        lv_32fc_t inPhase = {1, 0};
+        lv_32fc_t outPhase = {4, 0};
+        lv_32fc_t phaseDelta;
+        lv_32fc_t phaseDeltaConj;
+        float _offset;
+        float _sampleRate;
+        float correctionRate;
+
+    };
 }

core/src/dsp/window.h

@@ -271,37 +271,125 @@ namespace dsp {
 
     };
 
-    class NotchWindow : public filter_window::generic_window {
+    // class NotchWindow : public filter_window::generic_complex_window {
+    // public:
+    //     NotchWindow() {}
+
+    //     NotchWindow(float frequency, float width, float sampleRate, int tapCount) { init(frequency, width, sampleRate, tapCount); }
+
+    //     ~NotchWindow() {
+    //         if (fft_in) { fftwf_free(fft_in); }
+    //         if (fft_out) { fftwf_free(fft_out); }
+    //         fftwf_destroy_plan(fft_plan);
+    //     }
+
+    //     void init(float frequency, float width, float sampleRate, int tapCount) {
+    //         _frequency = frequency;
+    //         _width = width;
+    //         _sampleRate = sampleRate;
+    //         _tapCount = tapCount;
+
+    //         // Ensure the number of taps is even
+    //         if (_tapCount & 1) { _tapCount++; }    
+        
+    //         fft_in = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
+    //         fft_out = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
+
+    //         fft_plan = fftwf_plan_dft_1d(_tapCount, (fftwf_complex*)fft_in, (fftwf_complex*)fft_out, FFTW_BACKWARD, FFTW_ESTIMATE);
+    //     }
+
+    //     void setFrequency(float frequency) {
+    //         _frequency = frequency;
+    //     }
+
+    //     void setWidth(float width) {
+    //         _width = width;
+    //     }
+
+    //     void setSampleRate(float sampleRate) {
+    //         _sampleRate = sampleRate;
+    //     }
+
+    //     void setTapCount(int count) {
+    //         _tapCount = count;
+
+    //         // Ensure the number of taps is even
+
+    //         // Free buffers
+    //         if (fft_in) { fftwf_free(fft_in); }
+    //         if (fft_out) { fftwf_free(fft_out); }
+    //         fftwf_destroy_plan(fft_plan);
+
+    //         // Reallocate
+    //         fft_in = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
+    //         fft_out = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
+
+    //         // Create new plan
+    //         fft_plan = fftwf_plan_dft_1d(_tapCount, (fftwf_complex*)fft_in, (fftwf_complex*)fft_out, FFTW_BACKWARD, FFTW_ESTIMATE);
+    //     }
+
+    //     int getTapCount() {
+    //         return _tapCount;
+    //     }
+
+    //     void createTaps(complex_t* taps, int tapCount, float factor = 1.0f) {
+    //         float ratio = _sampleRate / (float)tapCount;
+    //         int thalf = tapCount / 2;
+    //         float start = _frequency - (_width / 2.0f);
+    //         float stop = _frequency + (_width / 2.0f);
+            
+    //         // Fill taps
+    //         float freq;
+    //         float pratio = 2.0f * FL_M_PI / (float)tapCount;
+    //         complex_t phaseDiff = {cosf(pratio), -sinf(pratio)};
+    //         complex_t phasor = {1, 0};
+    //         for (int i = 0; i < tapCount; i++) {
+    //             freq = (i < thalf) ? ((float)i * ratio) : -((float)(tapCount - i) * ratio);
+    //             if (freq >= start && freq <= stop) {
+    //                 fft_in[i] = {0, 0};
+    //             }
+    //             else {
+    //                 fft_in[i] = phasor;
+    //             }
+    //             phasor = phasor * phaseDiff;
+    //         }
+
+    //         // Run IFFT
+    //         fftwf_execute(fft_plan);
+
+    //         // Apply window and copy to output
+    //         for (int i = 0; i < tapCount; i++) {
+    //             taps[tapCount - i - 1] = fft_out[i] / (float)tapCount;
+    //         }
+    //     }
+
+    // private:
+    //     complex_t* fft_in = NULL;
+    //     complex_t* fft_out = NULL;
+    //     float _frequency, _width, _sampleRate;
+    //     int _tapCount;
+
+    //     fftwf_plan fft_plan;
+
+    // };
+
+    class NotchWindow : public filter_window::generic_complex_window {
     public:
         NotchWindow() {}
 
         NotchWindow(float frequency, float width, float sampleRate, int tapCount) { init(frequency, width, sampleRate, tapCount); }
 
-        ~NotchWindow() {
-            if (fft_in) { fftwf_free(fft_in); }
-            if (fft_out) { fftwf_free(fft_out); }
-            fftwf_destroy_plan(fft_plan);
-        }
-
         void init(float frequency, float width, float sampleRate, int tapCount) {
             _frequency = frequency;
-            _width = width;
             _sampleRate = sampleRate;
-            _tapCount = _tapCount;
+            _tapCount = tapCount;
-
-            fft_in = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
-            fft_out = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
-
-            fft_plan = fftwf_plan_dft_1d(_tapCount, (fftwf_complex*)fft_in, (fftwf_complex*)fft_out, FFTW_BACKWARD, FFTW_ESTIMATE);
         }
 
         void setFrequency(float frequency) {
             _frequency = frequency;
         }
 
-        void setWidth(float width) {
+        void setWidth(float width) {}
-            _width = width;
-        }
 
         void setSampleRate(float sampleRate) {
             _sampleRate = sampleRate;
@@ -309,31 +397,28 @@ namespace dsp {
 
         void setTapCount(int count) {
             _tapCount = count;
-            if (fft_in) { fftwf_free(fft_in); }
-            if (fft_out) { fftwf_free(fft_out); }
-            fftwf_destroy_plan(fft_plan);
-
-            fft_in = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
-            fft_out = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
-
-            fft_plan = fftwf_plan_dft_1d(_tapCount, (fftwf_complex*)fft_in, (fftwf_complex*)fft_out, FFTW_BACKWARD, FFTW_ESTIMATE);
         }
 
         int getTapCount() {
             return _tapCount;
         }
 
-        void createTaps(float* taps, int tapCount, float factor = 1.0f) {
+        void createTaps(complex_t* taps, int tapCount, float factor = 1.0f) {
+            // Generate exponential decay
+            float fact = 1.0f / (float)tapCount;
+            for (int i = 0; i < tapCount; i++) {
+                taps[tapCount - i - 1] = {expf(-fact*i) * (float)window_function::blackman(i, tapCount - 1), 0};
+            }
 
+            // Frequency translate it to the right place
+            lv_32fc_t phase = lv_cmake(1.0f, 0.0f);
+            lv_32fc_t phaseDelta = lv_cmake(std::cos((-_frequency / _sampleRate) * 2.0f * FL_M_PI), std::sin((-_frequency / _sampleRate) * 2.0f * FL_M_PI));
+            volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)taps, (lv_32fc_t*)taps, phaseDelta, &phase, tapCount);
         }
 
     private:
-        complex_t* fft_in = NULL;
+        float _frequency, _sampleRate;
-        complex_t* fft_out = NULL;
-        float _frequency, _width, _sampleRate;
         int _tapCount;
 
-        fftwf_plan fft_plan;
-
     };
 }

decoder_modules/radio/src/radio_module.h

@@ -60,10 +60,12 @@ public:
         ifChain.init(vfo->output, &ifChainOutputChanged);
 
         fmnr.block.init(NULL, 32);
+        notch.block.init(NULL, 0.5, 0, 250000); // TODO: The rate has to depend on IF sample rate so the width is always the same
         squelch.block.init(NULL, MIN_SQUELCH);
 
-        ifChain.add(&fmnr);
+        ifChain.add(&notch);
         ifChain.add(&squelch);
+        ifChain.add(&fmnr);
 
         // Load configuration for and enabled all demodulators
         EventHandler<dsp::stream<dsp::stereo_t>*> _demodOutputChangeHandler;
@@ -256,9 +258,20 @@ private:
         }
         if (!_this->squelchEnabled && _this->enabled) { style::endDisabled(); }
 
+        // // Notch filter
+        // if (ImGui::Checkbox("Notch##_radio_notch_ena_", &_this->notchEnabled)) {
+        //     _this->ifChain.setState(&_this->notch, _this->notchEnabled);
+        // }
+        // if (ImGui::SliderFloat(("NF##_radio_notch_freq_" + _this->name).c_str(), &_this->notchPos, -7500, 7500)) {
+        //     _this->notch.block.setOffset(_this->notchPos);
+        // }
+        // if (ImGui::SliderFloat(("NW##_radio_notch_width_" + _this->name).c_str(), &_this->notchWidth, 0, 1000)) {
+        //     // TODO: Implement
+        // }
+
         // FM IF Noise Reduction
         if (_this->FMIFNRAllowed) {
-            if (ImGui::Checkbox("IF Noise Reduction##_radio_fmifnr_ena_", &_this->FMIFNREnabled)) {
+            if (ImGui::Checkbox(("IF Noise Reduction##_radio_fmifnr_ena_" + _this->name).c_str(), &_this->FMIFNREnabled)) {
                 _this->setFMIFNREnabled(_this->FMIFNREnabled);
             }
         }
@@ -343,6 +356,9 @@ private:
         // Configure FM IF Noise Reduction
         setFMIFNREnabled(FMIFNRAllowed ? FMIFNREnabled : false);
 
+        // Configure notch
+        notch.block.setSampleRate(selectedDemod->getIFSampleRate());
+
         // Configure squelch
         squelch.block.setLevel(squelchLevel);
         setSquelchEnabled(squelchEnabled);
@@ -550,6 +566,7 @@ private:
     // IF chain
     dsp::Chain<dsp::complex_t> ifChain;
     dsp::ChainLink<dsp::FMIFNoiseReduction, dsp::complex_t> fmnr;
+    dsp::ChainLink<dsp::NotchFilter, dsp::complex_t> notch;
     dsp::ChainLink<dsp::Squelch, dsp::complex_t> squelch;
 
     // Audio chain
@@ -579,6 +596,10 @@ private:
     bool FMIFNRAllowed;
     bool FMIFNREnabled = false;
 
+    bool notchEnabled = false;
+    float notchPos = 0;
+    float notchWidth = 500;
+
     const double MIN_SQUELCH = -100.0;
     const double MAX_SQUELCH = 0.0;