Esta versión v6 pasa a ser el nuevo master. Funciona correctamente
Dependencies: ADXL345 Display1602 MSCFileSystem SDFileSystem mbed FATFileSystem
filters.cpp@0:a5367bd4e404, 2014-05-20 (annotated)
- Committer:
- jjmedina
- Date:
- Tue May 20 15:11:16 2014 +0000
- Revision:
- 0:a5367bd4e404
Copia master que funciona. Ha habido una confusi?n entre las versiones 4 y 5 y, con ?sta, se solucoina.
Who changed what in which revision?
User | Revision | Line number | New 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 | } |