File content as of revision 0:a5367bd4e404:

#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];
}