virtualmech / Mbed 2 deprecated vmConfort_v6

Esta versión v6 pasa a ser el nuevo master. Funciona correctamente

Dependencies:   ADXL345 Display1602 MSCFileSystem SDFileSystem mbed FATFileSystem

filters.cpp@2:cc4a43d806e2, 2014-06-06 (annotated)

Committer:
JuanManuelAmador
Date:
Fri Jun 06 08:52:28 2014 +0000
Revision:
2:cc4a43d806e2
Parent:
0:a5367bd4e404 
Correci?n del baudrate de 112500 a 115200

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jjmedina 0:a5367bd4e404 1 #include "filters.h"
jjmedina 0:a5367bd4e404 2
jjmedina 0:a5367bd4e404 3 float isoFilter_Wk_z_Hl (float y_2, float y_1, float x[3], int PasoTiempo )
jjmedina 0:a5367bd4e404 4 {
jjmedina 0:a5367bd4e404 5 float y_0 = 0.0;//valor filtrado
jjmedina 0:a5367bd4e404 6
jjmedina 0:a5367bd4e404 7 //valores de norma ISO 2631-1:1997, pagina 25
jjmedina 0:a5367bd4e404 8 //float f2 = 100.0;
jjmedina 0:a5367bd4e404 9 float fs = (1000000.0/PasoTiempo);//frecuencia
jjmedina 0:a5367bd4e404 10
jjmedina 0:a5367bd4e404 11 //composición de 4 filtros: 1 Hh paso alto, 2 Hl paso bajo (para limitar bandas);
jjmedina 0:a5367bd4e404 12 //3 Ht filtro trancisión aceleración-velocidad, 4 Hs subida de escala
jjmedina 0:a5367bd4e404 13
jjmedina 0:a5367bd4e404 14 //filtros límite banda Hh y Hl
jjmedina 0:a5367bd4e404 15 //Hl
jjmedina 0:a5367bd4e404 16 //comprobación por si nyq<f2
jjmedina 0:a5367bd4e404 17 if ( fs == 1000 )
jjmedina 0:a5367bd4e404 18 {
jjmedina 0:a5367bd4e404 19 y_0 = toLowPass100HzO2_1000( y_2, y_1, x );//adquisición a 1000Hz
jjmedina 0:a5367bd4e404 20
jjmedina 0:a5367bd4e404 21 }else if ( fs == 320 )
jjmedina 0:a5367bd4e404 22 {
jjmedina 0:a5367bd4e404 23 y_0 = toLowPass100HzO2_320( y_2, y_1, x );//adquisición a 320Hz
jjmedina 0:a5367bd4e404 24 }else//seguridad
jjmedina 0:a5367bd4e404 25 {
jjmedina 0:a5367bd4e404 26 y_0 = toLowPass100HzO2_320( y_2, y_1, x );//adquisición a 320Hz
jjmedina 0:a5367bd4e404 27 }
jjmedina 0:a5367bd4e404 28
jjmedina 0:a5367bd4e404 29 return y_0;
jjmedina 0:a5367bd4e404 30 }
jjmedina 0:a5367bd4e404 31
jjmedina 0:a5367bd4e404 32 float isoFilter_Wd_xy_Hl (float y_2, float y_1, float x[3], int PasoTiempo )
jjmedina 0:a5367bd4e404 33 {
jjmedina 0:a5367bd4e404 34 float y_0 = 0.0;//valor filtrado
jjmedina 0:a5367bd4e404 35
jjmedina 0:a5367bd4e404 36 //valores de norma ISO 2631-1:1997, pagina 25
jjmedina 0:a5367bd4e404 37 //float f2 = 100.0;
jjmedina 0:a5367bd4e404 38 float fs = (1000000.0/PasoTiempo);//frecuencia
jjmedina 0:a5367bd4e404 39
jjmedina 0:a5367bd4e404 40 //composición de 4 filtros: 1 Hh paso alto, 2 Hl paso bajo (para limitar bandas);
jjmedina 0:a5367bd4e404 41 //3 Ht filtro trancisión aceleración-velocidad, 4 Hs subida de escala
jjmedina 0:a5367bd4e404 42
jjmedina 0:a5367bd4e404 43 //filtros límite banda Hh y Hl
jjmedina 0:a5367bd4e404 44 //Hl
jjmedina 0:a5367bd4e404 45 //comprobación por si nyq<f2
jjmedina 0:a5367bd4e404 46 if ( fs == 1000 )
jjmedina 0:a5367bd4e404 47 {
jjmedina 0:a5367bd4e404 48 y_0 = toLowPass100HzO2_1000( y_2, y_1, x );//adquisición a 1000Hz
jjmedina 0:a5367bd4e404 49
jjmedina 0:a5367bd4e404 50 }else if ( fs == 320 )
jjmedina 0:a5367bd4e404 51 {
jjmedina 0:a5367bd4e404 52 y_0 = toLowPass100HzO2_320( y_2, y_1, x );//adquisición a 320Hz
jjmedina 0:a5367bd4e404 53 }else//seguridad
jjmedina 0:a5367bd4e404 54 {
jjmedina 0:a5367bd4e404 55 y_0 = toLowPass100HzO2_320( y_2, y_1, x );//adquisición a 320Hz
jjmedina 0:a5367bd4e404 56 }
jjmedina 0:a5367bd4e404 57
jjmedina 0:a5367bd4e404 58 return y_0;
jjmedina 0:a5367bd4e404 59 }
jjmedina 0:a5367bd4e404 60
jjmedina 0:a5367bd4e404 61 float isoFilter_Wk_z_Hh (float y_2, float y_1, float x[3], int PasoTiempo )
jjmedina 0:a5367bd4e404 62 {
jjmedina 0:a5367bd4e404 63 float y_0 = 0.0;//valor filtrado
jjmedina 0:a5367bd4e404 64 float fs = (1000000.0/PasoTiempo);//frecuencia
jjmedina 0:a5367bd4e404 65
jjmedina 0:a5367bd4e404 66 //valores de norma ISO 2631-1:1997, pagina 25
jjmedina 0:a5367bd4e404 67 if ( fs == 320 )
jjmedina 0:a5367bd4e404 68 {
jjmedina 0:a5367bd4e404 69 y_0 = toHighPass04HzO2_320( y_2, y_1, x );
jjmedina 0:a5367bd4e404 70 }else if ( fs == 1000 )
jjmedina 0:a5367bd4e404 71 {
jjmedina 0:a5367bd4e404 72 y_0 = toHighPass04HzO2_1000( y_2, y_1, x );
jjmedina 0:a5367bd4e404 73 }else//seguridad
jjmedina 0:a5367bd4e404 74 {
jjmedina 0:a5367bd4e404 75 y_0 = toHighPass04HzO2_320( y_2, y_1, x );
jjmedina 0:a5367bd4e404 76 }
jjmedina 0:a5367bd4e404 77 return y_0;
jjmedina 0:a5367bd4e404 78
jjmedina 0:a5367bd4e404 79 }
jjmedina 0:a5367bd4e404 80
jjmedina 0:a5367bd4e404 81 float isoFilter_Wd_xy_Hh (float y_2, float y_1, float x[3], int PasoTiempo )
jjmedina 0:a5367bd4e404 82 {
jjmedina 0:a5367bd4e404 83 float y_0 = 0.0;//valor filtrado
jjmedina 0:a5367bd4e404 84 float fs = (1000000.0/PasoTiempo);//frecuencia
jjmedina 0:a5367bd4e404 85
jjmedina 0:a5367bd4e404 86 //valores de norma ISO 2631-1:1997, pagina 25
jjmedina 0:a5367bd4e404 87 if ( fs == 320 )
jjmedina 0:a5367bd4e404 88 {
jjmedina 0:a5367bd4e404 89 y_0 = toHighPass04HzO2_320( y_2, y_1, x );
jjmedina 0:a5367bd4e404 90 }else if ( fs == 1000 )
jjmedina 0:a5367bd4e404 91 {
jjmedina 0:a5367bd4e404 92 y_0 = toHighPass04HzO2_1000( y_2, y_1, x );
jjmedina 0:a5367bd4e404 93 }else//seguridad
jjmedina 0:a5367bd4e404 94 {
jjmedina 0:a5367bd4e404 95 y_0 = toHighPass04HzO2_320( y_2, y_1, x );
jjmedina 0:a5367bd4e404 96 }
jjmedina 0:a5367bd4e404 97 return y_0;
jjmedina 0:a5367bd4e404 98
jjmedina 0:a5367bd4e404 99 }
jjmedina 0:a5367bd4e404 100
jjmedina 0:a5367bd4e404 101 float isoFilter_Wk_z_Ht (float y_2, float y_1, float x[3], int PasoTiempo )
jjmedina 0:a5367bd4e404 102 {
jjmedina 0:a5367bd4e404 103 float y_0 = 0.0;//valor filtrado
jjmedina 0:a5367bd4e404 104 float fs = (1000000.0/PasoTiempo);//frecuencia de Nyquist
jjmedina 0:a5367bd4e404 105 float a[3],b[3];
jjmedina 0:a5367bd4e404 106
jjmedina 0:a5367bd4e404 107 if ( fs == 320 )
jjmedina 0:a5367bd4e404 108 {
jjmedina 0:a5367bd4e404 109 //Ht,--Para fs=320.
jjmedina 0:a5367bd4e404 110 b[0] = 0.113880370820785;
jjmedina 0:a5367bd4e404 111 b[1] = 0.024895331352765;
jjmedina 0:a5367bd4e404 112 b[2] = -0.088985039468020;
jjmedina 0:a5367bd4e404 113 a[0] = 1.0;
jjmedina 0:a5367bd4e404 114 a[1] = -1.628200749534463;
jjmedina 0:a5367bd4e404 115 a[2] = 0.677991412239993;
jjmedina 0:a5367bd4e404 116 }else if ( fs == 1000) {
jjmedina 0:a5367bd4e404 117 //Ht,--Para fs=1000.
jjmedina 0:a5367bd4e404 118 b[0] = 0.038361670159002;
jjmedina 0:a5367bd4e404 119 b[1] = 0.002899072228580;
jjmedina 0:a5367bd4e404 120 b[2] = -0.035462597930422;
jjmedina 0:a5367bd4e404 121 a[0] = 1.0;
jjmedina 0:a5367bd4e404 122 a[1] = -1.877020477623120;
jjmedina 0:a5367bd4e404 123 a[2] = 0.882818622080279;
jjmedina 0:a5367bd4e404 124 }
jjmedina 0:a5367bd4e404 125 else
jjmedina 0:a5367bd4e404 126 {
jjmedina 0:a5367bd4e404 127 b[0] = 0.113880370820785;
jjmedina 0:a5367bd4e404 128 b[1] = 0.024895331352765;
jjmedina 0:a5367bd4e404 129 b[2] = -0.088985039468020;
jjmedina 0:a5367bd4e404 130 a[0] = 1.0;
jjmedina 0:a5367bd4e404 131 a[1] = -1.628200749534463;
jjmedina 0:a5367bd4e404 132 a[2] = 0.677991412239993;
jjmedina 0:a5367bd4e404 133 }
jjmedina 0:a5367bd4e404 134 y_0 = toFilter( b, a, y_2, y_1, x);
jjmedina 0:a5367bd4e404 135 return y_0;
jjmedina 0:a5367bd4e404 136
jjmedina 0:a5367bd4e404 137 }
jjmedina 0:a5367bd4e404 138
jjmedina 0:a5367bd4e404 139 float isoFilter_Wd_xy_Ht (float y_2, float y_1, float x[3], int PasoTiempo )
jjmedina 0:a5367bd4e404 140 {
jjmedina 0:a5367bd4e404 141 float y_0 = 0.0;//valor filtrado
jjmedina 0:a5367bd4e404 142 float fs = (1000000.0/PasoTiempo);//frecuencia
jjmedina 0:a5367bd4e404 143 float a[3],b[3];
jjmedina 0:a5367bd4e404 144
jjmedina 0:a5367bd4e404 145 if ( fs == 320 )
jjmedina 0:a5367bd4e404 146 {
jjmedina 0:a5367bd4e404 147 //Ht,--Para fs=320.
jjmedina 0:a5367bd4e404 148 b[0] = 0.019408118133464;
jjmedina 0:a5367bd4e404 149 b[1] = 0.000747478314467;
jjmedina 0:a5367bd4e404 150 b[2] = -0.018660639818997;
jjmedina 0:a5367bd4e404 151 a[0] = 1.0;
jjmedina 0:a5367bd4e404 152 a[1] = -1.938078443446524;
jjmedina 0:a5367bd4e404 153 a[2] = 0.939573400075458;
jjmedina 0:a5367bd4e404 154 }else if ( fs == 1000 ) {
jjmedina 0:a5367bd4e404 155 //Ht,--Para fs=1000.
jjmedina 0:a5367bd4e404 156 b[0] = 0.006259983831114;
jjmedina 0:a5367bd4e404 157 b[1] = 0.000078174094539;
jjmedina 0:a5367bd4e404 158 b[2] = -0.006181809736574;
jjmedina 0:a5367bd4e404 159 a[0] = 1.000000000000000;
jjmedina 0:a5367bd4e404 160 a[1] = -1.980094773132050;
jjmedina 0:a5367bd4e404 161 a[2] = 0.980251121321129;
jjmedina 0:a5367bd4e404 162 }
jjmedina 0:a5367bd4e404 163 else
jjmedina 0:a5367bd4e404 164 {
jjmedina 0:a5367bd4e404 165 b[0] = 0.019408118133464;
jjmedina 0:a5367bd4e404 166 b[1] = 0.000747478314467;
jjmedina 0:a5367bd4e404 167 b[2] = -0.018660639818997;
jjmedina 0:a5367bd4e404 168 a[0] = 1.0;
jjmedina 0:a5367bd4e404 169 a[1] = -1.938078443446524;
jjmedina 0:a5367bd4e404 170 a[2] = 0.939573400075458;
jjmedina 0:a5367bd4e404 171 }
jjmedina 0:a5367bd4e404 172 y_0 = toFilter( b, a, y_2, y_1, x);
jjmedina 0:a5367bd4e404 173 return y_0;
jjmedina 0:a5367bd4e404 174
jjmedina 0:a5367bd4e404 175 }
jjmedina 0:a5367bd4e404 176
jjmedina 0:a5367bd4e404 177 float isoFilter_Wk_z_Hs (float y_2, float y_1, float x[3], int PasoTiempo )
jjmedina 0:a5367bd4e404 178 {
jjmedina 0:a5367bd4e404 179 float y_0 = 0.0;//valor filtrado
jjmedina 0:a5367bd4e404 180 float fs = (1000000.0/PasoTiempo);//frecuencia
jjmedina 0:a5367bd4e404 181 float a[3],b[3];
jjmedina 0:a5367bd4e404 182
jjmedina 0:a5367bd4e404 183 if ( fs == 320 )
jjmedina 0:a5367bd4e404 184 {
jjmedina 0:a5367bd4e404 185 //Hs,--Para fs=320.
jjmedina 0:a5367bd4e404 186 b[0] = 0.989285860229759;
jjmedina 0:a5367bd4e404 187 b[1] = -1.927181919272167;
jjmedina 0:a5367bd4e404 188 b[2] = 0.939983837287274;
jjmedina 0:a5367bd4e404 189 a[0] = 1.0;
jjmedina 0:a5367bd4e404 190 a[1] = -1.926140130231110 ;
jjmedina 0:a5367bd4e404 191 a[2] = 0.930311486558090;
jjmedina 0:a5367bd4e404 192 }else if ( fs == 1000 )
jjmedina 0:a5367bd4e404 193 {
jjmedina 0:a5367bd4e404 194 //Hs,--Para fs=1000.
jjmedina 0:a5367bd4e404 195 b[0] = 0.996601106679302;
jjmedina 0:a5367bd4e404 196 b[1] = -1.976807985278312;
jjmedina 0:a5367bd4e404 197 b[2] = 0.980426065701832;
jjmedina 0:a5367bd4e404 198 a[0] = 1.000000000000000;
jjmedina 0:a5367bd4e404 199 a[1] = -1.976698612205658 ;
jjmedina 0:a5367bd4e404 200 a[2] = 0.977136545453787;
jjmedina 0:a5367bd4e404 201 }
jjmedina 0:a5367bd4e404 202 else{
jjmedina 0:a5367bd4e404 203 b[0] = 0.989285860229759;
jjmedina 0:a5367bd4e404 204 b[1] = -1.927181919272167;
jjmedina 0:a5367bd4e404 205 b[2] = 0.939983837287274;
jjmedina 0:a5367bd4e404 206 a[0] = 1.0;
jjmedina 0:a5367bd4e404 207 a[1] = -1.926140130231110 ;
jjmedina 0:a5367bd4e404 208 a[2] = 0.930311486558090;
jjmedina 0:a5367bd4e404 209 }
jjmedina 0:a5367bd4e404 210 y_0 = toFilter( b, a, y_2, y_1, x);
jjmedina 0:a5367bd4e404 211 return y_0;
jjmedina 0:a5367bd4e404 212
jjmedina 0:a5367bd4e404 213 }
jjmedina 0:a5367bd4e404 214
jjmedina 0:a5367bd4e404 215 float toFilter ( float b[3], float a[3], float y_2, float y_1, float x[3] )
jjmedina 0:a5367bd4e404 216 {
jjmedina 0:a5367bd4e404 217 float data = b[0]*x[2] + b[1]*x[1] + b[2]*x[0] - a[1]*y_1 - a[2]*y_2;
jjmedina 0:a5367bd4e404 218
jjmedina 0:a5367bd4e404 219 return data;
jjmedina 0:a5367bd4e404 220 }
jjmedina 0:a5367bd4e404 221
jjmedina 0:a5367bd4e404 222 float toLowPass100HzO2_320 ( float y_2, float y_1, float x[3] )
jjmedina 0:a5367bd4e404 223 {
jjmedina 0:a5367bd4e404 224 #define NZEROS 2
jjmedina 0:a5367bd4e404 225 #define NPOLES 2
jjmedina 0:a5367bd4e404 226 #define GAIN1 2.391409984e+00
jjmedina 0:a5367bd4e404 227
jjmedina 0:a5367bd4e404 228 float xv[NZEROS+1], yv[NPOLES+1];
jjmedina 0:a5367bd4e404 229
jjmedina 0:a5367bd4e404 230
jjmedina 0:a5367bd4e404 231 xv[0] = x[0]/ GAIN1; xv[1] = x[1]/ GAIN1;
jjmedina 0:a5367bd4e404 232 xv[2] = x[2] / GAIN1;
jjmedina 0:a5367bd4e404 233 yv[0] = y_2; yv[1] = y_1;
jjmedina 0:a5367bd4e404 234 yv[2] = (xv[0] + xv[2]) + 2.0 * xv[1]
jjmedina 0:a5367bd4e404 235 + ( -0.2097153578 * yv[0]) + ( -0.4629380253 * yv[1]);
jjmedina 0:a5367bd4e404 236 return yv[2];
jjmedina 0:a5367bd4e404 237 }
jjmedina 0:a5367bd4e404 238
jjmedina 0:a5367bd4e404 239 float toLowPass100HzO2_1000 ( float y_2, float y_1, float x[3] )
jjmedina 0:a5367bd4e404 240 {
jjmedina 0:a5367bd4e404 241 #define NZEROS 2
jjmedina 0:a5367bd4e404 242 #define NPOLES 2
jjmedina 0:a5367bd4e404 243 #define GAIN11 1.482463775e+01
jjmedina 0:a5367bd4e404 244
jjmedina 0:a5367bd4e404 245 float xv[NZEROS+1], yv[NPOLES+1];
jjmedina 0:a5367bd4e404 246
jjmedina 0:a5367bd4e404 247
jjmedina 0:a5367bd4e404 248 xv[0] = x[0]/ GAIN11; xv[1] = x[1]/ GAIN11;
jjmedina 0:a5367bd4e404 249 xv[2] = x[2] / GAIN11;
jjmedina 0:a5367bd4e404 250 yv[0] = y_2; yv[1] = y_1;
jjmedina 0:a5367bd4e404 251 yv[2] = (xv[0] + xv[2]) + 2.0 * xv[1]
jjmedina 0:a5367bd4e404 252 + ( -0.4128015981 * yv[0]) + ( 1.1429805025 * yv[1]);
jjmedina 0:a5367bd4e404 253 return yv[2];
jjmedina 0:a5367bd4e404 254 }
jjmedina 0:a5367bd4e404 255
jjmedina 0:a5367bd4e404 256 float toHighPass04HzO2_320 ( float y_2, float y_1, float x[3] )
jjmedina 0:a5367bd4e404 257 {
jjmedina 0:a5367bd4e404 258 #define NZEROS 2
jjmedina 0:a5367bd4e404 259 #define NPOLES 2
jjmedina 0:a5367bd4e404 260 #define GAIN4 1.005569054e+00;
jjmedina 0:a5367bd4e404 261
jjmedina 0:a5367bd4e404 262 static float xv[NZEROS+1], yv[NPOLES+1];
jjmedina 0:a5367bd4e404 263
jjmedina 0:a5367bd4e404 264
jjmedina 0:a5367bd4e404 265 xv[0] = x[0]/ GAIN4; xv[1] = x[1]/ GAIN4;
jjmedina 0:a5367bd4e404 266 xv[2] = x[2] / GAIN4;
jjmedina 0:a5367bd4e404 267 // xv[2] = x[2];
jjmedina 0:a5367bd4e404 268 yv[0] = y_2; yv[1] = y_1;
jjmedina 0:a5367bd4e404 269 yv[2] = (xv[0] + xv[2]) - 2.0 * xv[1]
jjmedina 0:a5367bd4e404 270 + ( -0.9889542499 * yv[0]) + ( 1.9888929059 * yv[1]);
jjmedina 0:a5367bd4e404 271 return (yv[2]);
jjmedina 0:a5367bd4e404 272 }
jjmedina 0:a5367bd4e404 273
jjmedina 0:a5367bd4e404 274 float toHighPass04HzO2_1000 ( float y_2, float y_1, float x[3] )
jjmedina 0:a5367bd4e404 275 {
jjmedina 0:a5367bd4e404 276 #define NZEROS 2
jjmedina 0:a5367bd4e404 277 #define NPOLES 2
jjmedina 0:a5367bd4e404 278 #define GAIN44 1.001778733e+00;
jjmedina 0:a5367bd4e404 279
jjmedina 0:a5367bd4e404 280 static float xv[NZEROS+1], yv[NPOLES+1];
jjmedina 0:a5367bd4e404 281
jjmedina 0:a5367bd4e404 282
jjmedina 0:a5367bd4e404 283 xv[0] = x[0]/ GAIN44; xv[1] = x[1]/ GAIN44;
jjmedina 0:a5367bd4e404 284 xv[2] = x[2] / GAIN44;
jjmedina 0:a5367bd4e404 285 // xv[2] = x[2];
jjmedina 0:a5367bd4e404 286 yv[0] = y_2; yv[1] = y_1;
jjmedina 0:a5367bd4e404 287 yv[2] = (xv[0] + xv[2]) - 2.0 * xv[1]
jjmedina 0:a5367bd4e404 288 + ( -0.9964520027 * yv[0]) + ( 1.9964456974 * yv[1]);
jjmedina 0:a5367bd4e404 289 return (yv[2]);
jjmedina 0:a5367bd4e404 290 }
jjmedina 0:a5367bd4e404 291
jjmedina 0:a5367bd4e404 292 float toLowPass1HzO2_320 ( float y_2, float y_1, float x[3] )
jjmedina 0:a5367bd4e404 293 {
jjmedina 0:a5367bd4e404 294 #define NZEROS 2
jjmedina 0:a5367bd4e404 295 #define NPOLES 2
jjmedina 0:a5367bd4e404 296 #define GAIN6 1.051966853e+04
jjmedina 0:a5367bd4e404 297
jjmedina 0:a5367bd4e404 298 float xv[NZEROS+1], yv[NPOLES+1];
jjmedina 0:a5367bd4e404 299
jjmedina 0:a5367bd4e404 300
jjmedina 0:a5367bd4e404 301 xv[0] = x[0]/ GAIN6; xv[1] = x[1]/ GAIN6;
jjmedina 0:a5367bd4e404 302 xv[2] = x[2] / GAIN6;
jjmedina 0:a5367bd4e404 303 yv[0] = y_2; yv[1] = y_1;
jjmedina 0:a5367bd4e404 304 yv[2] = (xv[0] + xv[2]) + 2.0 * xv[1]
jjmedina 0:a5367bd4e404 305 + ( -0.9726139693 * yv[0]) + ( 1.9722337292 * yv[1]);
jjmedina 0:a5367bd4e404 306 return yv[2];
jjmedina 0:a5367bd4e404 307 }
jjmedina 0:a5367bd4e404 308
jjmedina 0:a5367bd4e404 309 float toLowPass1HzO2_1000 ( float y_2, float y_1, float x[3] )
jjmedina 0:a5367bd4e404 310 {
jjmedina 0:a5367bd4e404 311 #define NZEROS 2
jjmedina 0:a5367bd4e404 312 #define NPOLES 2
jjmedina 0:a5367bd4e404 313 #define GAIN66 1.017716736e+05
jjmedina 0:a5367bd4e404 314
jjmedina 0:a5367bd4e404 315 float xv[NZEROS+1], yv[NPOLES+1];
jjmedina 0:a5367bd4e404 316
jjmedina 0:a5367bd4e404 317
jjmedina 0:a5367bd4e404 318 xv[0] = x[0]/ GAIN66; xv[1] = x[1]/ GAIN66;
jjmedina 0:a5367bd4e404 319 xv[2] = x[2] / GAIN66;
jjmedina 0:a5367bd4e404 320 yv[0] = y_2; yv[1] = y_1;
jjmedina 0:a5367bd4e404 321 yv[2] = (xv[0] + xv[2]) + 2.0 * xv[1]
jjmedina 0:a5367bd4e404 322 + ( -0.9911535959 * yv[0]) + ( 1.9911142922 * yv[1]);
jjmedina 0:a5367bd4e404 323 return yv[2];
jjmedina 0:a5367bd4e404 324 }