Revision 3:abd20f34fd68, committed 2012-02-16

Comitter:

mhowellaz

Date:

Thu Feb 16 00:07:57 2012 +0000

Parent:

2:9a9b643fcc66

Commit message:

Changed in this revision

--- a/biquad.cpp	Wed Feb 15 18:12:50 2012 +0000
+++ b/biquad.cpp	Thu Feb 16 00:07:57 2012 +0000
@@ -1,9 +1,10 @@
 /*
  *  biquad.cpp
  *
- *  Original source material from Robert Bristow Johnson at 
+ *  Original source material from Robert Bristow Johnson at
  *  http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
- *  
+ *  and Nigel Redmon at http://www.earlevel.com/
+ *
  *  Derived from public domain C implementation by Tom St. Denis
  *
  *  Thanks gents!
@@ -11,63 +12,99 @@
  *  C++-ification by Mark J. Howell
  *
  */
- 
-#include "mbed.h" 
- 
+
+#include "mbed.h"
+
 #include <stdlib.h>
 
 #include "biquad.h"
 
-//#define SPEW
+#define SPEW
 
 template <typename T>
-Biquad<T>::Biquad(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth) {
+Biquad<T>::Biquad(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T Q, BIQUAD_FORMULATION_TYPE form) {
 
 #ifdef SPEW
-    printf("Biquad::Biquad(%d, %f, %f, %f, %f\n", type, dbGain, freq, srate, bandwidth);
+    printf("Biquad::Biquad(%d, %f, %f, %f, %f)\n", type, dbGain, freq, srate, Q);
 #endif
 
-    Reset(type, dbGain, freq, srate, bandwidth, true);
+    Reset(type, dbGain, freq, srate, Q, true, form);
 
 }
 
 template <typename T>
-void Biquad<T>::Reset(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth, bool clearHistory) {
-    
+void Biquad<T>::Reset(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T Q, bool clearHistory, BIQUAD_FORMULATION_TYPE _form) {
+
+    form = _form;
+
+#ifdef SPEW
+    printf("Biquad::Reset(%d, %f, %f, %f, %f, %s, %d)\n", type, dbGain, freq, srate, Q, (clearHistory ? "true" : "false"), form);
+#endif
+
+#if 0
+    switch (form) {
+        case DIRECT_FORM_I:
+            CalcCoefficientsDirectI(type, dbGain, freq, srate, Q);
+            break;
+            
+        case TRANSPOSED_DIRECT_FORM_II:
+            CalcCoefficientsTransposedDirectII(type, dbGain, freq, srate,Q);
+            break;
+            
+        default:
+            break;
+    }
+#else
+    CalcCoefficientsDirectI(type, dbGain, freq, srate, Q);
+    CalcCoefficientsTransposedDirectII(type, dbGain, freq, srate, Q);    
+#endif
+
+    if (clearHistory) {
+
+        /* zero initial samples */
+        x1 = x2 = 0;
+        y1 = y2 = 0;
+        z1 = z2 = 0;
+
+    }
+
+}
+
+template <typename T>
+void Biquad<T>::CalcCoefficientsDirectI(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T Q) {
+
     T A, omega, sn, cs, alpha, beta;
     T a0 = 0, a1 = 0, a2 = 0, b0 = 0, b1 = 0, b2 = 0;
 
-#ifdef SPEW
-    printf("Biquad::Reset(%d, %f, %f, %f, %f, %s\n", type, dbGain, freq, srate, bandwidth, (clearHistory ? "true" : "false"));
-#endif
-
     /* setup variables */
-    A = pow(10, dbGain /40);
-    omega = 2 * M_PI * freq /srate;
+    A = pow(10, dbGain / 40); 
+    omega = 2 * M_PI * freq / srate;
     sn = sin(omega);
     cs = cos(omega);
-    alpha = sn * sinh(M_LN2 /2 * bandwidth * omega /sn);
+    //alpha = sn * sinh(M_LN2 / 2 * bandwidth * omega / sn); // for BW
+    //alpha = sn / 2 * sqrt( ( A + 1 / A) * (1 / S - 1) + 2) // for S
+    alpha = sn / (2 * Q); // for Q
     beta = sqrt(A + A);
 
     switch (type) {
         case LPF:
-            b0 = (1 - cs) /2;
+            b0 = (1 - cs) / 2;
             b1 = 1 - cs;
-            b2 = (1 - cs) /2;
+            b2 = (1 - cs) / 2;
             a0 = 1 + alpha;
             a1 = -2 * cs;
             a2 = 1 - alpha;
             break;
-            
+
         case HPF:
-            b0 = (1 + cs) /2;
+            b0 = (1 + cs) / 2;
             b1 = -(1 + cs);
-            b2 = (1 + cs) /2;
+            b2 = (1 + cs) / 2;
             a0 = 1 + alpha;
             a1 = -2 * cs;
             a2 = 1 - alpha;
             break;
-            
+
         case BPF:
             b0 = alpha;
             b1 = 0;
@@ -76,7 +113,7 @@
             a1 = -2 * cs;
             a2 = 1 - alpha;
             break;
-            
+
         case NOTCH:
             b0 = 1;
             b1 = -2 * cs;
@@ -85,16 +122,16 @@
             a1 = -2 * cs;
             a2 = 1 - alpha;
             break;
-            
+
         case PEQ:
             b0 = 1 + (alpha * A);
             b1 = -2 * cs;
             b2 = 1 - (alpha * A);
-            a0 = 1 + (alpha /A);
+            a0 = 1 + (alpha / A);
             a1 = -2 * cs;
-            a2 = 1 - (alpha /A);
+            a2 = 1 - (alpha / A);
             break;
-            
+
         case LSH:
             b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
             b1 = 2 * A * ((A - 1) - (A + 1) * cs);
@@ -103,7 +140,7 @@
             a1 = -2 * ((A - 1) + (A + 1) * cs);
             a2 = (A + 1) + (A - 1) * cs - beta * sn;
             break;
-            
+
         case HSH:
             b0 = A * ((A + 1) + (A - 1) * cs + beta * sn);
             b1 = -2 * A * ((A - 1) + (A + 1) * cs);
@@ -112,45 +149,152 @@
             a1 = 2 * ((A - 1) - (A + 1) * cs);
             a2 = (A + 1) - (A - 1) * cs - beta * sn;
             break;
-            
+
         default:
             break;
     }
 
+#ifdef SPEW
+    printf("calculated coefficients a0 %f, a1 %f, a2 %f, b0 %f, b1 %f, b2 %f\n", a0, a1, a2, b0, b1, b2);
+#endif
+
     /* precompute the coefficients */
-    this->a0 = b0 /a0;
-    this->a1 = b1 /a0;
-    this->a2 = b2 /a0;
-    this->a3 = a1 /a0;
-    this->a4 = a2 /a0;
+    // DIRECT_FORM_I
+    this->a0_I = b0 /a0;
+    this->a1_I = b1 /a0;
+    this->a2_I = b2 /a0;
+    this->a3_I = a1 /a0;
+    this->a4_I = a2 /a0;
+
+}
+
+template <typename T>
+void Biquad<T>::CalcCoefficientsTransposedDirectII(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate,  T Q) {
+
+    T a0 = 0, a1 = 0, a2 = 0, b1 = 0, b2 = 0, norm = 0, V, K;   
+    
+    V = pow(10, fabs(dbGain) / 20);
+    K = tan(M_PI * freq / srate);
+    
+    switch (type) {
+        case LPF:
+            norm = 1 / (1 + K / Q + K * K);
+            a0 = K * K * norm;
+            a1 = 2 * a0;
+            a2 = a0;
+            b1 = 2 * (K * K - 1) * norm;
+            b2 = (1 - K / Q + K * K) * norm;            
+            break;
+
+        case HPF:
+            norm = 1 / (1 + K / Q + K * K);
+            a0 = 1 * norm;
+            a1 = -2 * a0;
+            a2 = a0;
+            b1 = 2 * (K * K - 1) * norm;
+            b2 = (1 - K / Q + K * K) * norm;
+            break;
+
+        case BPF:
+            norm = 1 / (1 + K / Q + K * K);
+            a0 = K / Q * norm;
+            a1 = 0;
+            a2 = -a0;
+            b1 = 2 * (K * K - 1) * norm;
+            b2 = (1 - K / Q + K * K) * norm;
+            break;
 
-    if (clearHistory) {
+        case NOTCH:
+            norm = 1 / (1 + K / Q + K * K);
+            a0 = (1 + K * K) * norm;
+            a1 = 2 * (K * K - 1) * norm;
+            a2 = a0;
+            b1 = a1;
+            b2 = (1 - K / Q + K * K) * norm;
+            break;
 
-        /* zero initial samples */
-        x1 = x2 = 0;
-        y1 = y2 = 0;
-        
+        case PEQ:
+            if (dbGain >= 0) { // boost
+                norm = 1 / (1 + 1 / Q * K + K * K);
+                a0 = (1 + V / Q * K + K * K) * norm;
+                a1 = 2 * (K * K - 1) * norm;
+                a2 = (1 - V / Q * K + K * K) * norm;
+                b1 = a1;
+                b2 = (1 - 1 / Q * K + K * K) * norm;
+            } else { // cut
+                norm = 1 / (1 + V / Q * K + K * K);
+                a0 = (1 + 1 / Q * K + K * K) * norm;
+                a1 = 2 * (K * K - 1) * norm;
+                a2 = (1 - 1 / Q * K + K * K) * norm;
+                b1 = a1;
+                b2 = (1 - V / Q * K + K * K) * norm;
+            }
+            break;
+
+        case LSH:
+            // TBI
+            break;
+
+        case HSH:
+            // TBI
+            break;
+
+        default:
+            break;
     }
-    
+
+#ifdef SPEW
+    printf("calculated coefficients a0 %f, a1 %f, a2 %f, b1 %f, b2 %f\n", a0, a1, a2, b1, b2);
+#endif
+
+    // TRANSPOSED_DIRECT_FORM_II
+    this->a0_II = a0;
+    this->a1_II = a1;
+    this->a2_II = a2;
+    this->b1_II = b1;
+    this->b2_II = b2;
+
 }
 
 template <typename T>
 /* Computes a BiQuad filter on a sample */
 T Biquad<T>::Calculate(T sample) {
 
-    T result;
+    T result = 0;
+
+    switch (form) {    
+        case DIRECT_FORM_I:
+            /* compute result */
+            result = a0_I * sample + a1_I * x1 + a2_I * x2 -
+                     a3_I * y1 - a4_I * y2;
 
-    /* compute result */
-    result = a0 * sample + a1 * x1 + a2 * x2 -
-             a3 * y1 - a4 * y2;
+            /* shift x1 to x2, sample to x1 */
+            x2 = x1;
+            x1 = sample;
+
+            /* shift y1 to y2, result to y1 */
+            y2 = y1;
+            y1 = result;
+            break;
 
-    /* shift x1 to x2, sample to x1 */
-    x2 = x1;
-    x1 = sample;
+        case TRANSPOSED_DIRECT_FORM_II:
+#if 1        
+            result = sample * a0_II + z1;
+            z1 = sample * a1_II + z2 - b1_II * result;
+            z2 = sample * a2_II - b2_II * result;
+#else
+            result = a0_II * sample + a1_II * x1 + a2_II * x2 - b1_II * y1 - b2_II * y2;
+            x2 = x1;
+            x1 = sample;
+            y2 = y1;
+            y1 = result;
+#endif            
+            break;
 
-    /* shift y1 to y2, result to y1 */
-    y2 = y1;
-    y1 = result;
+        default:
+            break;
+
+    }
 
     return result;
 }

--- a/biquad.h	Wed Feb 15 18:12:50 2012 +0000
+++ b/biquad.h	Thu Feb 16 00:07:57 2012 +0000
@@ -1,8 +1,9 @@
 /*
  *  biquad.h
  *
- *  Original source material from Robert Bristow Johnson at 
+ *  Original source material from Robert Bristow-Johnson at 
  *  http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
+ *  and Nigel Redmon at http://www.earlevel.com/ 
  *  
  *  Derived from public domain C implementation by Tom St. Denis
  *
@@ -25,6 +26,12 @@
 #define M_PI        3.14159265358979323846
 #endif
 
+/* formulation types */
+typedef enum {
+    DIRECT_FORM_I = 0,
+    TRANSPOSED_DIRECT_FORM_II
+} BIQUAD_FORMULATION_TYPE;
+
 /* filter types */
 typedef enum {
     LPF, /* low pass filter */
@@ -41,17 +48,29 @@
 
 public:
     Biquad(BIQUAD_FILTER_TYPE type, 
-        T dbGain,  /* gain of filter */
+        T dbGain,  /* peak gain of filter */
         T freq,  /* center frequency */
         T srate,  /* sampling rate */
-        T bandwidth); /* bandwidth in octaves */
+        T Q, /* Q */
+        BIQUAD_FORMULATION_TYPE form = DIRECT_FORM_I);                       
         
-    void Reset(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth, bool clearHistory = false);
+    void Reset(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T Q, bool clearHistory = false, BIQUAD_FORMULATION_TYPE form = DIRECT_FORM_I);
+    
+    void CalcCoefficientsDirectI(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T Q);
+    
+    void CalcCoefficientsTransposedDirectII(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T Q);
     
     T Calculate(T sample);
     
-    T a0, a1, a2, a3, a4;
+    BIQUAD_FORMULATION_TYPE form;    
+    
+    // used for DIRECT_FORM_1
+    T a0_I, a1_I, a2_I, a3_I, a4_I; 
     T x1, x2, y1, y2;
+    
+    // used for TRANSPOSED_DIRECT_FORM_II
+    T a0_II, a1_II, a2_II, b1_II, b2_II;    
+    T z1, z2;        
 
 };