SDRAngel  4.11.5
Developer docs for <a href="https://github.com/f4exb/sdrangel">SDRangel<\a>, an Open Source Qt5 / OpenGL 3.0+ SDR and signal analyzer frontend to various hardware.
afsquelch.cpp
Go to the documentation of this file.
1 // Copyright (C) 2015 Edouard Griffiths, F4EXB. //
3 // //
4 // This program is free software; you can redistribute it and/or modify //
5 // it under the terms of the GNU General Public License as published by //
6 // the Free Software Foundation as version 3 of the License, or //
7 // (at your option) any later version. //
8 // //
9 // This program is distributed in the hope that it will be useful, //
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of //
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
12 // GNU General Public License V3 for more details. //
13 // //
14 // You should have received a copy of the GNU General Public License //
15 // along with this program. If not, see <http://www.gnu.org/licenses/>. //
17 
18 #include <math.h>
19 #include "dsp/afsquelch.h"
20 
21 #undef M_PI
22 #define M_PI 3.14159265358979323846
23 
25  m_nbAvg(128),
26  m_N(24),
27  m_sampleRate(48000),
28  m_samplesProcessed(0),
29  m_samplesAvgProcessed(0),
30  m_maxPowerIndex(0),
31  m_nTones(2),
32  m_samplesAttack(0),
33  m_attackCount(0),
34  m_samplesDecay(0),
35  m_decayCount(0),
36  m_squelchCount(0),
37  m_isOpen(false),
38  m_threshold(0.0)
39 {
40  m_k = new double[m_nTones];
41  m_coef = new double[m_nTones];
42  m_toneSet = new double[m_nTones];
43  m_u0 = new double[m_nTones];
44  m_u1 = new double[m_nTones];
45  m_power = new double[m_nTones];
47 
48  for (unsigned int j = 0; j < m_nTones; ++j)
49  {
50  m_toneSet[j] = j == 0 ? 1000.0 : 6000.0;
51  m_k[j] = ((double)m_N * m_toneSet[j]) / (double) m_sampleRate;
52  m_coef[j] = 2.0 * cos((2.0 * M_PI * m_toneSet[j])/(double) m_sampleRate);
53  m_u0[j] = 0.0;
54  m_u1[j] = 0.0;
55  m_power[j] = 0.0;
56  m_movingAverages[j].fill(0.0);
57  }
58 }
59 
60 
62 {
63  delete[] m_k;
64  delete[] m_coef;
65  delete[] m_toneSet;
66  delete[] m_u0;
67  delete[] m_u1;
68  delete[] m_power;
69 }
70 
72  unsigned int N,
73  unsigned int nbAvg,
74  unsigned int sampleRate,
75  unsigned int samplesAttack,
76  unsigned int samplesDecay,
77  const double *tones)
78 {
79  m_N = N; // save the basic parameters for use during analysis
80  m_nbAvg = nbAvg;
81  m_sampleRate = sampleRate;
82  m_samplesAttack = samplesAttack;
83  m_samplesDecay = samplesDecay;
87  m_maxPowerIndex = 0;
88  m_attackCount = 0;
89  m_decayCount = 0;
90  m_squelchCount = 0;
91  m_isOpen = false;
92  m_threshold = 0.0;
93 
94  // for each of the frequencies (tones) of interest calculate
95  // k and the associated filter coefficient as per the Goertzel
96  // algorithm. Note: we are using a real value (as opposed to
97  // an integer as described in some references. k is retained
98  // for later display. The tone set is specified in the
99  // constructor. Notice that the resulting coefficients are
100  // independent of N.
101 
102  for (unsigned int j = 0; j < m_nTones; ++j)
103  {
104  m_toneSet[j] = tones[j] < ((double) m_sampleRate) * 0.4 ? tones[j] : ((double) m_sampleRate) * 0.4; // guarantee 80% Nyquist rate
105  m_k[j] = ((double)m_N * m_toneSet[j]) / (double)m_sampleRate;
106  m_coef[j] = 2.0 * cos((2.0 * M_PI * m_toneSet[j])/(double)m_sampleRate);
107  m_u0[j] = 0.0;
108  m_u1[j] = 0.0;
109  m_power[j] = 0.0;
110  m_movingAverages[j].fill(0.0);
111  }
112 }
113 
114 
115 // Analyze an input signal
116 bool AFSquelch::analyze(double sample)
117 {
118 
119  feedback(sample); // Goertzel feedback
120 
121  if (m_samplesProcessed < m_N) // completed a block of N
122  {
124  return false;
125  }
126  else
127  {
128  feedForward(); // calculate the power at each tone
129  m_samplesProcessed = 0;
130 
132  {
134  return false;
135  }
136  else
137  {
138  return true; // have a result
139  }
140  }
141 }
142 
143 
144 void AFSquelch::feedback(double in)
145 {
146  double t;
147 
148  // feedback for each tone
149  for (unsigned int j = 0; j < m_nTones; ++j)
150  {
151  t = m_u0[j];
152  m_u0[j] = in + (m_coef[j] * m_u0[j]) - m_u1[j];
153  m_u1[j] = t;
154  }
155 }
156 
157 
159 {
160  for (unsigned int j = 0; j < m_nTones; ++j)
161  {
162  m_power[j] = (m_u0[j] * m_u0[j]) + (m_u1[j] * m_u1[j]) - (m_coef[j] * m_u0[j] * m_u1[j]);
163  m_movingAverages[j].feed(m_power[j]);
164  m_u0[j] = 0.0;
165  m_u1[j] = 0.0; // reset for next block.
166  }
167 
168  evaluate();
169 }
170 
171 
173 {
174  for (unsigned int j = 0; j < m_nTones; ++j)
175  {
176  m_u0[j] = 0.0;
177  m_u1[j] = 0.0;
178  m_power[j] = 0.0;
179  m_movingAverages[j].fill(0.0);
180  }
181 
182  m_samplesProcessed = 0;
183  m_maxPowerIndex = 0;
184  m_isOpen = false;
185 }
186 
187 
189 {
190  double maxPower = 0.0;
191  double minPower;
192  int minIndex = 0, maxIndex = 0;
193 
194  for (unsigned int j = 0; j < m_nTones; ++j)
195  {
196  if (m_movingAverages[j].sum() > maxPower)
197  {
198  maxPower = m_movingAverages[j].sum();
199  maxIndex = j;
200  }
201  }
202 
203  if (maxPower == 0.0)
204  {
205  return m_isOpen;
206  }
207 
208  minPower = maxPower;
209 
210  for (unsigned int j = 0; j < m_nTones; ++j)
211  {
212  if (m_movingAverages[j].sum() < minPower) {
213  minPower = m_movingAverages[j].sum();
214  minIndex = j;
215  }
216  }
217 
218 // m_isOpen = ((minPower/maxPower < m_threshold) && (minIndex > maxIndex));
219 
220  if ((minPower/maxPower < m_threshold) && (minIndex > maxIndex)) // open condition
221  {
223  {
224  m_squelchCount++;
225  }
226  }
227  else
228  {
230  {
231  m_squelchCount--;
232  }
233  else
234  {
235  m_squelchCount = 0;
236  }
237  }
238 
240 
241 // if ((minPower/maxPower < m_threshold) && (minIndex > maxIndex)) // open condition
242 // {
243 // if ((m_samplesAttack > 0) && (m_attackCount < m_samplesAttack))
244 // {
245 // m_isOpen = false;
246 // m_attackCount++;
247 // }
248 // else
249 // {
250 // m_isOpen = true;
251 // m_decayCount = 0;
252 // }
253 // }
254 // else
255 // {
256 // if ((m_samplesDecay > 0) && (m_decayCount < m_samplesDecay))
257 // {
258 // m_isOpen = true;
259 // m_decayCount++;
260 // }
261 // else
262 // {
263 // m_isOpen = false;
264 // m_attackCount = 0;
265 // }
266 // }
267 
268  return m_isOpen;
269 }
270 
271 void AFSquelch::setThreshold(double threshold)
272 {
273  qDebug("AFSquelch::setThreshold: threshold: %f", threshold);
274  m_threshold = threshold;
275  reset();
276 }
double * m_k
Definition: afsquelch.h:82
unsigned int m_samplesAvgProcessed
Definition: afsquelch.h:72
#define M_PI
Definition: afsquelch.cpp:22
Fixed< IntType, IntBits > cos(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2271
double * m_u0
Definition: afsquelch.h:85
void feedback(double sample)
Definition: afsquelch.cpp:144
double * m_coef
Definition: afsquelch.h:83
double * m_u1
Definition: afsquelch.h:86
bool analyze(double sample)
Definition: afsquelch.cpp:116
double * m_power
Definition: afsquelch.h:87
unsigned int m_N
Definition: afsquelch.h:69
bool evaluate()
Definition: afsquelch.cpp:188
unsigned int m_maxPowerIndex
Definition: afsquelch.h:73
unsigned int m_squelchCount
Definition: afsquelch.h:79
double m_threshold
Definition: afsquelch.h:81
std::vector< MovingAverage< double > > m_movingAverages
Definition: afsquelch.h:88
bool m_isOpen
Definition: afsquelch.h:80
unsigned int m_samplesProcessed
Definition: afsquelch.h:71
unsigned int m_samplesAttack
Definition: afsquelch.h:75
unsigned int m_samplesDecay
Definition: afsquelch.h:77
unsigned int m_nbAvg
number of power samples taken for moving average
Definition: afsquelch.h:68
unsigned int m_attackCount
Definition: afsquelch.h:76
void feedForward()
Definition: afsquelch.cpp:158
unsigned int m_nTones
Definition: afsquelch.h:74
unsigned int m_decayCount
Definition: afsquelch.h:78
double * m_toneSet
Definition: afsquelch.h:84
void setThreshold(double _threshold)
Definition: afsquelch.cpp:271
void reset()
Definition: afsquelch.cpp:172
unsigned int m_sampleRate
Definition: afsquelch.h:70
virtual ~AFSquelch()
Definition: afsquelch.cpp:61
void setCoefficients(unsigned int N, unsigned int nbAvg, unsigned int sampleRate, unsigned int samplesAttack, unsigned int samplesDecay, const double *tones)
center frequency of tones tested
Definition: afsquelch.cpp:71