diff --git a/core/src/dsp/demod/fm.h b/core/src/dsp/demod/fm.h index 8a028f5f..96b025a8 100644 --- a/core/src/dsp/demod/fm.h +++ b/core/src/dsp/demod/fm.h @@ -3,6 +3,8 @@ #include "quadrature.h" #include "../filter/fir.h" #include "../taps/low_pass.h" +#include "../taps/high_pass.h" +#include "../taps/band_pass.h" #include "../convert/mono_to_stereo.h" namespace dsp::demod { @@ -17,22 +19,26 @@ namespace dsp::demod { ~FM() { if (!base_type::_block_init) { return; } base_type::stop(); - dsp::taps::free(lpfTaps); + dsp::taps::free(filterTaps); } - void init(dsp::stream* in, double samplerate, double bandwidth, bool lowPass) { + void init(dsp::stream* in, double samplerate, double bandwidth, bool lowPass, bool highPass) { _samplerate = samplerate; _bandwidth = bandwidth; _lowPass = lowPass; + _highPass = highPass; demod.init(NULL, bandwidth / 2.0, _samplerate); - lpfTaps = dsp::taps::lowPass(_bandwidth / 2.0, (_bandwidth / 2.0) * 0.1, _samplerate); - lpf.init(NULL, lpfTaps); + loadDummyTaps(); + fir.init(NULL, filterTaps); + + // Initialize taps + updateFilter(lowPass, highPass); if constexpr (std::is_same_v) { demod.out.free(); } - lpf.out.free(); + fir.out.free(); base_type::init(in); } @@ -43,9 +49,7 @@ namespace dsp::demod { base_type::tempStop(); _samplerate = samplerate; demod.setDeviation(_bandwidth / 2.0, _samplerate); - dsp::taps::free(lpfTaps); - lpfTaps = dsp::taps::lowPass(_bandwidth / 2.0, (_bandwidth / 2.0) * 0.1, _samplerate); - lpf.setTaps(lpfTaps); + updateFilter(_lowPass, _highPass); base_type::tempStart(); } @@ -54,19 +58,20 @@ namespace dsp::demod { std::lock_guard lck(base_type::ctrlMtx); if (bandwidth == _bandwidth) { return; } _bandwidth = bandwidth; - std::lock_guard lck2(lpfMtx); demod.setDeviation(_bandwidth / 2.0, _samplerate); - dsp::taps::free(lpfTaps); - lpfTaps = dsp::taps::lowPass(_bandwidth / 2, (_bandwidth / 2) * 0.1, _samplerate); - lpf.setTaps(lpfTaps); + updateFilter(_lowPass, _highPass); } void setLowPass(bool lowPass) { assert(base_type::_block_init); std::lock_guard lck(base_type::ctrlMtx); - std::lock_guard lck2(lpfMtx); - _lowPass = lowPass; - lpf.reset(); + updateFilter(lowPass, _highPass); + } + + void setHighPass(bool highPass) { + assert(base_type::_block_init); + std::lock_guard lck(base_type::ctrlMtx); + updateFilter(_lowPass, highPass); } void reset() { @@ -74,23 +79,23 @@ namespace dsp::demod { std::lock_guard lck(base_type::ctrlMtx); base_type::tempStop(); demod.reset(); - lpf.reset(); + fir.reset(); base_type::tempStart(); } inline int process(int count, dsp::complex_t* in, T* out) { if constexpr (std::is_same_v) { demod.process(count, in, out); - if (_lowPass) { - std::lock_guard lck(lpfMtx); - lpf.process(count, out, out); + if (filtering) { + std::lock_guard lck(filterMtx); + fir.process(count, out, out); } } if constexpr (std::is_same_v) { demod.process(count, in, demod.out.writeBuf); - if (_lowPass) { - std::lock_guard lck(lpfMtx); - lpf.process(count, demod.out.writeBuf, demod.out.writeBuf); + if (filtering) { + std::lock_guard lck(filterMtx); + fir.process(count, demod.out.writeBuf, demod.out.writeBuf); } convert::MonoToStereo::process(count, demod.out.writeBuf, out); } @@ -109,13 +114,50 @@ namespace dsp::demod { } private: + void updateFilter(bool lowPass, bool highPass) { + std::lock_guard lck(filterMtx); + + // Update values + _lowPass = lowPass; + _highPass = highPass; + filtering = (lowPass || highPass); + + // Free filter taps + dsp::taps::free(filterTaps); + + // Generate filter depending on low and high pass settings + if (_lowPass && _highPass) { + filterTaps = dsp::taps::bandPass(300.0, _bandwidth / 2.0, 100.0, _samplerate); + } + else if (_highPass) { + filterTaps = dsp::taps::highPass(300.0, 100.0, _samplerate); + } + else if (_lowPass) { + filterTaps = dsp::taps::lowPass(_bandwidth / 2.0, (_bandwidth / 2.0) * 0.1, _samplerate); + } + else { + loadDummyTaps(); + } + + // Set filter to use new taps + fir.setTaps(filterTaps); + fir.reset(); + } + + void loadDummyTaps() { + float dummyTap = 1.0f; + filterTaps = dsp::taps::fromArray(1, &dummyTap); + } + double _samplerate; double _bandwidth; bool _lowPass; + bool _highPass; + bool filtering; Quadrature demod; - tap lpfTaps; - filter::FIR lpf; - std::mutex lpfMtx; + tap filterTaps; + filter::FIR fir; + std::mutex filterMtx; }; } \ No newline at end of file diff --git a/decoder_modules/radio/src/demodulators/nfm.h b/decoder_modules/radio/src/demodulators/nfm.h index 29e5f9ad..c631c6b1 100644 --- a/decoder_modules/radio/src/demodulators/nfm.h +++ b/decoder_modules/radio/src/demodulators/nfm.h @@ -19,15 +19,17 @@ namespace demod { // Load config _config->acquire(); - bool modified = false; if (config->conf[name][getName()].contains("lowPass")) { _lowPass = config->conf[name][getName()]["lowPass"]; } - _config->release(modified); + if (config->conf[name][getName()].contains("highPass")) { + _highPass = config->conf[name][getName()]["highPass"]; + } + _config->release(); // Define structure - demod.init(input, getIFSampleRate(), bandwidth, _lowPass); + demod.init(input, getIFSampleRate(), bandwidth, _lowPass, _highPass); } void start() { demod.start(); } @@ -41,6 +43,12 @@ namespace demod { _config->conf[name][getName()]["lowPass"] = _lowPass; _config->release(true); } + if (ImGui::Checkbox(("High Pass##_radio_wfm_highpass_" + name).c_str(), &_highPass)) { + demod.setHighPass(_highPass); + _config->acquire(); + _config->conf[name][getName()]["highPass"] = _highPass; + _config->release(true); + } } void setBandwidth(double bandwidth) { @@ -75,6 +83,7 @@ namespace demod { ConfigManager* _config = NULL; bool _lowPass = true; + bool _highPass = false; std::string name; };