Biquad filter library in C++ template form. On mbed LPC1768, can run about 100K operations per second with double precision float, or about 200K with single precision float.
biquad.cpp
00001 /* 00002 * biquad.cpp 00003 * 00004 * Original source material from Robert Bristow Johnson at 00005 * http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt 00006 * and Nigel Redmon at http://www.earlevel.com/ 00007 * 00008 * Derived from public domain C implementation by Tom St. Denis 00009 * 00010 * Thanks gents! 00011 * 00012 * C++-ification by Mark J. Howell 00013 * 00014 */ 00015 00016 #include "mbed.h" 00017 00018 #include <stdlib.h> 00019 00020 #include "biquad.h" 00021 00022 #define SPEW 00023 00024 template <typename T> 00025 Biquad<T>::Biquad(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T Q, BIQUAD_FORMULATION_TYPE form) { 00026 00027 #ifdef SPEW 00028 printf("Biquad::Biquad(%d, %f, %f, %f, %f)\n", type, dbGain, freq, srate, Q); 00029 #endif 00030 00031 Reset(type, dbGain, freq, srate, Q, true, form); 00032 00033 } 00034 00035 template <typename T> 00036 void Biquad<T>::Reset(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T Q, bool clearHistory, BIQUAD_FORMULATION_TYPE _form) { 00037 00038 form = _form; 00039 00040 #ifdef SPEW 00041 printf("Biquad::Reset(%d, %f, %f, %f, %f, %s, %d)\n", type, dbGain, freq, srate, Q, (clearHistory ? "true" : "false"), form); 00042 #endif 00043 00044 #if 0 00045 switch (form) { 00046 case DIRECT_FORM_I: 00047 CalcCoefficientsDirectI(type, dbGain, freq, srate, Q); 00048 break; 00049 00050 case TRANSPOSED_DIRECT_FORM_II: 00051 CalcCoefficientsTransposedDirectII(type, dbGain, freq, srate,Q); 00052 break; 00053 00054 default: 00055 break; 00056 } 00057 #else 00058 CalcCoefficientsDirectI(type, dbGain, freq, srate, Q); 00059 CalcCoefficientsTransposedDirectII(type, dbGain, freq, srate, Q); 00060 #endif 00061 00062 if (clearHistory) { 00063 00064 /* zero initial samples */ 00065 x1 = x2 = 0; 00066 y1 = y2 = 0; 00067 z1 = z2 = 0; 00068 00069 } 00070 00071 } 00072 00073 template <typename T> 00074 void Biquad<T>::CalcCoefficientsDirectI(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T Q) { 00075 00076 T A, omega, sn, cs, alpha, beta; 00077 T a0 = 0, a1 = 0, a2 = 0, b0 = 0, b1 = 0, b2 = 0; 00078 00079 /* setup variables */ 00080 A = pow(10, dbGain / 40); 00081 omega = 2 * M_PI * freq / srate; 00082 sn = sin(omega); 00083 cs = cos(omega); 00084 //alpha = sn * sinh(M_LN2 / 2 * bandwidth * omega / sn); // for BW 00085 //alpha = sn / 2 * sqrt( ( A + 1 / A) * (1 / S - 1) + 2) // for S 00086 alpha = sn / (2 * Q); // for Q 00087 beta = sqrt(A + A); 00088 00089 switch (type) { 00090 case LPF: 00091 b0 = (1 - cs) / 2; 00092 b1 = 1 - cs; 00093 b2 = (1 - cs) / 2; 00094 a0 = 1 + alpha; 00095 a1 = -2 * cs; 00096 a2 = 1 - alpha; 00097 break; 00098 00099 case HPF: 00100 b0 = (1 + cs) / 2; 00101 b1 = -(1 + cs); 00102 b2 = (1 + cs) / 2; 00103 a0 = 1 + alpha; 00104 a1 = -2 * cs; 00105 a2 = 1 - alpha; 00106 break; 00107 00108 case BPF: 00109 b0 = alpha; 00110 b1 = 0; 00111 b2 = -alpha; 00112 a0 = 1 + alpha; 00113 a1 = -2 * cs; 00114 a2 = 1 - alpha; 00115 break; 00116 00117 case NOTCH: 00118 b0 = 1; 00119 b1 = -2 * cs; 00120 b2 = 1; 00121 a0 = 1 + alpha; 00122 a1 = -2 * cs; 00123 a2 = 1 - alpha; 00124 break; 00125 00126 case PEQ: 00127 b0 = 1 + (alpha * A); 00128 b1 = -2 * cs; 00129 b2 = 1 - (alpha * A); 00130 a0 = 1 + (alpha / A); 00131 a1 = -2 * cs; 00132 a2 = 1 - (alpha / A); 00133 break; 00134 00135 case LSH: 00136 b0 = A * ((A + 1) - (A - 1) * cs + beta * sn); 00137 b1 = 2 * A * ((A - 1) - (A + 1) * cs); 00138 b2 = A * ((A + 1) - (A - 1) * cs - beta * sn); 00139 a0 = (A + 1) + (A - 1) * cs + beta * sn; 00140 a1 = -2 * ((A - 1) + (A + 1) * cs); 00141 a2 = (A + 1) + (A - 1) * cs - beta * sn; 00142 break; 00143 00144 case HSH: 00145 b0 = A * ((A + 1) + (A - 1) * cs + beta * sn); 00146 b1 = -2 * A * ((A - 1) + (A + 1) * cs); 00147 b2 = A * ((A + 1) + (A - 1) * cs - beta * sn); 00148 a0 = (A + 1) - (A - 1) * cs + beta * sn; 00149 a1 = 2 * ((A - 1) - (A + 1) * cs); 00150 a2 = (A + 1) - (A - 1) * cs - beta * sn; 00151 break; 00152 00153 default: 00154 break; 00155 } 00156 00157 #ifdef SPEW 00158 printf("calculated coefficients a0 %f, a1 %f, a2 %f, b0 %f, b1 %f, b2 %f\n", a0, a1, a2, b0, b1, b2); 00159 #endif 00160 00161 /* precompute the coefficients */ 00162 // DIRECT_FORM_I 00163 this->a0_I = b0 /a0; 00164 this->a1_I = b1 /a0; 00165 this->a2_I = b2 /a0; 00166 this->a3_I = a1 /a0; 00167 this->a4_I = a2 /a0; 00168 00169 } 00170 00171 template <typename T> 00172 void Biquad<T>::CalcCoefficientsTransposedDirectII(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T Q) { 00173 00174 T a0 = 0, a1 = 0, a2 = 0, b1 = 0, b2 = 0, norm = 0, V, K; 00175 00176 V = pow(10, fabs(dbGain) / 20); 00177 K = tan(M_PI * freq / srate); 00178 00179 switch (type) { 00180 case LPF: 00181 norm = 1 / (1 + K / Q + K * K); 00182 a0 = K * K * norm; 00183 a1 = 2 * a0; 00184 a2 = a0; 00185 b1 = 2 * (K * K - 1) * norm; 00186 b2 = (1 - K / Q + K * K) * norm; 00187 break; 00188 00189 case HPF: 00190 norm = 1 / (1 + K / Q + K * K); 00191 a0 = 1 * norm; 00192 a1 = -2 * a0; 00193 a2 = a0; 00194 b1 = 2 * (K * K - 1) * norm; 00195 b2 = (1 - K / Q + K * K) * norm; 00196 break; 00197 00198 case BPF: 00199 norm = 1 / (1 + K / Q + K * K); 00200 a0 = K / Q * norm; 00201 a1 = 0; 00202 a2 = -a0; 00203 b1 = 2 * (K * K - 1) * norm; 00204 b2 = (1 - K / Q + K * K) * norm; 00205 break; 00206 00207 case NOTCH: 00208 norm = 1 / (1 + K / Q + K * K); 00209 a0 = (1 + K * K) * norm; 00210 a1 = 2 * (K * K - 1) * norm; 00211 a2 = a0; 00212 b1 = a1; 00213 b2 = (1 - K / Q + K * K) * norm; 00214 break; 00215 00216 case PEQ: 00217 if (dbGain >= 0) { // boost 00218 norm = 1 / (1 + 1 / Q * K + K * K); 00219 a0 = (1 + V / Q * K + K * K) * norm; 00220 a1 = 2 * (K * K - 1) * norm; 00221 a2 = (1 - V / Q * K + K * K) * norm; 00222 b1 = a1; 00223 b2 = (1 - 1 / Q * K + K * K) * norm; 00224 } else { // cut 00225 norm = 1 / (1 + V / Q * K + K * K); 00226 a0 = (1 + 1 / Q * K + K * K) * norm; 00227 a1 = 2 * (K * K - 1) * norm; 00228 a2 = (1 - 1 / Q * K + K * K) * norm; 00229 b1 = a1; 00230 b2 = (1 - V / Q * K + K * K) * norm; 00231 } 00232 break; 00233 00234 case LSH: 00235 // TBI 00236 break; 00237 00238 case HSH: 00239 // TBI 00240 break; 00241 00242 default: 00243 break; 00244 } 00245 00246 #ifdef SPEW 00247 printf("calculated coefficients a0 %f, a1 %f, a2 %f, b1 %f, b2 %f\n", a0, a1, a2, b1, b2); 00248 #endif 00249 00250 // TRANSPOSED_DIRECT_FORM_II 00251 this->a0_II = a0; 00252 this->a1_II = a1; 00253 this->a2_II = a2; 00254 this->b1_II = b1; 00255 this->b2_II = b2; 00256 00257 } 00258 00259 template <typename T> 00260 /* Computes a BiQuad filter on a sample */ 00261 T Biquad<T>::Calculate(T sample) { 00262 00263 T result = 0; 00264 00265 switch (form) { 00266 case DIRECT_FORM_I: 00267 /* compute result */ 00268 result = a0_I * sample + a1_I * x1 + a2_I * x2 - 00269 a3_I * y1 - a4_I * y2; 00270 00271 /* shift x1 to x2, sample to x1 */ 00272 x2 = x1; 00273 x1 = sample; 00274 00275 /* shift y1 to y2, result to y1 */ 00276 y2 = y1; 00277 y1 = result; 00278 break; 00279 00280 case TRANSPOSED_DIRECT_FORM_II: 00281 #if 1 00282 result = sample * a0_II + z1; 00283 z1 = sample * a1_II + z2 - b1_II * result; 00284 z2 = sample * a2_II - b2_II * result; 00285 #else 00286 result = a0_II * sample + a1_II * x1 + a2_II * x2 - b1_II * y1 - b2_II * y2; 00287 x2 = x1; 00288 x1 = sample; 00289 y2 = y1; 00290 y1 = result; 00291 #endif 00292 break; 00293 00294 default: 00295 break; 00296 00297 } 00298 00299 return result; 00300 } 00301 00302 template class Biquad<long double>; 00303 template class Biquad<double>; 00304 template class Biquad<float>;
Generated on Tue Jul 19 2022 00:25:10 by
1.7.2