Diff: main.cpp

Revision:

1:0b510109a099

Parent:

0:e239fd599412

Child:

2:1d99e9b70f0d

--- a/main.cpp	Sun Dec 06 19:39:37 2015 +0000
+++ b/main.cpp	Sun Dec 06 20:14:46 2015 +0000
@@ -34,6 +34,10 @@
 inline float FilterFloat(float Variable);
 // declare output filter
 inline float OutputFilterFloat(float Variable);
+// declare first filter
+inline float FirstFilterFloat(float Variable);
+// declare second filter
+inline float SecondFilterFloat(float Variable);
 //nadav - declare simple filter
 inline float simpleFilterFloat(float Variable);
 
@@ -70,10 +74,27 @@
 //float AHPF=0.885458; //7KHz cut off fs=340
 //float AHPF=0.931168; //8KHz cut off fs=340*2
 float AHPF=0.999128; //100Hz cut off fs=340*2
+float SOSMat[6]= { 1.000000, -1.970399, 1.000000, 1.000000, -1.942826, 0.946079}; 
+float Gscale=0.097947; 
+//nrackoz ydagon hpf
+float SOSMat_H[6]= { 1.000000, -2.000000, 1.000000, 1.000000, -1.949235, 0.950492}; 
+float Gscale_H=0.974932; 
+//nrakocz ydagon - simple filter variables
+//float AHPF=0.730141;
+//filter 1 bus
+float LLastY=0;
 float LastY=0;
 float CurY=0;
+float LLastU=0;
 float LastU=0;
 float CurU=0;
+//filter 2 bus
+float LLastY_H=0;
+float LastY_H=0;
+float CurY_H=0;
+float LLastU_H=0;
+float LastU_H=0;
+float CurU_H=0;
 
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -147,6 +168,8 @@
         
         ADCFloat=simpleFilterFloat(ADCFloat); //nadav - filtering input
         
+        ADCFloat=FirstFilterFloat(ADCFloat);
+        ADCFloat=SecondFilterFloat(ADCFloat);
         
         ////////////////////////////
         // Apply Filter to Output //
@@ -204,9 +227,46 @@
     return CurY;
 }// end output filter function
 
+
+inline float FirstFilterFloat(float Variable){
+    LLastU_H=LastU_H;
+    LastU_H=CurU_H;
+    CurU_H=Variable;
+    LLastY_H=LastY_H;
+    LastY_H=CurY_H;
+    
+   // IIR biquad filter direct form 1 // y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]
+    CurY_H=SOSMat_H[0]*CurU_H + SOSMat_H[1]*LastU_H + SOSMat_H[2]*LLastU_H - SOSMat_H[4]*LastY_H - SOSMat_H[5]*LLastY_H;
+    return CurY_H*Gscale_H; // scale output
+}// end filter function
+
+
 ///////////////////////////////////////////
 // Output Filter Function /////////////////
 ///////////////////////////////////////////
+
+///////////////////////////////////////////
+inline float SecondFilterFloat(float Variable, int LPF_flag){
+        
+    // Buffer variables
+    LLastU=LastU;
+    LastU=CurU;
+    CurU=Variable;
+    LLastY=LastY;
+    LastY=CurY;
+    
+        // IIR biquad filter direct form 1 // y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2] 
+        CurY=SOSMat[0]*CurU + SOSMat[1]*LastU + SOSMat[2]*LLastU - SOSMat[4]*LastY - SOSMat[5]*LLastY;
+        return CurY*Gscale;
+
+        //IIR biquad filter direct form 1 // y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]
+        CurY=SOSMat_H[0]*CurU + SOSMat_H[1]*LastU + SOSMat_H[2]*LLastU - SOSMat_H[4]*LastY - SOSMat_H[5]*LLastY;
+        return CurY*Gscale_H; // scale output
+
+// end output filter function
+}
+
+/////gain filter/////
 inline float OutputFilterFloat(float Variable){
     Variable*=1.0f; // Example of Math operation, Make sure to use float operations and not Double.
     return Variable;