From 241632288e03aad234087a858cb5869b425166ef Mon Sep 17 00:00:00 2001 From: AlexandreRouma Date: Thu, 9 Dec 2021 19:43:57 +0100 Subject: [PATCH] Fixed IF reduction not working with multivfo + Added beginning of IF notch code --- core/src/dsp/noise_reduction.h | 81 ++++++++++++ core/src/dsp/window.h | 149 ++++++++++++++++++----- decoder_modules/radio/src/radio_module.h | 25 +++- 3 files changed, 221 insertions(+), 34 deletions(-) diff --git a/core/src/dsp/noise_reduction.h b/core/src/dsp/noise_reduction.h index 6cf1b94d..9409161b 100644 --- a/core/src/dsp/noise_reduction.h +++ b/core/src/dsp/noise_reduction.h @@ -384,4 +384,85 @@ namespace dsp { stream* _in; }; + + class NotchFilter : public generic_block { + public: + NotchFilter() {} + + NotchFilter(stream* in, float rate, float offset, float sampleRate) { init(in, rate, offset, sampleRate); } + + void init(stream* 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::registerInput(_in); + generic_block::registerOutput(&out); + generic_block::_block_init = true; + } + + void setInput(stream* in) { + assert(generic_block::_block_init); + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + generic_block::unregisterInput(_in); + _in = in; + generic_block::registerInput(_in); + generic_block::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 out; + + private: + stream* _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; + + }; } \ No newline at end of file diff --git a/core/src/dsp/window.h b/core/src/dsp/window.h index 89e760a2..7f45e7c7 100644 --- a/core/src/dsp/window.h +++ b/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; - - 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); + _tapCount = tapCount; } void setFrequency(float frequency) { _frequency = frequency; } - void setWidth(float width) { - _width = width; - } + void setWidth(float 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; - complex_t* fft_out = NULL; - float _frequency, _width, _sampleRate; + float _frequency, _sampleRate; int _tapCount; - fftwf_plan fft_plan; - }; } \ No newline at end of file diff --git a/decoder_modules/radio/src/radio_module.h b/decoder_modules/radio/src/radio_module.h index 253deb1f..a2f2ffec 100644 --- a/decoder_modules/radio/src/radio_module.h +++ b/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(¬ch); ifChain.add(&squelch); + ifChain.add(&fmnr); // Load configuration for and enabled all demodulators EventHandler*> _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 ifChain; dsp::ChainLink fmnr; + dsp::ChainLink notch; dsp::ChainLink 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;