new FM IF noise reduction + bugfix

core/src/dsp/noise_reduction.h

@@ -3,9 +3,143 @@
 #include <dsp/utils/window_functions.h>
 #include <fftw3.h>
 
-#define NR_TAP_COUNT    4096
+#define NR_TAP_COUNT    64
 
 namespace dsp {
+    class FMIFNoiseReduction : public generic_block<FMIFNoiseReduction> {
+    public:
+        FMIFNoiseReduction() {}
+
+        FMIFNoiseReduction(stream<complex_t>* in, int tapCount) { init(in, tapCount); }
+
+        ~FMIFNoiseReduction() {
+            if (!generic_block<FMIFNoiseReduction>::_block_init) { return; }
+            generic_block<FMIFNoiseReduction>::stop();
+            fftwf_destroy_plan(forwardPlan);
+            fftwf_destroy_plan(backwardPlan);
+            fftwf_free(delay);
+            fftwf_free(fft_in);
+            fftwf_free(fft_window);
+            fftwf_free(amp_buf);
+            fftwf_free(fft_cout);
+            fftwf_free(fft_fcout);
+        }
+
+        void init(stream<complex_t>* in, int tapCount) {
+            _in = in;
+            _tapCount = tapCount;
+
+            delay = (complex_t*)fftwf_malloc(sizeof(complex_t)*STREAM_BUFFER_SIZE);
+            fft_in = (complex_t*)fftwf_malloc(sizeof(complex_t)*_tapCount);
+            fft_window = (float*)fftwf_malloc(sizeof(float)*_tapCount);
+            amp_buf = (float*)fftwf_malloc(sizeof(float)*_tapCount);
+            fft_cout = (complex_t*)fftwf_malloc(sizeof(complex_t)*_tapCount);
+            fft_fcout = (complex_t*)fftwf_malloc(sizeof(complex_t)*_tapCount);
+            delay_start = &delay[_tapCount];
+
+            memset(delay, 0, sizeof(complex_t)*STREAM_BUFFER_SIZE);
+            memset(fft_in, 0, sizeof(complex_t)*_tapCount);
+            memset(amp_buf, 0, sizeof(float)*_tapCount);
+            memset(fft_cout, 0, sizeof(complex_t)*_tapCount);
+            memset(fft_fcout, 0, sizeof(complex_t)*_tapCount);
+            
+            for (int i = 0; i < _tapCount; i++) {
+                fft_window[i] = window_function::blackman(i, _tapCount - 1);
+            }
+
+            forwardPlan = fftwf_plan_dft_1d(_tapCount, (fftwf_complex*)fft_in, (fftwf_complex*)fft_cout, FFTW_FORWARD, FFTW_ESTIMATE);
+            backwardPlan = fftwf_plan_dft_1d(_tapCount, (fftwf_complex*)fft_cout, (fftwf_complex*)fft_fcout, FFTW_BACKWARD, FFTW_ESTIMATE);
+
+            generic_block<FMIFNoiseReduction>::registerInput(_in);
+            generic_block<FMIFNoiseReduction>::registerOutput(&out);
+            generic_block<FMIFNoiseReduction>::_block_init = true;
+        }
+
+        void setInput(stream<complex_t>* in) {
+            assert(generic_block<FMIFNoiseReduction>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<FMIFNoiseReduction>::ctrlMtx);
+            generic_block<FMIFNoiseReduction>::tempStop();
+            generic_block<FMIFNoiseReduction>::unregisterInput(_in);
+            _in = in;
+            generic_block<FMIFNoiseReduction>::registerInput(_in);
+            generic_block<FMIFNoiseReduction>::tempStart();
+        }
+
+        void setLevel(float level) {
+            _level = powf(10.0f, level / 10.0f);
+        }
+
+        int run() {
+            int count = _in->read();
+            if (count < 0) { return -1; }
+
+            // Bypass
+            if (!bypass) {
+                memcpy(out.writeBuf, _in->readBuf, count * sizeof(complex_t));
+                _in->flush();
+                if (!out.swap(count)) { return -1; }
+                return count;
+            }
+
+            // Write to delay buffer
+            memcpy(delay_start, _in->readBuf, count * sizeof(complex_t));
+
+            uint32_t idx = 0;
+            float actLevel = 0;
+
+            // Iterate the FFT
+            for (int i = 0; i < count; i++) {
+                // Apply windows
+                volk_32fc_32f_multiply_32fc((lv_32fc_t*)fft_in, (lv_32fc_t*)&delay[i], fft_window, _tapCount);
+
+                // Do forward FFT
+                fftwf_execute(forwardPlan);
+
+                // Process bins here
+                volk_32fc_magnitude_32f(amp_buf, (lv_32fc_t*)fft_cout, _tapCount);
+                volk_32f_index_max_32u(&idx, amp_buf, _tapCount);
+
+                for (int j = 0; j < _tapCount; j++) {
+                    if (j == idx) { continue; }
+                    fft_cout[j] = {0, 0};
+                }
+
+                // Do reverse FFT and get first element
+                fftwf_execute(backwardPlan);
+                out.writeBuf[i] = fft_fcout[_tapCount/2];
+            }
+
+            volk_32f_s32f_multiply_32f((float*)out.writeBuf, (float*)out.writeBuf, 1.0f/(float)_tapCount, count * 2);
+
+            // Copy last values to delay
+            memmove(delay, &delay[count], _tapCount * sizeof(complex_t));
+
+            _in->flush();
+            if (!out.swap(count)) { return -1; }
+            return count;
+        }
+        
+        bool bypass = true;
+        stream<complex_t> out;
+
+        float _level = 0.0f;
+
+    private:
+        stream<complex_t>* _in;
+        fftwf_plan forwardPlan;
+        fftwf_plan backwardPlan;
+        complex_t* delay;
+        complex_t* fft_in;
+        float* fft_window;
+        float* amp_buf;
+        complex_t* delay_start;
+        complex_t* fft_cout;
+        complex_t* fft_fcout;
+
+        int _tapCount;
+
+    };
+
     class FFTNoiseReduction : public generic_block<FFTNoiseReduction> {
     public:
         FFTNoiseReduction() {}