read_filter_new.cpp

00001 #include "mbed.h"
00002 #include "read_filter_emg.h"
00003 //included for fabs() function
00004 #include <math.h>
00005 
00006 //input (u) = analogin
00007 
00008 AnalogIn(/*pin-name*/) = &aI;
00009 input = aI.read();
00010 
00011 double v1=0;
00012 double v2=0;
00013 
00014 //general biquad filter that can be called in all the filter functions
00015 double biquad(double u, double &v1, double &v2, const double a1,
00016                     const double a2, const double b0, const double b1, const double b2)
00017 {
00018     double v = u * a1*v1 * a2*v2;
00019     double y = b0*v + b1*v1 + b2*v2;
00020     //values of v2 and v1 are updated, as they are passed by reference
00021     //they update globally
00022     v2 = v1; v1 = v;
00023     return y;
00024 }
00025 
00026 //Specifying filter coefficients highpass
00027 
00028    /* high pass filter consists of three cascaded biquads
00029     blow coefficients for those three biquads */
00030     //first high pass biquad
00031     const double highp1_a1 = -0.67538034389;
00032     const double highp1_a2 = 0.12769255668;
00033     const double highp1_b0 = 1.00000000000;
00034     const double highp1_b1 = -2.00000000000;
00035     const double highp1_b2 = 1.00000000000;
00036 
00037     //second high pass biquad
00038     const double highp2_a1 = -0.76475499450;
00039     const double highp2_a2 = 0.27692273367;
00040     const double highp2_b0 = 1.00000000000;
00041     const double highp2_b1 = -2.00000000000;
00042     const double highp2_b2 = 1.00000000000;
00043 
00044     //third high pass biquad
00045     const double highp3_a1 = -0.99216561242;
00046     const double highp3_a2 = 0.65663360837;
00047     const double highp3_b0 = 1.00000000000;
00048     const double highp3_b1 = -2.00000000000;
00049     const double highp3_b2 = 1.00000000000;
00050     
00051 //Specifying filter coefficients lowpass
00052 
00053     /* lowpass filter consists of three cascaded biquads
00054     below the coefficients for those three biquads */
00055     //first high pass biquad
00056     const double lowp1_a1 = -1.05207469728;
00057     const double lowp1_a2 = 0.28586907478;
00058     const double lowp1_b0 = 1.00000000000;
00059     const double lowp1_b1 = 2.00000000000;
00060     const double lowp1_b2 = 1.00000000000;
00061 
00062     //second high pass biquad
00063     const double lowp2_a1 = -1.16338171052;
00064     const double lowp2_a2 = 0.42191097989;
00065     const double lowp2_b0 = 1.00000000000;
00066     const double lowp2_b1 = 2.00000000000;
00067     const double lowp2_b2 = 1.00000000000;
00068 
00069     //third high pass biquad
00070     const double lowp3_a1 = -1.42439823874;
00071     const double lowp3_a2 = 0.74093118112;
00072     const double lowp3_b0 = 1.00000000000;
00073     const double lowp3_b1 = 2.00000000000;
00074     const double lowp3_b2 = 1.00000000000;
00075     
00076 //input to each filter is output of the filter before(excl. first which uses input_sample)
00077     /* NOT SURE IF PASSING V1 AND V2 IS CORRECT,
00078     WILL IT UPDATE IN THE MEMORY POSITION SO THAT
00079     V1 IS CHANGED GLOBALLY */
00080     
00081 //highpass
00082 
00083 double highpass_filter(double input)
00084 {
00085     double y1 = biquad(input, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
00086     double y2 = biquad(y1, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
00087     double y3 = biquad(y2, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2); 
00088     
00089     return y3;
00090 }
00091     
00092     
00093 //rectifier
00094 double rectify(double y3)
00095 {
00096     y3 = fabs(y3);
00097     return y3;
00098 }
00099 
00100 //lowpass
00101 
00102 double lowpass_filter(double y3)
00103 {
00104     double y4 = biquad(y3, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
00105     double y5 = biquad(y4, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
00106     double filtered_signal = biquad(y5, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
00107     
00108     return filtered_signal;
00109 }
00110 
00111 double filter(double input)
00112 {
00113     /* function passes the input through the three filters
00114     returns the final output value as filtered sample
00115     this is used in check_state() function to determine state of system
00116     */
00117     double y1 = highpass_filter(input);
00118     double y2 = rectify(y3);
00119     double filtered_signal = lowpass_filter(y3);
00120 
00121     return filtered_signal;
00122 }