Diff: filters.cpp

Revision:

0:a5367bd4e404

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/filters.cpp	Tue May 20 15:11:16 2014 +0000
@@ -0,0 +1,324 @@
+#include "filters.h"
+
+float isoFilter_Wk_z_Hl (float y_2, float y_1, float x[3], int PasoTiempo )
+{
+    float y_0 = 0.0;//valor filtrado
+
+    //valores de norma ISO 2631-1:1997, pagina 25
+    //float f2 = 100.0;
+    float fs = (1000000.0/PasoTiempo);//frecuencia
+
+    //composición de 4 filtros: 1 Hh paso alto, 2 Hl paso bajo (para limitar bandas);
+    //3 Ht filtro trancisión aceleración-velocidad, 4 Hs subida de escala
+
+    //filtros límite banda Hh y Hl
+    //Hl
+    //comprobación por si nyq<f2
+    if ( fs == 1000 )
+    {
+        y_0 = toLowPass100HzO2_1000( y_2, y_1, x );//adquisición a 1000Hz
+        
+    }else if ( fs == 320 )
+    {
+        y_0 = toLowPass100HzO2_320( y_2, y_1, x );//adquisición a 320Hz
+    }else//seguridad
+    {
+        y_0 = toLowPass100HzO2_320( y_2, y_1, x );//adquisición a 320Hz
+    }
+
+    return y_0;
+}
+
+float isoFilter_Wd_xy_Hl (float y_2, float y_1, float x[3], int PasoTiempo )
+{
+    float y_0 = 0.0;//valor filtrado
+
+    //valores de norma ISO 2631-1:1997, pagina 25
+    //float f2 = 100.0;
+    float fs = (1000000.0/PasoTiempo);//frecuencia
+
+    //composición de 4 filtros: 1 Hh paso alto, 2 Hl paso bajo (para limitar bandas);
+    //3 Ht filtro trancisión aceleración-velocidad, 4 Hs subida de escala
+
+    //filtros límite banda Hh y Hl
+    //Hl
+    //comprobación por si nyq<f2
+    if ( fs == 1000 )
+    {
+        y_0 = toLowPass100HzO2_1000( y_2, y_1, x );//adquisición a 1000Hz
+        
+    }else if ( fs == 320 )
+    {
+        y_0 = toLowPass100HzO2_320( y_2, y_1, x );//adquisición a 320Hz
+    }else//seguridad
+    {
+        y_0 = toLowPass100HzO2_320( y_2, y_1, x );//adquisición a 320Hz
+    }
+
+    return y_0;
+}
+
+float isoFilter_Wk_z_Hh (float y_2, float y_1, float x[3], int PasoTiempo )
+{
+    float y_0 = 0.0;//valor filtrado
+    float fs = (1000000.0/PasoTiempo);//frecuencia
+
+    //valores de norma ISO 2631-1:1997, pagina 25
+    if ( fs == 320 )
+    {
+        y_0 = toHighPass04HzO2_320( y_2, y_1, x );
+    }else if ( fs == 1000 )
+    {
+        y_0 = toHighPass04HzO2_1000( y_2, y_1, x );
+    }else//seguridad
+    {
+        y_0 = toHighPass04HzO2_320( y_2, y_1, x );
+    }
+    return y_0;
+
+}
+
+float isoFilter_Wd_xy_Hh (float y_2, float y_1, float x[3], int PasoTiempo )
+{
+    float y_0 = 0.0;//valor filtrado
+    float fs = (1000000.0/PasoTiempo);//frecuencia
+
+    //valores de norma ISO 2631-1:1997, pagina 25
+    if ( fs == 320 )
+    {
+        y_0 = toHighPass04HzO2_320( y_2, y_1, x );
+    }else if ( fs == 1000 )
+    {
+        y_0 = toHighPass04HzO2_1000( y_2, y_1, x );
+    }else//seguridad
+    {
+        y_0 = toHighPass04HzO2_320( y_2, y_1, x );
+    }
+    return y_0;
+
+}
+
+float isoFilter_Wk_z_Ht (float y_2, float y_1, float x[3], int PasoTiempo )
+{
+    float y_0 = 0.0;//valor filtrado
+    float fs = (1000000.0/PasoTiempo);//frecuencia de Nyquist
+    float a[3],b[3];
+
+    if ( fs == 320 )
+    {
+        //Ht,--Para fs=320.
+        b[0] = 0.113880370820785;
+        b[1] = 0.024895331352765;
+        b[2] = -0.088985039468020;
+        a[0] = 1.0;
+        a[1] = -1.628200749534463;
+        a[2] = 0.677991412239993;
+     }else if ( fs == 1000) {  
+        //Ht,--Para fs=1000.
+        b[0] = 0.038361670159002;
+        b[1] = 0.002899072228580;
+        b[2] = -0.035462597930422;
+        a[0] = 1.0;
+        a[1] = -1.877020477623120;
+        a[2] = 0.882818622080279;
+     }
+     else
+     {
+        b[0] = 0.113880370820785;
+        b[1] = 0.024895331352765;
+        b[2] = -0.088985039468020;
+        a[0] = 1.0;
+        a[1] = -1.628200749534463;
+        a[2] = 0.677991412239993;
+     }
+     y_0 = toFilter( b, a, y_2, y_1, x);
+     return y_0;
+
+}
+
+float isoFilter_Wd_xy_Ht (float y_2, float y_1, float x[3], int PasoTiempo )
+{
+    float y_0 = 0.0;//valor filtrado
+    float fs = (1000000.0/PasoTiempo);//frecuencia
+    float a[3],b[3];
+
+    if ( fs == 320 )
+    {
+        //Ht,--Para fs=320.
+        b[0] = 0.019408118133464;
+        b[1] = 0.000747478314467;
+        b[2] = -0.018660639818997;
+        a[0] = 1.0;
+        a[1] = -1.938078443446524;
+        a[2] = 0.939573400075458;
+    }else if ( fs == 1000 ) {
+        //Ht,--Para fs=1000.
+        b[0] = 0.006259983831114;
+        b[1] = 0.000078174094539;
+        b[2] = -0.006181809736574;
+        a[0] = 1.000000000000000;
+        a[1] = -1.980094773132050;
+        a[2] = 0.980251121321129;
+     }
+     else
+     {
+        b[0] = 0.019408118133464;
+        b[1] = 0.000747478314467;
+        b[2] = -0.018660639818997;
+        a[0] = 1.0;
+        a[1] = -1.938078443446524;
+        a[2] = 0.939573400075458;
+     }
+     y_0 = toFilter( b, a, y_2, y_1, x);
+     return y_0;
+
+}
+
+float isoFilter_Wk_z_Hs (float y_2, float y_1, float x[3], int PasoTiempo )
+{
+    float y_0 = 0.0;//valor filtrado
+    float fs = (1000000.0/PasoTiempo);//frecuencia
+    float a[3],b[3];
+    
+    if ( fs == 320 )
+    {
+        //Hs,--Para fs=320.
+        b[0] = 0.989285860229759;
+        b[1] = -1.927181919272167;
+        b[2] = 0.939983837287274;
+        a[0] = 1.0;
+        a[1] = -1.926140130231110 ;
+        a[2] = 0.930311486558090;
+     }else if ( fs == 1000 )   
+     {
+        //Hs,--Para fs=1000.
+        b[0] = 0.996601106679302;
+        b[1] = -1.976807985278312;
+        b[2] = 0.980426065701832;
+        a[0] = 1.000000000000000;
+        a[1] = -1.976698612205658 ;
+        a[2] = 0.977136545453787;
+     }
+     else{
+        b[0] = 0.989285860229759;
+        b[1] = -1.927181919272167;
+        b[2] = 0.939983837287274;
+        a[0] = 1.0;
+        a[1] = -1.926140130231110 ;
+        a[2] = 0.930311486558090;
+     }
+     y_0 = toFilter( b, a, y_2, y_1, x);
+    return y_0;
+
+}
+
+float toFilter ( float b[3], float a[3], float y_2, float y_1, float x[3] )
+{
+    float data = b[0]*x[2] + b[1]*x[1] + b[2]*x[0] - a[1]*y_1 - a[2]*y_2;
+
+    return data;
+}
+
+float toLowPass100HzO2_320 ( float y_2, float y_1, float x[3]  )
+{
+    #define NZEROS 2
+    #define NPOLES 2
+    #define GAIN1   2.391409984e+00
+
+    float xv[NZEROS+1], yv[NPOLES+1];
+
+
+    xv[0] = x[0]/ GAIN1; xv[1] = x[1]/ GAIN1;
+    xv[2] = x[2] / GAIN1;
+    yv[0] = y_2; yv[1] = y_1;
+    yv[2] =   (xv[0] + xv[2]) + 2.0 * xv[1]
+              + ( -0.2097153578 * yv[0]) + ( -0.4629380253 * yv[1]);
+    return yv[2];
+}
+
+float toLowPass100HzO2_1000 ( float y_2, float y_1, float x[3]  )
+{
+    #define NZEROS 2
+    #define NPOLES 2
+    #define GAIN11   1.482463775e+01
+
+    float xv[NZEROS+1], yv[NPOLES+1];
+
+
+    xv[0] = x[0]/ GAIN11; xv[1] = x[1]/ GAIN11;
+    xv[2] = x[2] / GAIN11;
+    yv[0] = y_2; yv[1] = y_1;
+    yv[2] =   (xv[0] + xv[2]) + 2.0 * xv[1]
+                     + ( -0.4128015981 * yv[0]) + (  1.1429805025 * yv[1]);
+    return yv[2];
+}
+
+float toHighPass04HzO2_320 ( float y_2, float y_1, float x[3]  )
+{
+    #define NZEROS 2
+    #define NPOLES 2
+    #define GAIN4 1.005569054e+00;
+
+    static float xv[NZEROS+1], yv[NPOLES+1];
+
+
+    xv[0] = x[0]/ GAIN4; xv[1] = x[1]/ GAIN4;
+    xv[2] = x[2] / GAIN4;
+//    xv[2] = x[2];
+    yv[0] = y_2; yv[1] = y_1;
+    yv[2] =   (xv[0] + xv[2]) - 2.0 * xv[1]
+              + ( -0.9889542499 * yv[0]) + (  1.9888929059 * yv[1]);
+    return (yv[2]);
+}
+
+float toHighPass04HzO2_1000 ( float y_2, float y_1, float x[3]  )
+{
+    #define NZEROS 2
+    #define NPOLES 2
+    #define GAIN44 1.001778733e+00;
+
+    static float xv[NZEROS+1], yv[NPOLES+1];
+
+
+    xv[0] = x[0]/ GAIN44; xv[1] = x[1]/ GAIN44;
+    xv[2] = x[2] / GAIN44;
+//    xv[2] = x[2];
+    yv[0] = y_2; yv[1] = y_1;
+    yv[2] =   (xv[0] + xv[2]) - 2.0 * xv[1]
+                     + ( -0.9964520027 * yv[0]) + (  1.9964456974 * yv[1]);
+    return (yv[2]);
+}
+
+float toLowPass1HzO2_320 ( float y_2, float y_1, float x[3]  )
+{
+    #define NZEROS 2
+    #define NPOLES 2
+    #define GAIN6 1.051966853e+04
+
+    float xv[NZEROS+1], yv[NPOLES+1];
+
+
+    xv[0] = x[0]/ GAIN6; xv[1] = x[1]/ GAIN6;
+    xv[2] = x[2] / GAIN6;
+    yv[0] = y_2; yv[1] = y_1;
+    yv[2] =   (xv[0] + xv[2]) + 2.0 * xv[1]
+              + ( -0.9726139693 * yv[0]) + ( 1.9722337292 * yv[1]);
+    return yv[2];
+}
+
+float toLowPass1HzO2_1000 ( float y_2, float y_1, float x[3]  )
+{
+    #define NZEROS 2
+    #define NPOLES 2
+    #define GAIN66 1.017716736e+05
+
+    float xv[NZEROS+1], yv[NPOLES+1];
+
+
+    xv[0] = x[0]/ GAIN66; xv[1] = x[1]/ GAIN66;
+    xv[2] = x[2] / GAIN66;
+    yv[0] = y_2; yv[1] = y_1;
+    yv[2] =   (xv[0] + xv[2]) + 2.0 * xv[1]
+                     + ( -0.9911535959 * yv[0]) + (  1.9911142922 * yv[1]);
+    return yv[2];
+}
\ No newline at end of file