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.
audiofilter.cpp
Go to the documentation of this file.
1 // Copyright (C) 2019 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 #define _USE_MATH_DEFINES
19 #include <math.h>
20 #include <algorithm>
21 #include <QDebug>
22 
23 #include "audiofilter.h"
24 
25 // f(-3dB) = 3.6 kHz @ 48000 Hz SR (w = 0.0375):
26 const float AudioFilter::m_lpa[3] = {1.0, 1.392667E+00, -5.474446E-01};
27 const float AudioFilter::m_lpb[3] = {3.869430E-02, 7.738860E-02, 3.869430E-02};
28 // f(-3dB) = 300 Hz @ 8000 Hz SR (w = 0.0375):
29 const float AudioFilter::m_hpa[3] = {1.000000e+00, 1.667871e+00, -7.156964e-01};
30 const float AudioFilter::m_hpb[3] = {8.459039e-01, -1.691760e+00, 8.459039e-01};
31 
33  m_filterLP(m_lpa, m_lpb),
34  m_filterHP(m_hpa, m_hpb),
35  m_useHP(false)
36 {}
37 
39 {}
40 
41 
42 void AudioFilter::setDecimFilters(int srHigh, int srLow, float fcHigh, float fcLow, float fgain)
43 {
44  double fcNormHigh = fcHigh / srHigh;
45  double fcNormLow = fcLow / srLow;
46 
47  calculate2(false, fcNormHigh, m_lpva, m_lpvb, fgain);
48  calculate2(true, fcNormLow, m_hpva, m_hpvb, fgain);
49 
52 }
53 
54 void AudioFilter::calculate2(bool highPass, double fc, float *va, float *vb, float fgain)
55 {
56  double a[22], b[22];
57 
58  cheby(highPass, fc, 0.5, 2, a, b, fgain); // low-pass, 0.5% ripple, 2 pole filter
59 
60  // Copy to the 2-pole filter coefficients
61  for (int i=0; i<3; i++) {
62  vb[i] = a[i];
63  va[i] = b[i];
64  }
65 
66  va[0] = 1.0;
67 
68  qDebug() << "AudioFilter::calculate2:"
69  << " highPass: " << highPass
70  << " fc: " << fc
71  << " a0: " << va[0]
72  << " a1: " << va[1]
73  << " a2: " << va[2]
74  << " b0: " << vb[0]
75  << " b1: " << vb[1]
76  << " b2: " << vb[2];
77 }
78 
79 /*
80  * Adapted from BASIC program in table 20-4 of
81  * https://www.analog.com/media/en/technical-documentation/dsp-book/dsp_book_Ch20.pdf
82  */
83 void AudioFilter::cheby(bool highPass, double fc, float pr, int np, double *a, double *b, float fgain)
84 {
85  double a0, a1, a2, b1, b2;
86  double ta[22], tb[22];
87 
88  std::fill(a, a+22, 0.0);
89  std::fill(b, b+22, 0.0);
90  a[2] = 1.0;
91  b[2] = 1.0;
92 
93  for (int p = 1; p <= np/2; p++)
94  {
95  cheby_sub(highPass, fc, pr, np, p, a0, a1, a2, b1, b2);
96 
97  // Add coefficients to the cascade
98  for (int i=0; i<22; i++)
99  {
100  ta[i] = a[i];
101  tb[i] = b[i];
102  }
103 
104  for (int i=2; i<22; i++)
105  {
106  a[i] = a0*ta[i] + a1*ta[i-1] + a2*ta[i-2];
107  b[i] = tb[i] - b1*tb[i-1] - b2*tb[i-2];
108  }
109  }
110 
111  // Finish combining coefficients
112  b[2] = 0;
113 
114  for (int i=0; i<20; i++)
115  {
116  a[i] = a[i+2];
117  b[i] = -b[i+2];
118  }
119 
120  // Normalize the gain
121  double sa = 0.0;
122  double sb = 0.0;
123 
124  for (int i=0; i<20; i++)
125  {
126  if (highPass)
127  {
128  sa += i%2 == 0 ? a[i] : -a[i];
129  sb += i%2 == 0 ? b[i] : -b[i];
130  }
131  else
132  {
133  sa += a[i];
134  sb += b[i];
135  }
136  }
137 
138  double gain = sa/(1.0 -sb);
139  gain /= fgain;
140 
141  for (int i=0; i<20; i++) {
142  a[i] /= gain;
143  }
144 }
145 
146 /*
147  * Adapted from BASIC subroutine in table 20-5 of
148  * https://www.analog.com/media/en/technical-documentation/dsp-book/dsp_book_Ch20.pdf
149  */
150 void AudioFilter::cheby_sub(bool highPass, double fc, float pr, int np, int stage, double& a0, double& a1, double& a2, double& b1, double& b2)
151 {
152  double rp = -cos((M_PI/(np*2)) + (stage-1)*(M_PI/np));
153  double ip = sin((M_PI/(np*2)) + (stage-1)*(M_PI/np));
154 
155  // Warp from a circle to an ellipse
156  double esx = 100.0 / (100.0 - pr);
157  double es = sqrt(esx*esx -1.0);
158  double vx = (1.0/np) * log((1.0/es) + sqrt((1.0/(es*es)) + 1.0));
159  double kx = (1.0/np) * log((1.0/es) + sqrt((1.0/(es*es)) - 1.0));
160  kx = (exp(kx) + exp(-kx))/2.0;
161  rp = rp * ((exp(vx) - exp(-vx))/2.0) / kx;
162  ip = ip * ((exp(vx) + exp(-vx))/2.0) / kx;
163 
164  double t = 2.0 * tan(0.5);
165  double w = 2.0 * M_PI * fc;
166  double m = rp*rp + ip*ip;
167  double d = 4.0 - 4.0*rp*t + m*t*t;
168  double x0 = (t*t)/d;
169  double x1 = (2.0*t*t)/d;
170  double x2 = (t*t)/d;
171  double y1 = (8.0 - 2.0*m*t*t)/d;
172  double y2 = (-4.0 - 4.0*rp*t - m*t*t)/d;
173  double k;
174 
175  if (highPass) {
176  k = -cos(w/2.0 + 0.5) / cos(w/2.0 - 0.5);
177  } else {
178  k = sin(0.5 - w/2.0) / sin(0.5 + w/2.0);
179  }
180 
181  d = 1.0 + y1*k - y2*k*k;
182 
183  a0 = (x0 - x1*k + x2*k*k)/d;
184  a1 = (-2.0*x0*k + x1 + x1*k*k - 2.0*x2*k)/d;
185  a2 = (x0*k*k - x1*k + x2)/d;
186  b1 = (2.0*k + y1 + y1*k*k - 2.0*y2*k)/d;
187  b2 = (-(k*k) - y1*k + y2)/d;
188 
189  if (highPass)
190  {
191  a1 = -a1;
192  b1 = -b1;
193  }
194 }
195 
196 float AudioFilter::run(const float& sample)
197 {
198  return m_useHP ? m_filterLP.run(m_filterHP.run(sample)) : m_filterLP.run(sample);
199 }
200 
201 float AudioFilter::runHP(const float& sample)
202 {
203  return m_filterHP.run(sample);
204 }
205 
206 float AudioFilter::runLP(const float& sample)
207 {
208  return m_filterLP.run(sample);
209 }
Fixed< IntType, IntBits > cos(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2271
Fixed< IntType, IntBits > tan(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2277
bool m_useHP
Definition: audiofilter.h:64
Type run(const Type &sample)
Definition: iirfilter.h:98
static const float m_hpb[3]
Definition: audiofilter.h:72
void setCoeffs(const Type *a, const Type *b)
Definition: iirfilter.h:84
#define M_PI
Definition: rdsdemod.cpp:27
float m_lpvb[3]
Definition: audiofilter.h:66
Fixed< IntType, IntBits > exp(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2289
float m_lpva[3]
Definition: audiofilter.h:65
uint8_t b2
Definition: decimators.h:57
IIRFilter< float, 2 > m_filterLP
Definition: audiofilter.h:62
Fixed< IntType, IntBits > log(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2295
uint8_t b1
Definition: decimators.h:56
Fixed< IntType, IntBits > sin(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2265
int32_t i
Definition: decimators.h:244
IIRFilter< float, 2 > m_filterHP
Definition: audiofilter.h:63
Fixed< IntType, IntBits > sqrt(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2283
void cheby_sub(bool highPass, double fc, float pr, int np, int stage, double &a0, double &a1, double &a2, double &b1, double &b2)
float m_hpva[3]
Definition: audiofilter.h:67
void cheby(bool highPass, double fc, float pr, int np, double *a, double *b, float fgain)
Definition: audiofilter.cpp:83
static const float m_hpa[3]
Definition: audiofilter.h:71
float run(const float &sample)
void calculate2(bool highPass, double fc, float *a, float *b, float fgain)
Definition: audiofilter.cpp:54
float m_hpvb[3]
Definition: audiofilter.h:68
float runLP(const float &sample)
static const float m_lpb[3]
Definition: audiofilter.h:70
void setDecimFilters(int srHigh, int srLow, float fcHigh, float fcLow, float gain=1.0f)
Definition: audiofilter.cpp:42
static const float m_lpa[3]
Definition: audiofilter.h:69
float runHP(const float &sample)