added high-pass filter in NFM mode

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<dsp::complex_t>* in, double samplerate, double bandwidth, bool lowPass) {
+        void init(dsp::stream<dsp::complex_t>* 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);
+            loadDummyTaps();
-            lpf.init(NULL, lpfTaps);
+            fir.init(NULL, filterTaps);
+
+            // Initialize taps
+            updateFilter(lowPass, highPass);
 
             if constexpr (std::is_same_v<T, float>) {
                 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);
+            updateFilter(_lowPass, _highPass);
-            lpfTaps = dsp::taps::lowPass(_bandwidth / 2.0, (_bandwidth / 2.0) * 0.1, _samplerate);
-            lpf.setTaps(lpfTaps);
             base_type::tempStart();
         }
 
@@ -54,19 +58,20 @@ namespace dsp::demod {
             std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
             if (bandwidth == _bandwidth) { return; }
             _bandwidth = bandwidth;
-            std::lock_guard<std::mutex> lck2(lpfMtx);
             demod.setDeviation(_bandwidth / 2.0, _samplerate);
-            dsp::taps::free(lpfTaps);
+            updateFilter(_lowPass, _highPass);
-            lpfTaps = dsp::taps::lowPass(_bandwidth / 2, (_bandwidth / 2) * 0.1, _samplerate);
-            lpf.setTaps(lpfTaps);
         }
 
         void setLowPass(bool lowPass) {
             assert(base_type::_block_init);
             std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
-            std::lock_guard<std::mutex> lck2(lpfMtx);
+            updateFilter(lowPass, _highPass);
-            _lowPass = lowPass;
+        }
-            lpf.reset();
+
+        void setHighPass(bool highPass) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            updateFilter(_lowPass, highPass);
         }
 
         void reset() {
@@ -74,23 +79,23 @@ namespace dsp::demod {
             std::lock_guard<std::recursive_mutex> 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<T, float>) {
                 demod.process(count, in, out);
-                if (_lowPass) {
+                if (filtering) {
-                    std::lock_guard<std::mutex> lck(lpfMtx);
+                    std::lock_guard<std::mutex> lck(filterMtx);
-                    lpf.process(count, out, out);
+                    fir.process(count, out, out);
                 }
             }
             if constexpr (std::is_same_v<T, stereo_t>) {
                 demod.process(count, in, demod.out.writeBuf);
-                if (_lowPass) {
+                if (filtering) {
-                    std::lock_guard<std::mutex> lck(lpfMtx);
+                    std::lock_guard<std::mutex> lck(filterMtx);
-                    lpf.process(count, demod.out.writeBuf, demod.out.writeBuf);
+                    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<std::mutex> 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<float>(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<float>(1, &dummyTap);
+        }
+
         double _samplerate;
         double _bandwidth;
         bool _lowPass;
+        bool _highPass;
+        bool filtering;
 
         Quadrature demod;
-        tap<float> lpfTaps;
+        tap<float> filterTaps;
-        filter::FIR<float, float> lpf;
+        filter::FIR<float, float> fir;
-        std::mutex lpfMtx;
+        std::mutex filterMtx;
     };
 }

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;
     };