Revision 6:d2e6404e1cb8, committed 2016-11-01

Comitter:

Nickname

Date:

Tue Nov 01 09:18:44 2016 +0000

Parent:

5:51a28834cd5b

Commit message:

Filter with extra notch filter 100 Hz

Changed in this revision

--- a/main.cpp	Fri Oct 28 08:08:37 2016 +0000
+++ b/main.cpp	Tue Nov 01 09:18:44 2016 +0000
@@ -1,4 +1,4 @@
-#include "mbed.h"
+ #include "mbed.h"
 #include "BiQuad.h"
 #include "HIDScope.h"
 #include "MODSERIAL.h"
@@ -30,7 +30,20 @@
 double rno_b1 = -1.6036;   
 double rno_b2 = 0.9911;    
 double rno_a1 = -1.603;    
-double rno_a2 = 0.9822;    
+double rno_a2 = 0.9822;
+
+
+double lno2_b0 = 0.9824;
+double lno2_b1 = -0.6071;
+double lno2_b2 = 0.9824;
+double lno2_a1 = -0.6071;
+double lno2_a2 = 0.9647;
+   
+double rno2_b0 = 0.9824;
+double rno2_b1 = -0.6071;
+double rno2_b2 = 0.9824;
+double rno2_a1 = -0.6071;
+double rno2_a2 = 0.9647;
 
 
 double lhf_b0 = 0.9355;    
@@ -61,10 +74,12 @@
 
 //starting values of the biquads of the corresponding filters
 double lno_v1 = 0, lno_v2 = 0;
+double lno2_v1 = 0, lno2_v2 = 0;
 double lhf_v1 = 0, lhf_v2 = 0;
 double llf_v1 = 0, llf_v2 = 0;
 
 double rno_v1 = 0, rno_v2 = 0;
+double rno2_v1 = 0, rno2_v2 = 0;
 double rhf_v1 = 0, rhf_v2 = 0;
 double rlf_v1 = 0, rlf_v2 = 0;
 
@@ -73,10 +88,12 @@
 so lno_y goes into lhf_y etc.
 */ 
 double lno_y;
+double lno2_y;
 double lhf_y;
 double llf_y;
 double lrect_y;
 double rno_y;
+double rno2_y;
 double rhf_y;
 double rlf_y;
 double rrect_y;
@@ -101,6 +118,16 @@
     return y;
 }
 
+double biquad_lno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+    const double b1 , const double b2 )
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
 double biquad_lhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
     const double b1 , const double b2 )
 {
@@ -130,6 +157,16 @@
     return y;
 }
 
+double biquad_rno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+    const double b1 , const double b2 )
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
 double biquad_rhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
     const double b1 , const double b2 )
 {
@@ -156,11 +193,13 @@
 The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
 void scopeSend(void){
     lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
-    lhf_y = biquad_lhf(lno_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
+    lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2);
+    lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
     lrect_y = fabs(lhf_y);
     llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2;
     rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
-    rhf_y = biquad_rhf(rno_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
+    rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2);
+    rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
     rrect_y = fabs(rhf_y);
     rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;  
     scope.set(1, llf_y);