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.

Dependents:   EMG_Wiesje

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers biquad.cpp Source File

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>;