diff --git a/sdrbase/audio/audiofilter.cpp b/sdrbase/audio/audiofilter.cpp index 696a94299..5cf963c96 100644 --- a/sdrbase/audio/audiofilter.cpp +++ b/sdrbase/audio/audiofilter.cpp @@ -14,11 +14,16 @@ // along with this program. If not, see . // /////////////////////////////////////////////////////////////////////////////////// +#include +#include +#include + #include "audiofilter.h" +// f(-3dB) = 3.6 kHz @ 48000 Hz SR (w = 0.0375): const float AudioFilter::m_lpa[3] = {1.0, 1.392667E+00, -5.474446E-01}; const float AudioFilter::m_lpb[3] = {3.869430E-02, 7.738860E-02, 3.869430E-02}; -// f(-3dB) = 300 Hz @ 8000 Hz SR (w = 0.075): +// f(-3dB) = 300 Hz @ 8000 Hz SR (w = 0.0375): const float AudioFilter::m_hpa[3] = {1.000000e+00, 1.667871e+00, -7.156964e-01}; const float AudioFilter::m_hpb[3] = {8.459039e-01, -1.691760e+00, 8.459039e-01}; @@ -26,12 +31,166 @@ AudioFilter::AudioFilter() : m_filterLP(m_lpa, m_lpb), m_filterHP(m_hpa, m_hpb), m_useHP(false) -{ -} +{} AudioFilter::~AudioFilter() {} + +void AudioFilter::setDecimFilters(int sr, uint32_t decim) +{ + int downSR = sr / (decim == 0 ? 1 : decim); + double fcH = (0.45 * downSR) / (sr <= 0 ? 1 : sr); // high cut frequency normalized to SR + double fcL = 300.0 / downSR; // low cut frequency normalized to downsampled SR + + calculate2(false, fcH, m_lpva, m_lpvb); + calculate2(true, fcL, m_hpva, m_hpvb); + + m_filterLP.setCoeffs(m_lpva, m_lpvb); + m_filterHP.setCoeffs(m_hpva, m_hpvb); +} + +void AudioFilter::calculate2(bool highPass, double fc, float *va, float *vb) +{ + double a[22], b[22]; + + cheby(highPass, fc, 0.5, 2, a, b); // low-pass, 0.5% ripple, 2 pole filter + + // Copy to the 2-pole filter coefficients + for (int i=0; i<3; i++) { + vb[i] = a[i]; + va[i] = b[i]; + } + + va[0] = 1.0; + + qDebug() << "AudioFilter::calculate2:" + << " highPass: " << highPass + << " fc: " << fc + << " a0: " << va[0] + << " a1: " << va[1] + << " a2: " << va[2] + << " b0: " << vb[0] + << " b1: " << vb[1] + << " b2: " << vb[2]; +} + +/* + * Adapted from BASIC program in table 20-4 of + * https://www.analog.com/media/en/technical-documentation/dsp-book/dsp_book_Ch20.pdf + */ +void AudioFilter::cheby(bool highPass, double fc, float pr, int np, double *a, double *b) +{ + double a0, a1, a2, b1, b2; + double ta[22], tb[22]; + + std::fill(a, a+22, 0.0); + std::fill(b, b+22, 0.0); + a[2] = 1.0; + b[2] = 1.0; + + for (int p = 1; p <= np/2; p++) + { + cheby_sub(highPass, fc, pr, np, p, a0, a1, a2, b1, b2); + + // Add coefficients to the cascade + for (int i=0; i<22; i++) + { + ta[i] = a[i]; + tb[i] = b[i]; + } + + for (int i=2; i<22; i++) + { + a[i] = a0*ta[i] + a1*ta[i-1] + a2*ta[i-2]; + b[i] = tb[i] - b1*tb[i-1] - b2*tb[i-2]; + } + } + + // Finish combining coefficients + b[2] = 0; + + for (int i=0; i<20; i++) + { + a[i] = a[i+2]; + b[i] = -b[i+2]; + } + + // Normalize the gain + double sa = 0.0; + double sb = 0.0; + + for (int i=0; i<20; i++) + { + if (highPass) + { + sa += i%2 == 0 ? a[i] : -a[i]; + sb += i%2 == 0 ? b[i] : -b[i]; + } + else + { + sa += a[i]; + sb += b[i]; + } + } + + double gain = sa/(1.0 -sb); + + for (int i=0; i<20; i++) { + a[i] /= gain; + } +} + +/* + * Adapted from BASIC subroutine in table 20-5 of + * https://www.analog.com/media/en/technical-documentation/dsp-book/dsp_book_Ch20.pdf + */ +void AudioFilter::cheby_sub(bool highPass, double fc, float pr, int np, int stage, double& a0, double& a1, double& a2, double& b1, double& b2) +{ + double rp = -cos((M_PI/(np*2)) + (stage-1)*(M_PI/np)); + double ip = sin((M_PI/(np*2)) + (stage-1)*(M_PI/np)); + + // Warp from a circle to an ellipse + double esx = 100.0 / (100.0 - pr); + double es = sqrt(esx*esx -1.0); + double vx = (1.0/np) * log((1.0/es) + sqrt((1.0/(es*es)) + 1.0)); + double kx = (1.0/np) * log((1.0/es) + sqrt((1.0/(es*es)) - 1.0)); + kx = (exp(kx) + exp(-kx))/2.0; + rp = rp * ((exp(vx) - exp(-vx))/2.0) / kx; + ip = ip * ((exp(vx) + exp(-vx))/2.0) / kx; + + double t = 2.0 * tan(0.5); + double w = 2.0 * M_PI * fc; + double m = rp*rp + ip*ip; + double d = 4.0 - 4.0*rp*t + m*t*t; + double x0 = (t*t)/d; + double x1 = (2.0*t*t)/d; + double x2 = (t*t)/d; + double y1 = (8.0 - 2.0*m*t*t)/d; + double y2 = (-4.0 - 4.0*rp*t - m*t*t)/d; + double k; + + if (highPass) { + k = -cos(w/2.0 + 0.5) / cos(w/2.0 - 0.5); + } else { + k = sin(0.5 - w/2.0) / sin(0.5 + w/2.0); + } + + d = 1.0 + y1*k - y2*k*k; + + a0 = (x0 - x1*k + x2*k*k)/d; + a1 = (-2.0*x0*k + x1 + x1*k*k - 2.0*x2*k)/d; + a2 = (x0*k*k - x1*k + x2)/d; + b1 = (2.0*k + y1 + y1*k*k - 2.0*y2*k)/d; + b2 = (-(k*k) - y1*k + y2)/d; + + if (highPass) + { + a1 = -a1; + b1 = -b1; + } +} + float AudioFilter::run(const float& sample) { return m_useHP ? m_filterLP.run(m_filterHP.run(sample)) : m_filterLP.run(sample); @@ -45,4 +204,4 @@ float AudioFilter::runHP(const float& sample) float AudioFilter::runLP(const float& sample) { return m_filterLP.run(sample); -} \ No newline at end of file +} diff --git a/sdrbase/audio/audiofilter.h b/sdrbase/audio/audiofilter.h index 4a003fc55..de2dbddf6 100644 --- a/sdrbase/audio/audiofilter.h +++ b/sdrbase/audio/audiofilter.h @@ -21,7 +21,7 @@ #include "dsp/iirfilter.h" /** - * This is a 2 pole lowpass Chebyshev (recursive) filter at fc=0.075 using coefficients found in table 20-1 of + * By default this is a 2 pole lowpass Chebyshev (recursive) filter at fc=0.075 using coefficients found in table 20-1 of * http://www.analog.com/media/en/technical-documentation/dsp-book/dsp_book_Ch20.pdf * * At the interpolated sampling frequency of 48 kHz the -3 dB corner is at 48 * .075 = 3.6 kHz which is perfect for voice @@ -36,6 +36,8 @@ * * This one works directly with floats * + * It can be generalized using the program found in tables 20-4 and 20-5 of the same document. This form is used as a + * decimation filter and can be set with the setDecimFilters method */ class SDRBASE_API AudioFilter { @@ -45,18 +47,29 @@ public: void useHP(bool useHP) { m_useHP = useHP; } bool usesHP() const { return m_useHP; } + void setDecimFilters(int sr, uint32_t decim); float run(const float& sample); float runHP(const float& sample); float runLP(const float& sample); private: + void calculate2(bool highPass, double fc, float *a, float *b); // two pole Chebyshev calculation + void cheby(bool highPass, double fc, float pr, int np, double *a, double *b); + void cheby_sub(bool highPass, double fc, float pr, int np, int stage, + double& a0, double& a1, double& a2, double& b1, double& b2); + IIRFilter m_filterLP; IIRFilter m_filterHP; bool m_useHP; + float m_lpva[3]; + float m_lpvb[3]; + float m_hpva[3]; + float m_hpvb[3]; static const float m_lpa[3]; static const float m_lpb[3]; static const float m_hpa[3]; static const float m_hpb[3]; + }; -#endif // _SDRBASE_AUDIO_AUDIOFILTER_H_ \ No newline at end of file +#endif // _SDRBASE_AUDIO_AUDIOFILTER_H_