From e66d9a417f210dda197eb2ec97cd1d3b96a9fbad Mon Sep 17 00:00:00 2001 From: f4exb Date: Fri, 19 Jun 2015 08:27:29 +0200 Subject: [PATCH] Changed NFM RF threshold squelch for after demod squelch --- CMakeLists.txt | 1 + include-gpl/dsp/afsquelch.h | 87 ++++++++++++ plugins/channel/nfm/nfmdemod.cpp | 107 ++++++++------- plugins/channel/nfm/nfmdemod.h | 7 +- plugins/channel/nfm/nfmdemodgui.ui | 9 +- sdrbase/dsp/afsquelch.cpp | 214 +++++++++++++++++++++++++++++ 6 files changed, 370 insertions(+), 55 deletions(-) create mode 100644 include-gpl/dsp/afsquelch.h create mode 100644 sdrbase/dsp/afsquelch.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index cfd8d09f2..4e629fa2a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,7 @@ set(sdrbase_SOURCES sdrbase/audio/audiofifo.cpp sdrbase/audio/audiooutput.cpp + sdrbase/dsp/afsquelch.cpp sdrbase/dsp/channelizer.cpp sdrbase/dsp/channelmarker.cpp sdrbase/dsp/ctcssdetector.cpp diff --git a/include-gpl/dsp/afsquelch.h b/include-gpl/dsp/afsquelch.h new file mode 100644 index 000000000..a6d588e06 --- /dev/null +++ b/include-gpl/dsp/afsquelch.h @@ -0,0 +1,87 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015 Edouard Griffiths, F4EXB. // +// // +// This program is free software; you can redistribute it and/or modify // +// it under the terms of the GNU General Public License as published by // +// the Free Software Foundation as version 3 of the License, or // +// // +// This program is distributed in the hope that it will be useful, // +// but WITHOUT ANY WARRANTY; without even the implied warranty of // +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // +// GNU General Public License V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +/////////////////////////////////////////////////////////////////////////////////// + +#ifndef INCLUDE_GPL_DSP_AFSQUELCH_H_ +#define INCLUDE_GPL_DSP_AFSQUELCH_H_ + +#include "dsp/dsptypes.h" + +/** AFSquelch: AF squelch class based on the Modified Goertzel + * algorithm. + */ +class AFSquelch { +public: + // Constructors and Destructor + AFSquelch(); + // allows user defined tone pair + AFSquelch(unsigned int nbTones, const Real *tones); + virtual ~AFSquelch(); + + // setup the basic parameters and coefficients + void setCoefficients( + int N, // the algorithm "block" size + int SampleRate, // input signal sample rate + int _samplesAttack, // number of results before squelch opens + int _samplesDecay); // number of results keeping squelch open + + // set the detection threshold + void setThreshold(double _threshold) { + threshold = _threshold; + } + + // analyze a sample set and optionally filter + // the tone frequencies. + bool analyze(Real *sample); // input signal sample + + // get the tone set + const Real *getToneSet() const + { + return toneSet; + } + + bool open() const { + return isOpen; + } + + void reset(); // reset the analysis algorithm + +protected: + void feedback(Real sample); + void feedForward(); + void evaluate(); + +private: + int N; + int sampleRate; + int samplesProcessed; + int maxPowerIndex; + int nTones; + int samplesAttack; + int attackCount; + int samplesDecay; + int decayCount; + bool isOpen; + double threshold; + double *k; + double *coef; + Real *toneSet; + double *u0; + double *u1; + double *power; +}; + + +#endif /* INCLUDE_GPL_DSP_CTCSSDETECTOR_H_ */ diff --git a/plugins/channel/nfm/nfmdemod.cpp b/plugins/channel/nfm/nfmdemod.cpp index 3bfbe9017..33d37ef39 100644 --- a/plugins/channel/nfm/nfmdemod.cpp +++ b/plugins/channel/nfm/nfmdemod.cpp @@ -26,11 +26,15 @@ #include +static const Real afSqTones[2] = {2000.0, 8000.0}; + MESSAGE_CLASS_DEFINITION(NFMDemod::MsgConfigureNFMDemod, Message) NFMDemod::NFMDemod(AudioFifo* audioFifo, SampleSink* sampleSink) : m_ctcssIndex(0), m_sampleCount(0), + m_afSquelch(2, afSqTones), + m_squelchOpen(false), m_sampleSink(sampleSink), m_audioFifo(audioFifo) { @@ -38,7 +42,7 @@ NFMDemod::NFMDemod(AudioFifo* audioFifo, SampleSink* sampleSink) : m_config.m_inputFrequencyOffset = 0; m_config.m_rfBandwidth = 12500; m_config.m_afBandwidth = 3000; - m_config.m_squelch = -40.0; + m_config.m_squelch = -30.0; m_config.m_volume = 2.0; m_config.m_audioSampleRate = 48000; @@ -52,6 +56,8 @@ NFMDemod::NFMDemod(AudioFifo* audioFifo, SampleSink* sampleSink) : m_AGC.resize(4096, m_agcLevel, 0, 0.1*m_agcLevel); m_ctcssDetector.setCoefficients(3000, 6000.0); // 0.5s / 2 Hz resolution + m_afSquelch.setCoefficients(24, 48000.0, 1, 1); // 4000 Hz span, 250us + m_afSquelch.setThreshold(0.001); } NFMDemod::~NFMDemod() @@ -109,56 +115,52 @@ void NFMDemod::feed(SampleVector::const_iterator begin, SampleVector::const_iter if(m_interpolator.interpolate(&m_interpolatorDistanceRemain, c, &ci)) { m_sampleBuffer.push_back(Sample(ci.real() * 32767.0, ci.imag() * 32767.0)); - m_movingAverage.feed(ci.real() * ci.real() + ci.imag() * ci.imag()); - if(m_movingAverage.average() >= m_squelchLevel) - m_squelchState = m_running.m_audioSampleRate/ 20; - qint16 sample; - if(m_squelchState > 0) + + m_AGC.feed(abs(ci)); + ci *= (m_agcLevel / m_AGC.getValue()); + + // demod + /* + Real argument = arg(ci); + Real demod = argument - m_lastArgument; + m_lastArgument = argument; + */ + + /* + // Original NFM + Complex d = conj(m_m1Sample) * ci; + Real demod = atan2(d.imag(), d.real()); + demod /= M_PI; + */ + + /* + Real argument1 = arg(ci);//atan2(ci.imag(), ci.real()); + Real argument2 = m_lastSample.real(); + Real demod = angleDist(argument2, argument1); + m_lastSample = Complex(argument1, 0); + */ + + // Alternative without atan - needs AGC + // http://www.embedded.com/design/configurable-systems/4212086/DSP-Tricks--Frequency-demodulation-algorithms- + Real ip = ci.real() - m_m2Sample.real(); + Real qp = ci.imag() - m_m2Sample.imag(); + Real h1 = m_m1Sample.real() * qp; + Real h2 = m_m1Sample.imag() * ip; + Real demod = (h1 - h2) * 10000; + + m_m2Sample = m_m1Sample; + m_m1Sample = ci; + m_sampleCount++; + + // AF processing + + if(m_afSquelch.analyze(&demod)) { + m_squelchOpen = m_afSquelch.open(); + } + + if (m_squelchOpen) { - m_squelchState--; - - m_AGC.feed(abs(ci)); - ci *= (m_agcLevel / m_AGC.getValue()); - - // demod - /* - Real argument = arg(ci); - Real demod = argument - m_lastArgument; - m_lastArgument = argument; - */ - - /* - // Original NFM - Complex d = conj(m_m1Sample) * ci; - Real demod = atan2(d.imag(), d.real()); - demod /= M_PI; - */ - - /* - Real argument1 = arg(ci);//atan2(ci.imag(), ci.real()); - Real argument2 = m_lastSample.real(); - Real demod = angleDist(argument2, argument1); - m_lastSample = Complex(argument1, 0); - */ - - // Alternative without atan - needs AGC - // http://www.embedded.com/design/configurable-systems/4212086/DSP-Tricks--Frequency-demodulation-algorithms- - Real ip = ci.real() - m_m2Sample.real(); - Real qp = ci.imag() - m_m2Sample.imag(); - Real h1 = m_m1Sample.real() * qp; - Real h2 = m_m1Sample.imag() * ip; - Real demod = (h1 - h2) * 10000; - - m_m2Sample = m_m1Sample; - m_m1Sample = ci; - m_sampleCount++; - - // AF processing - - //demod = m_lowpass.filter(demod); - //sample = demod * 32700; - Real ctcss_sample = m_lowpass.filter(demod); if ((m_sampleCount & 7) == 7) // decimate 48k -> 6k @@ -186,7 +188,7 @@ void NFMDemod::feed(SampleVector::const_iterator begin, SampleVector::const_iter if (m_ctcssIndexSelected && (m_ctcssIndexSelected != m_ctcssIndex)) { - sample = 0.0; + sample = 0; } else { @@ -234,7 +236,6 @@ void NFMDemod::feed(SampleVector::const_iterator begin, SampleVector::const_iter void NFMDemod::start() { - m_squelchState = 0; m_audioFifo->clear(); m_interpolatorRegulation = 0.9999; m_interpolatorDistance = 1.0; @@ -292,8 +293,10 @@ void NFMDemod::apply() } if(m_config.m_squelch != m_running.m_squelch) { - m_squelchLevel = pow(10.0, m_config.m_squelch / 20.0); + m_squelchLevel = pow(10.0, m_config.m_squelch / 10.0); m_squelchLevel *= m_squelchLevel; + m_afSquelch.setThreshold(m_squelchLevel); + m_afSquelch.reset(); } m_running.m_inputSampleRate = m_config.m_inputSampleRate; diff --git a/plugins/channel/nfm/nfmdemod.h b/plugins/channel/nfm/nfmdemod.h index d5eaf426f..71bbba7c1 100644 --- a/plugins/channel/nfm/nfmdemod.h +++ b/plugins/channel/nfm/nfmdemod.h @@ -27,6 +27,7 @@ #include "dsp/movingaverage.h" #include "dsp/agc.h" #include "dsp/ctcssdetector.h" +#include "dsp/afsquelch.h" #include "audio/audiofifo.h" #include "util/message.h" @@ -136,8 +137,10 @@ private: int m_ctcssIndexSelected; int m_sampleCount; - Real m_squelchLevel; - int m_squelchState; + double m_squelchLevel; + //int m_squelchState; + AFSquelch m_afSquelch; + bool m_squelchOpen; Real m_lastArgument; Complex m_m1Sample; diff --git a/plugins/channel/nfm/nfmdemodgui.ui b/plugins/channel/nfm/nfmdemodgui.ui index 120826f94..f5efc0792 100644 --- a/plugins/channel/nfm/nfmdemodgui.ui +++ b/plugins/channel/nfm/nfmdemodgui.ui @@ -253,10 +253,17 @@ - + + + Set CTCSS + + + + CTCSS detected + -- diff --git a/sdrbase/dsp/afsquelch.cpp b/sdrbase/dsp/afsquelch.cpp new file mode 100644 index 000000000..77f1b95c8 --- /dev/null +++ b/sdrbase/dsp/afsquelch.cpp @@ -0,0 +1,214 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015 Edouard Griffiths, F4EXB. // +// // +// This program is free software; you can redistribute it and/or modify // +// it under the terms of the GNU General Public License as published by // +// the Free Software Foundation as version 3 of the License, or // +// // +// This program is distributed in the hope that it will be useful, // +// but WITHOUT ANY WARRANTY; without even the implied warranty of // +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // +// GNU General Public License V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +/////////////////////////////////////////////////////////////////////////////////// + +#include +#include "dsp/afsquelch.h" + +AFSquelch::AFSquelch() : + N(0), + sampleRate(0), + samplesProcessed(0), + maxPowerIndex(0), + nTones(2), + samplesAttack(0), + attackCount(0), + samplesDecay(0), + decayCount(0), + isOpen(false), + threshold(0.0) +{ + k = new double[nTones]; + coef = new double[nTones]; + toneSet = new Real[nTones]; + u0 = new double[nTones]; + u1 = new double[nTones]; + power = new double[nTones]; + + toneSet[0] = 2000.0; + toneSet[1] = 10000.0; +} + +AFSquelch::AFSquelch(unsigned int nbTones, const Real *tones) : + N(0), + sampleRate(0), + samplesProcessed(0), + maxPowerIndex(0), + nTones(nbTones), + samplesAttack(0), + attackCount(0), + samplesDecay(0), + decayCount(0), + isOpen(false), + threshold(0.0) +{ + k = new double[nTones]; + coef = new double[nTones]; + toneSet = new Real[nTones]; + u0 = new double[nTones]; + u1 = new double[nTones]; + power = new double[nTones]; + + for (int j = 0; j < nTones; ++j) + { + toneSet[j] = tones[j]; + } +} + + +AFSquelch::~AFSquelch() +{ + delete[] k; + delete[] coef; + delete[] toneSet; + delete[] u0; + delete[] u1; + delete[] power; +} + + +void AFSquelch::setCoefficients(int _N, int _samplerate, int _samplesAttack, int _samplesDecay ) +{ + N = _N; // save the basic parameters for use during analysis + sampleRate = _samplerate; + samplesAttack = _samplesAttack; + samplesDecay = _samplesDecay; + + // for each of the frequencies (tones) of interest calculate + // k and the associated filter coefficient as per the Goertzel + // algorithm. Note: we are using a real value (as apposed to + // an integer as described in some references. k is retained + // for later display. The tone set is specified in the + // constructor. Notice that the resulting coefficients are + // independent of N. + for (int j = 0; j < nTones; ++j) + { + k[j] = ((double)N * toneSet[j]) / (double)sampleRate; + coef[j] = 2.0 * cos((2.0 * M_PI * toneSet[j])/(double)sampleRate); + } +} + + +// Analyze an input signal +bool AFSquelch::analyze(Real *sample) +{ + + feedback(*sample); // Goertzel feedback + samplesProcessed += 1; + + if (samplesProcessed == N) // completed a block of N + { + feedForward(); // calculate the power at each tone + samplesProcessed = 0; + return true; // have a result + } + else + { + return false; + } +} + + +void AFSquelch::feedback(Real in) +{ + double t; + + // feedback for each tone + for (int j = 0; j < nTones; ++j) + { + t = u0[j]; + u0[j] = in + (coef[j] * u0[j]) - u1[j]; + u1[j] = t; + } +} + + +void AFSquelch::feedForward() +{ + for (int j = 0; j < nTones; ++j) + { + power[j] = (u0[j] * u0[j]) + (u1[j] * u1[j]) - (coef[j] * u0[j] * u1[j]); + u0[j] = u1[j] = 0.0; // reset for next block. + } + + evaluate(); +} + + +void AFSquelch::reset() +{ + for (int j = 0; j < nTones; ++j) + { + power[j] = u0[j] = u1[j] = 0.0; // reset + } + + samplesProcessed = 0; + maxPowerIndex = 0; + isOpen = false; +} + + +void AFSquelch::evaluate() +{ + double maxPower = 0.0; + double minPower; + int minIndex = 0, maxIndex = 0; + + for (int j = 0; j < nTones; ++j) + { + if (power[j] > maxPower) { + maxPower = power[j]; + maxIndex = j; + } + } + + minPower = maxPower; + + for (int j = 0; j < nTones; ++j) + { + if (power[j] < minPower) { + minPower = power[j]; + minIndex = j; + } + } + + // principle is to open if power is uneven because noise gives even power + bool open = ((maxPower - minPower) > threshold) && (minIndex > maxIndex); + + if (open) + { + if (attackCount < samplesAttack) + { + attackCount++; + } + else + { + isOpen = true; + decayCount = 0; + } + } + else + { + if (decayCount < samplesDecay) + { + decayCount++; + } + else + { + isOpen = false; + attackCount = 0; + } + } +}