Esta versión v6 pasa a ser el nuevo master. Funciona correctamente
Dependencies: ADXL345 Display1602 MSCFileSystem SDFileSystem mbed FATFileSystem
filters.cpp
- Committer:
- JuanManuelAmador
- Date:
- 2014-06-06
- Revision:
- 2:cc4a43d806e2
- Parent:
- 0:a5367bd4e404
File content as of revision 2:cc4a43d806e2:
#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]; }