From 786640ee1f651d532fa620b21882763af75c4421 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20Michaj=C5=82ow?= Date: Thu, 5 Nov 2020 18:55:39 +0100 Subject: [PATCH] NFMDemod: Add RF filter for freq deviation - Run CTCSS filter with reduced rate to much detection - Convert tabs to spaces to be consistent in the file - Fix AF squelch threshold setting after changing SR --- plugins/channelrx/demodnfm/nfmdemodsink.cpp | 122 +++++++++++--------- plugins/channelrx/demodnfm/nfmdemodsink.h | 77 ++++++------ sdrbase/dsp/afsquelch.cpp | 2 +- sdrbase/dsp/fftfilt.cpp | 8 ++ sdrbase/dsp/fftfilt.h | 1 + sdrbase/dsp/firfilter.h | 17 +-- 6 files changed, 124 insertions(+), 103 deletions(-) diff --git a/plugins/channelrx/demodnfm/nfmdemodsink.cpp b/plugins/channelrx/demodnfm/nfmdemodsink.cpp index 8b39e1d7a..c5fbadb97 100644 --- a/plugins/channelrx/demodnfm/nfmdemodsink.cpp +++ b/plugins/channelrx/demodnfm/nfmdemodsink.cpp @@ -35,6 +35,8 @@ const double NFMDemodSink::afSqTones[] = {1000.0, 6000.0}; // {1200.0, 8000.0}; const double NFMDemodSink::afSqTones_lowrate[] = {1000.0, 3500.0}; +const unsigned NFMDemodSink::FFT_FILTER_LENGTH = 1024; +const unsigned NFMDemodSink::CTCSS_DETECTOR_RATE = 6000; NFMDemodSink::NFMDemodSink() : m_channelSampleRate(48000), @@ -42,13 +44,15 @@ NFMDemodSink::NFMDemodSink() : m_audioSampleRate(48000), m_audioBufferFill(0), m_audioFifo(48000), + m_rfFilter(FFT_FILTER_LENGTH), m_ctcssIndex(0), m_sampleCount(0), m_squelchCount(0), m_squelchGate(4800), - m_filterTaps(48000 / 48 + 1), + m_filterTaps((48000 / 48) | 1), m_squelchLevel(-990), m_squelchOpen(false), + m_afSquelchOpen(false), m_magsq(0.0f), m_magsqSum(0.0f), m_magsqPeak(0.0f), @@ -57,55 +61,57 @@ NFMDemodSink::NFMDemodSink() : m_squelchDelayLine(24000), m_messageQueueToGUI(nullptr) { - m_agcLevel = 1.0; m_audioBuffer.resize(1<<16); - m_phaseDiscri.setFMScaling(0.5f); - applySettings(m_settings, true); + applySettings(m_settings, true); applyChannelSettings(m_channelSampleRate, m_channelFrequencyOffset, true); } -NFMDemodSink::~NFMDemodSink() -{ -} - void NFMDemodSink::feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end) { - Complex ci; + for (SampleVector::const_iterator it = begin; it != end; ++it) + { + Complex c(it->real(), it->imag()); + c *= m_nco.nextIQ(); - for (SampleVector::const_iterator it = begin; it != end; ++it) - { - Complex c(it->real(), it->imag()); - c *= m_nco.nextIQ(); - - if (m_interpolatorDistance < 1.0f) // interpolate + Complex ci; + fftfilt::cmplx *rf; + int rf_out = m_rfFilter.runFilt(c, &rf); // filter RF before demod + for (int i = 0 ; i < rf_out; i++) { - while (!m_interpolator.interpolate(&m_interpolatorDistanceRemain, c, &ci)) + if (m_interpolatorDistance == 1.0f) { - processOneSample(ci); - m_interpolatorDistanceRemain += m_interpolatorDistance; + processOneSample(rf[i]); } - } - else // decimate - { - if (m_interpolator.decimate(&m_interpolatorDistanceRemain, c, &ci)) + else if (m_interpolatorDistance < 1.0f) // interpolate { - processOneSample(ci); - m_interpolatorDistanceRemain += m_interpolatorDistance; + while (!m_interpolator.interpolate(&m_interpolatorDistanceRemain, rf[i], &ci)) + { + processOneSample(ci); + m_interpolatorDistanceRemain += m_interpolatorDistance; + } + } + else // decimate + { + if (m_interpolator.decimate(&m_interpolatorDistanceRemain, rf[i], &ci)) + { + processOneSample(ci); + m_interpolatorDistanceRemain += m_interpolatorDistance; + } } } } - if (m_audioBufferFill > 0) - { - uint res = m_audioFifo.write((const quint8*)&m_audioBuffer[0], m_audioBufferFill); + if (m_audioBufferFill > 0) + { + uint res = m_audioFifo.write((const quint8*)&m_audioBuffer[0], m_audioBufferFill); - if (res != m_audioBufferFill) { - qDebug("NFMDemodSink::feed: %u/%u tail samples written", res, m_audioBufferFill); - } + if (res != m_audioBufferFill) { + qDebug("NFMDemodSink::feed: %u/%u tail samples written", res, m_audioBufferFill); + } - m_audioBufferFill = 0; - } + m_audioBufferFill = 0; + } } void NFMDemodSink::processOneSample(Complex &ci) @@ -124,12 +130,12 @@ void NFMDemodSink::processOneSample(Complex &ci) m_magsqCount++; m_sampleCount++; - bool squelchOpen = false; + bool squelchOpen = m_afSquelchOpen && m_settings.m_deltaSquelch; if (m_settings.m_deltaSquelch) { if (m_afSquelch.analyze(demod)) { - squelchOpen = m_afSquelch.evaluate(); + m_afSquelchOpen = squelchOpen = m_afSquelch.evaluate(); if (!squelchOpen) { m_squelchDelayLine.zeroBack(m_audioSampleRate/10); // zero out evaluation period @@ -164,12 +170,15 @@ void NFMDemodSink::processOneSample(Complex &ci) { if (m_settings.m_ctcssOn) { - Real ctcssSample = m_ctcssLowpass.filter(demod); - int factor = (m_audioSampleRate / 6000) - 1; // decimate -> 6k - if ((m_sampleCount & factor) == factor && m_ctcssDetector.analyze(&ctcssSample)) + int factor = (m_audioSampleRate / CTCSS_DETECTOR_RATE) - 1; // decimate -> 6k + if ((m_sampleCount & factor) == factor) { - int maxToneIndex; - ctcssIndex = m_ctcssDetector.getDetectedTone(maxToneIndex) ? maxToneIndex + 1 : 0; + Real ctcssSample = m_ctcssLowpass.filter(demod); + if (m_ctcssDetector.analyze(&ctcssSample)) + { + int maxToneIndex; + ctcssIndex = m_ctcssDetector.getDetectedTone(maxToneIndex) ? maxToneIndex + 1 : 0; + } } } @@ -227,9 +236,13 @@ void NFMDemodSink::applyChannelSettings(int channelSampleRate, int channelFreque if ((channelSampleRate != m_channelSampleRate) || force) { - m_interpolator.create(16, channelSampleRate, m_settings.m_rfBandwidth / 2.2); - m_interpolatorDistanceRemain = 0; - m_interpolatorDistance = (Real) channelSampleRate / (Real) m_audioSampleRate; + m_interpolator.create(16, channelSampleRate, m_settings.m_fmDeviation); + m_interpolatorDistance = Real(channelSampleRate) / Real(m_audioSampleRate); + m_interpolatorDistanceRemain = m_interpolatorDistance; + + Real lowCut = -Real(m_settings.m_fmDeviation) / channelSampleRate; + Real hiCut = Real(m_settings.m_fmDeviation) / channelSampleRate; + m_rfFilter.create_filter(lowCut, hiCut); } m_channelSampleRate = channelSampleRate; @@ -256,14 +269,16 @@ void NFMDemodSink::applySettings(const NFMDemodSettings& settings, bool force) if ((settings.m_rfBandwidth != m_settings.m_rfBandwidth) || force) { - m_interpolator.create(16, m_channelSampleRate, settings.m_rfBandwidth / 2.2); - m_interpolatorDistanceRemain = 0; - m_interpolatorDistance = (Real) m_channelSampleRate / (Real) m_audioSampleRate; + m_interpolator.create(16, m_channelSampleRate, settings.m_fmDeviation); + m_interpolatorDistance = Real(m_channelSampleRate) / Real(m_audioSampleRate); + m_interpolatorDistanceRemain = m_interpolatorDistance; } - if ((settings.m_fmDeviation != m_settings.m_fmDeviation) || force) - { - m_phaseDiscri.setFMScaling((0.5f *m_audioSampleRate) / static_cast(settings.m_fmDeviation)); // integrate 4x factor + if ((settings.m_fmDeviation != m_settings.m_fmDeviation) || force) { + Real lowCut = -Real(settings.m_fmDeviation) / m_channelSampleRate; + Real hiCut = Real(settings.m_fmDeviation) / m_channelSampleRate; + m_rfFilter.create_filter(lowCut, hiCut); + m_phaseDiscri.setFMScaling(Real(m_audioSampleRate) / (2.0f * settings.m_fmDeviation)); } if ((settings.m_afBandwidth != m_settings.m_afBandwidth) || force) @@ -313,24 +328,25 @@ void NFMDemodSink::applyAudioSampleRate(unsigned int sampleRate) qDebug("NFMDemodSink::applyAudioSampleRate: %u m_channelSampleRate: %d", sampleRate, m_channelSampleRate); - m_filterTaps = sampleRate / 48 + 1; - m_ctcssLowpass.create(m_filterTaps, sampleRate, 250.0); + m_filterTaps = (sampleRate / 48) | 1; + m_ctcssLowpass.create((CTCSS_DETECTOR_RATE / 48) | 1, CTCSS_DETECTOR_RATE, 250.0); m_bandpass.create(m_filterTaps, sampleRate, 300.0, m_settings.m_afBandwidth); m_lowpass.create(m_filterTaps, sampleRate, m_settings.m_afBandwidth); m_squelchGate = (sampleRate / 100) * m_settings.m_squelchGate; // gate is given in 10s of ms at 48000 Hz audio sample rate m_squelchCount = 0; // reset squelch open counter - m_ctcssDetector.setCoefficients(sampleRate/16, sampleRate/8.0f); // 0.5s / 2 Hz resolution + m_ctcssDetector.setCoefficients(sampleRate/16, CTCSS_DETECTOR_RATE); // 0.5s / 2 Hz resolution if (sampleRate < 16000) { m_afSquelch.setCoefficients(sampleRate/2000, 600, sampleRate, 200, 0, afSqTones_lowrate); // 0.5ms test period, 300ms average span, audio SR, 100ms attack, no decay } else { m_afSquelch.setCoefficients(sampleRate/2000, 600, sampleRate, 200, 0, afSqTones); // 0.5ms test period, 300ms average span, audio SR, 100ms attack, no decay } + m_afSquelch.setThreshold(m_squelchLevel); - m_phaseDiscri.setFMScaling((0.5f * sampleRate) / static_cast(m_settings.m_fmDeviation)); + m_phaseDiscri.setFMScaling(Real(sampleRate) / (2.0f * m_settings.m_fmDeviation)); m_audioFifo.setSize(sampleRate); m_squelchDelayLine.resize(sampleRate/2); - m_interpolatorDistanceRemain = 0; m_interpolatorDistance = Real(m_channelSampleRate) / Real(sampleRate); + m_interpolatorDistanceRemain = m_interpolatorDistance; m_audioSampleRate = sampleRate; } diff --git a/plugins/channelrx/demodnfm/nfmdemodsink.h b/plugins/channelrx/demodnfm/nfmdemodsink.h index bca38c93c..3ddee07e4 100644 --- a/plugins/channelrx/demodnfm/nfmdemodsink.h +++ b/plugins/channelrx/demodnfm/nfmdemodsink.h @@ -24,6 +24,7 @@ #include "dsp/phasediscri.h" #include "dsp/nco.h" #include "dsp/interpolator.h" +#include "dsp/fftfilt.h" #include "dsp/firfilter.h" #include "dsp/afsquelch.h" #include "dsp/agc.h" @@ -37,20 +38,19 @@ class NFMDemodSink : public ChannelSampleSink { public: NFMDemodSink(); - ~NFMDemodSink(); - virtual void feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end); + virtual void feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end); - const Real *getCtcssToneSet(int& nbTones) const { - nbTones = m_ctcssDetector.getNTones(); - return m_ctcssDetector.getToneSet(); - } + const Real *getCtcssToneSet(int& nbTones) const { + nbTones = m_ctcssDetector.getNTones(); + return m_ctcssDetector.getToneSet(); + } - void setSelectedCtcssIndex(int selectedCtcssIndex) { - m_ctcssIndexSelected = selectedCtcssIndex; - } + void setSelectedCtcssIndex(int selectedCtcssIndex) { + m_ctcssIndexSelected = selectedCtcssIndex; + } - bool getSquelchOpen() const { return m_squelchOpen; } + bool getSquelchOpen() const { return m_squelchOpen; } void getMagSqLevels(double& avg, double& peak, int& nbSamples) { @@ -89,53 +89,56 @@ private: double m_magsqPeak; }; - enum RateState { - RSInitialFill, - RSRunning - }; + enum RateState { + RSInitialFill, + RSRunning + }; int m_channelSampleRate; int m_channelFrequencyOffset; - NFMDemodSettings m_settings; + NFMDemodSettings m_settings; int m_audioSampleRate; AudioVector m_audioBuffer; uint m_audioBufferFill; AudioFifo m_audioFifo; - NCO m_nco; - Interpolator m_interpolator; - Real m_interpolatorDistance; - Real m_interpolatorDistanceRemain; - Lowpass m_ctcssLowpass; - Bandpass m_bandpass; + NCO m_nco; + Interpolator m_interpolator; + fftfilt m_rfFilter; + Real m_interpolatorDistance; + Real m_interpolatorDistanceRemain; + Lowpass m_ctcssLowpass; + Bandpass m_bandpass; Lowpass m_lowpass; - CTCSSDetector m_ctcssDetector; - int m_ctcssIndex; // 0 for nothing detected - int m_ctcssIndexSelected; - int m_sampleCount; - int m_squelchCount; - int m_squelchGate; - int m_filterTaps; + CTCSSDetector m_ctcssDetector; + int m_ctcssIndex; // 0 for nothing detected + int m_ctcssIndexSelected; + int m_sampleCount; + int m_squelchCount; + int m_squelchGate; + int m_filterTaps; - Real m_squelchLevel; - bool m_squelchOpen; - double m_magsq; //!< displayed averaged value - double m_magsqSum; - double m_magsqPeak; + Real m_squelchLevel; + bool m_squelchOpen; + bool m_afSquelchOpen; + double m_magsq; //!< displayed averaged value + double m_magsqSum; + double m_magsqPeak; int m_magsqCount; MagSqLevelsStore m_magSqLevelStore; - MovingAverageUtil m_movingAverage; - AFSquelch m_afSquelch; - Real m_agcLevel; // AGC will aim to this level - DoubleBufferFIFO m_squelchDelayLine; + MovingAverageUtil m_movingAverage; + AFSquelch m_afSquelch; + DoubleBufferFIFO m_squelchDelayLine; PhaseDiscriminators m_phaseDiscri; MessageQueue *m_messageQueueToGUI; static const double afSqTones[]; static const double afSqTones_lowrate[]; + static const unsigned FFT_FILTER_LENGTH; + static const unsigned CTCSS_DETECTOR_RATE; void processOneSample(Complex &ci); MessageQueue *getMessageQueueToGUI() { return m_messageQueueToGUI; } diff --git a/sdrbase/dsp/afsquelch.cpp b/sdrbase/dsp/afsquelch.cpp index 1ebaa75a6..4a558a66f 100644 --- a/sdrbase/dsp/afsquelch.cpp +++ b/sdrbase/dsp/afsquelch.cpp @@ -214,9 +214,9 @@ bool AFSquelch::evaluate() } // m_isOpen = ((minPower/maxPower < m_threshold) && (minIndex > maxIndex)); - if ((minPower/maxPower < m_threshold) && (minIndex > maxIndex)) // open condition { + if (m_squelchCount < m_samplesAttack + m_samplesDecay) { m_squelchCount++; diff --git a/sdrbase/dsp/fftfilt.cpp b/sdrbase/dsp/fftfilt.cpp index e05029b3c..5953ecaf0 100644 --- a/sdrbase/dsp/fftfilt.cpp +++ b/sdrbase/dsp/fftfilt.cpp @@ -75,6 +75,14 @@ void fftfilt::init_filter() // f1 == 0 ==> low pass filter // f2 == 0 ==> high pass filter //------------------------------------------------------------------------------ +fftfilt::fftfilt(int len) +{ + flen = len; + pass = 0; + window = 0; + init_filter(); +} + fftfilt::fftfilt(float f1, float f2, int len) { flen = len; diff --git a/sdrbase/dsp/fftfilt.h b/sdrbase/dsp/fftfilt.h index fa02d5432..55bae9c33 100644 --- a/sdrbase/dsp/fftfilt.h +++ b/sdrbase/dsp/fftfilt.h @@ -19,6 +19,7 @@ enum {NONE, BLACKMAN, HAMMING, HANNING}; public: typedef std::complex cmplx; + fftfilt(int len); fftfilt(float f1, float f2, int len); fftfilt(float f2, int len); ~fftfilt(); diff --git a/sdrbase/dsp/firfilter.h b/sdrbase/dsp/firfilter.h index 282e52505..2df3fe7f6 100644 --- a/sdrbase/dsp/firfilter.h +++ b/sdrbase/dsp/firfilter.h @@ -42,24 +42,17 @@ public: m_samples[m_ptr] = sample; - for (size_t i = 0; i < n_taps; ++i) + for (int i = 0; i < n_taps; ++i) { - acc += (m_samples[a++] + m_samples[b--]) * m_taps[i]; + acc += (m_samples[a] + m_samples[b]) * m_taps[i]; - if (a == n_samples) { - a = 0; - } - - if (b == -1) { - b = n_samples - 1; - } + a = (a == n_samples - 1) ? 0 : a + 1; + b = (b == 0) ? n_samples - 1 : b - 1; } acc += m_samples[a] * m_taps[n_taps]; - if (++m_ptr == n_samples) { - m_ptr = 0; - } + m_ptr = (m_ptr == n_samples - 1) ? 0 : m_ptr + 1; return acc; }