Miguel Angel Torres Lazaro / Mbed 2 deprecated Ventilador_Experimentos_03

pachas

Dependencies:   mbed QEI FastPWM

filters.cpp@12:3bc2465b034a, 2020-05-15 (annotated)

Committer:
miguelangel_2511
Date:
Fri May 15 20:29:30 2020 +0000
Revision:
12:3bc2465b034a
Parent:
11:5cb7ae8bd831 
hola papu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
miguelangel_2511 11:5cb7ae8bd831 1
miguelangel_2511 11:5cb7ae8bd831 2 /* File inclusion */
miguelangel_2511 11:5cb7ae8bd831 3 #include "stdint.h"
miguelangel_2511 11:5cb7ae8bd831 4 #include "project_defines.h"
miguelangel_2511 11:5cb7ae8bd831 5
miguelangel_2511 11:5cb7ae8bd831 6
miguelangel_2511 11:5cb7ae8bd831 7 float RC_LPF_01(float raw_value, uint8_t restart){
miguelangel_2511 11:5cb7ae8bd831 8
miguelangel_2511 11:5cb7ae8bd831 9 static float lpf_prev_out = 0.0;
miguelangel_2511 11:5cb7ae8bd831 10 float lpf_out, sample_period_sec, div;
miguelangel_2511 11:5cb7ae8bd831 11
miguelangel_2511 11:5cb7ae8bd831 12 if(restart){
miguelangel_2511 11:5cb7ae8bd831 13 lpf_prev_out = 0.0;
miguelangel_2511 11:5cb7ae8bd831 14 lpf_out = 0.0;
miguelangel_2511 11:5cb7ae8bd831 15 }else{
miguelangel_2511 11:5cb7ae8bd831 16 sample_period_sec = 0.001 * ((float)PRESSURE_SENSOR_READ_PERIOD_MS);
miguelangel_2511 11:5cb7ae8bd831 17 div = sample_period_sec + P1_LPF_RC_CONSTANT_SEC;
miguelangel_2511 11:5cb7ae8bd831 18
miguelangel_2511 11:5cb7ae8bd831 19 lpf_out = (raw_value * (sample_period_sec / div)) + (lpf_prev_out * (P1_LPF_RC_CONSTANT_SEC / div));
miguelangel_2511 11:5cb7ae8bd831 20 lpf_prev_out = lpf_out;
miguelangel_2511 11:5cb7ae8bd831 21 }
miguelangel_2511 11:5cb7ae8bd831 22
miguelangel_2511 11:5cb7ae8bd831 23 return lpf_out;
miguelangel_2511 11:5cb7ae8bd831 24
miguelangel_2511 11:5cb7ae8bd831 25 }
miguelangel_2511 11:5cb7ae8bd831 26
miguelangel_2511 11:5cb7ae8bd831 27
miguelangel_2511 11:5cb7ae8bd831 28 float RC_HPF_01(float raw_value, uint8_t restart){
miguelangel_2511 11:5cb7ae8bd831 29
miguelangel_2511 11:5cb7ae8bd831 30
miguelangel_2511 11:5cb7ae8bd831 31 static float prev_raw_value = 0.0, hpf_prev_out = 0.0;
miguelangel_2511 11:5cb7ae8bd831 32 float hpf_out, sample_period_sec, div;
miguelangel_2511 11:5cb7ae8bd831 33
miguelangel_2511 11:5cb7ae8bd831 34 if(restart){
miguelangel_2511 11:5cb7ae8bd831 35 prev_raw_value = 0.0;
miguelangel_2511 11:5cb7ae8bd831 36 hpf_prev_out = 0.0;
miguelangel_2511 11:5cb7ae8bd831 37 hpf_out = 0.0;
miguelangel_2511 11:5cb7ae8bd831 38 }else{
miguelangel_2511 11:5cb7ae8bd831 39 sample_period_sec = 0.001 * ((float)PRESSURE_SENSOR_READ_PERIOD_MS);
miguelangel_2511 11:5cb7ae8bd831 40 div = sample_period_sec + F_HPF_RC_CONSTANT_SEC;
miguelangel_2511 11:5cb7ae8bd831 41
miguelangel_2511 11:5cb7ae8bd831 42 hpf_out = (raw_value - prev_raw_value + hpf_prev_out) * (F_HPF_RC_CONSTANT_SEC / div);
miguelangel_2511 11:5cb7ae8bd831 43 hpf_prev_out = hpf_out;
miguelangel_2511 11:5cb7ae8bd831 44 prev_raw_value = raw_value;
miguelangel_2511 11:5cb7ae8bd831 45 }
miguelangel_2511 11:5cb7ae8bd831 46
miguelangel_2511 11:5cb7ae8bd831 47 return hpf_out;
miguelangel_2511 11:5cb7ae8bd831 48
miguelangel_2511 11:5cb7ae8bd831 49 }
miguelangel_2511 11:5cb7ae8bd831 50
miguelangel_2511 11:5cb7ae8bd831 51
miguelangel_2511 11:5cb7ae8bd831 52
miguelangel_2511 11:5cb7ae8bd831 53 float RC_LPF_02(float raw_value, uint8_t restart){
miguelangel_2511 11:5cb7ae8bd831 54
miguelangel_2511 11:5cb7ae8bd831 55 static float lpf_prev_out = 0;
miguelangel_2511 11:5cb7ae8bd831 56 float lpf_out, sample_period_sec, div;
miguelangel_2511 11:5cb7ae8bd831 57
miguelangel_2511 11:5cb7ae8bd831 58 if(restart){
miguelangel_2511 11:5cb7ae8bd831 59 lpf_prev_out = 0.0;
miguelangel_2511 11:5cb7ae8bd831 60 lpf_out = 0.0;
miguelangel_2511 11:5cb7ae8bd831 61 }else{
miguelangel_2511 11:5cb7ae8bd831 62 sample_period_sec = 0.001 * ((float)PRESSURE_SENSOR_READ_PERIOD_MS);
miguelangel_2511 11:5cb7ae8bd831 63 div = sample_period_sec + P2_LPF_RC_CONSTANT_SEC;
miguelangel_2511 11:5cb7ae8bd831 64
miguelangel_2511 11:5cb7ae8bd831 65 lpf_out = (raw_value * (sample_period_sec / div)) + (lpf_prev_out * (P2_LPF_RC_CONSTANT_SEC / div));
miguelangel_2511 11:5cb7ae8bd831 66 lpf_prev_out = lpf_out;
miguelangel_2511 11:5cb7ae8bd831 67 }
miguelangel_2511 11:5cb7ae8bd831 68
miguelangel_2511 11:5cb7ae8bd831 69 return lpf_out;
miguelangel_2511 11:5cb7ae8bd831 70
miguelangel_2511 11:5cb7ae8bd831 71 }
miguelangel_2511 11:5cb7ae8bd831 72
miguelangel_2511 11:5cb7ae8bd831 73
miguelangel_2511 11:5cb7ae8bd831 74
miguelangel_2511 11:5cb7ae8bd831 75 float Pressure_01_Cheby2_LPF(float raw_value, uint8_t restart){
miguelangel_2511 11:5cb7ae8bd831 76
miguelangel_2511 11:5cb7ae8bd831 77 const float b[PRESSURE_01_LPF_N] = {0.20077, 0.47142, 0.47142, 0.20077};
miguelangel_2511 11:5cb7ae8bd831 78 const float a[PRESSURE_01_LPF_N] = {1.0000000, -0.0398709, 0.3747845, 0.0094542};
miguelangel_2511 11:5cb7ae8bd831 79
miguelangel_2511 11:5cb7ae8bd831 80 static float x[PRESSURE_01_LPF_N];
miguelangel_2511 11:5cb7ae8bd831 81 static float y[PRESSURE_01_LPF_N];
miguelangel_2511 11:5cb7ae8bd831 82
miguelangel_2511 11:5cb7ae8bd831 83 int16_t i;
miguelangel_2511 11:5cb7ae8bd831 84
miguelangel_2511 11:5cb7ae8bd831 85 if(restart){
miguelangel_2511 11:5cb7ae8bd831 86 for(i = 0; i < PRESSURE_01_LPF_N; i++){
miguelangel_2511 11:5cb7ae8bd831 87 x[i] = 0.0;
miguelangel_2511 11:5cb7ae8bd831 88 y[i] = 0.0;
miguelangel_2511 11:5cb7ae8bd831 89 }
miguelangel_2511 11:5cb7ae8bd831 90 }else{
miguelangel_2511 11:5cb7ae8bd831 91
miguelangel_2511 11:5cb7ae8bd831 92 /* Update recorded input and output values */
miguelangel_2511 11:5cb7ae8bd831 93 for(i = PRESSURE_01_LPF_N - 1; i > 0; i--){
miguelangel_2511 11:5cb7ae8bd831 94 x[i] = x[i - 1];
miguelangel_2511 11:5cb7ae8bd831 95 y[i] = y[i - 1];
miguelangel_2511 11:5cb7ae8bd831 96 }
miguelangel_2511 11:5cb7ae8bd831 97
miguelangel_2511 11:5cb7ae8bd831 98 /* Update current input and output values */
miguelangel_2511 11:5cb7ae8bd831 99 x[0] = raw_value;
miguelangel_2511 11:5cb7ae8bd831 100 y[0] = b[0] * x[0];
miguelangel_2511 11:5cb7ae8bd831 101 for(i = 1; i < PRESSURE_01_LPF_N; i++){
miguelangel_2511 11:5cb7ae8bd831 102 y[0] += (b[i] * x[i]) - (a[i] * y[i]);
miguelangel_2511 11:5cb7ae8bd831 103 }
miguelangel_2511 11:5cb7ae8bd831 104
miguelangel_2511 11:5cb7ae8bd831 105 }
miguelangel_2511 11:5cb7ae8bd831 106
miguelangel_2511 11:5cb7ae8bd831 107 return y[0];
miguelangel_2511 11:5cb7ae8bd831 108 }
miguelangel_2511 11:5cb7ae8bd831 109
miguelangel_2511 11:5cb7ae8bd831 110
miguelangel_2511 11:5cb7ae8bd831 111
miguelangel_2511 11:5cb7ae8bd831 112 float Pressure_02_Cheby2_LPF(float raw_value, uint8_t restart){
miguelangel_2511 11:5cb7ae8bd831 113
miguelangel_2511 11:5cb7ae8bd831 114 const float b[PRESSURE_02_LPF_N] = {0.20077, 0.47142, 0.47142, 0.20077};
miguelangel_2511 11:5cb7ae8bd831 115 const float a[PRESSURE_02_LPF_N] = {1.0000000, -0.0398709, 0.3747845, 0.0094542};
miguelangel_2511 11:5cb7ae8bd831 116
miguelangel_2511 11:5cb7ae8bd831 117 static float x[PRESSURE_02_LPF_N];
miguelangel_2511 11:5cb7ae8bd831 118 static float y[PRESSURE_02_LPF_N];
miguelangel_2511 11:5cb7ae8bd831 119
miguelangel_2511 11:5cb7ae8bd831 120 int16_t i;
miguelangel_2511 11:5cb7ae8bd831 121
miguelangel_2511 11:5cb7ae8bd831 122 if(restart){
miguelangel_2511 11:5cb7ae8bd831 123 for(i = 0; i < PRESSURE_02_LPF_N; i++){
miguelangel_2511 11:5cb7ae8bd831 124 x[i] = 0.0;
miguelangel_2511 11:5cb7ae8bd831 125 y[i] = 0.0;
miguelangel_2511 11:5cb7ae8bd831 126 }
miguelangel_2511 11:5cb7ae8bd831 127 }else{
miguelangel_2511 11:5cb7ae8bd831 128
miguelangel_2511 11:5cb7ae8bd831 129 /* Update recorded input and output values */
miguelangel_2511 11:5cb7ae8bd831 130 for(i = PRESSURE_02_LPF_N - 1; i > 0; i--){
miguelangel_2511 11:5cb7ae8bd831 131 x[i] = x[i - 1];
miguelangel_2511 11:5cb7ae8bd831 132 y[i] = y[i - 1];
miguelangel_2511 11:5cb7ae8bd831 133 }
miguelangel_2511 11:5cb7ae8bd831 134
miguelangel_2511 11:5cb7ae8bd831 135 /* Update current input and output values */
miguelangel_2511 11:5cb7ae8bd831 136 x[0] = raw_value;
miguelangel_2511 11:5cb7ae8bd831 137 y[0] = b[0] * x[0];
miguelangel_2511 11:5cb7ae8bd831 138 for(i = 1; i < PRESSURE_02_LPF_N; i++){
miguelangel_2511 11:5cb7ae8bd831 139 y[0] += (b[i] * x[i]) - (a[i] * y[i]);
miguelangel_2511 11:5cb7ae8bd831 140 }
miguelangel_2511 11:5cb7ae8bd831 141
miguelangel_2511 11:5cb7ae8bd831 142 }
miguelangel_2511 11:5cb7ae8bd831 143
miguelangel_2511 11:5cb7ae8bd831 144 return y[0];
miguelangel_2511 11:5cb7ae8bd831 145 }
miguelangel_2511 11:5cb7ae8bd831 146
miguelangel_2511 11:5cb7ae8bd831 147
miguelangel_2511 11:5cb7ae8bd831 148
miguelangel_2511 11:5cb7ae8bd831 149
miguelangel_2511 11:5cb7ae8bd831 150
miguelangel_2511 11:5cb7ae8bd831 151 float Flow_Cheby2_HPF(float raw_value, uint8_t restart){
miguelangel_2511 11:5cb7ae8bd831 152
miguelangel_2511 11:5cb7ae8bd831 153 const float b[FLOW_HPF_N] = {0.97692, -1.95383, 0.97692};
miguelangel_2511 11:5cb7ae8bd831 154 const float a[FLOW_HPF_N] = {1.00000, -1.95330, 0.95438};
miguelangel_2511 11:5cb7ae8bd831 155
miguelangel_2511 11:5cb7ae8bd831 156 static float x[FLOW_HPF_N];
miguelangel_2511 11:5cb7ae8bd831 157 static float y[FLOW_HPF_N];
miguelangel_2511 11:5cb7ae8bd831 158
miguelangel_2511 11:5cb7ae8bd831 159 int16_t i;
miguelangel_2511 11:5cb7ae8bd831 160
miguelangel_2511 11:5cb7ae8bd831 161 if(restart){
miguelangel_2511 11:5cb7ae8bd831 162 for(i = 0; i < FLOW_HPF_N; i++){
miguelangel_2511 11:5cb7ae8bd831 163 x[i] = 0.0;
miguelangel_2511 11:5cb7ae8bd831 164 y[i] = 0.0;
miguelangel_2511 11:5cb7ae8bd831 165 }
miguelangel_2511 11:5cb7ae8bd831 166 }else{
miguelangel_2511 11:5cb7ae8bd831 167
miguelangel_2511 11:5cb7ae8bd831 168 /* Update recorded input and output values */
miguelangel_2511 11:5cb7ae8bd831 169 for(i = FLOW_HPF_N - 1; i > 0; i--){
miguelangel_2511 11:5cb7ae8bd831 170 x[i] = x[i - 1];
miguelangel_2511 11:5cb7ae8bd831 171 y[i] = y[i - 1];
miguelangel_2511 11:5cb7ae8bd831 172 }
miguelangel_2511 11:5cb7ae8bd831 173
miguelangel_2511 11:5cb7ae8bd831 174 /* Update current input and output values */
miguelangel_2511 11:5cb7ae8bd831 175 x[0] = raw_value;
miguelangel_2511 11:5cb7ae8bd831 176 y[0] = b[0] * x[0];
miguelangel_2511 11:5cb7ae8bd831 177 for(i = 1; i < FLOW_HPF_N; i++){
miguelangel_2511 11:5cb7ae8bd831 178 y[0] += (b[i] * x[i]) - (a[i] * y[i]);
miguelangel_2511 11:5cb7ae8bd831 179 }
miguelangel_2511 11:5cb7ae8bd831 180
miguelangel_2511 11:5cb7ae8bd831 181 }
miguelangel_2511 11:5cb7ae8bd831 182
miguelangel_2511 11:5cb7ae8bd831 183 return y[0];
miguelangel_2511 11:5cb7ae8bd831 184 }
miguelangel_2511 11:5cb7ae8bd831 185
miguelangel_2511 11:5cb7ae8bd831 186
miguelangel_2511 11:5cb7ae8bd831 187 float Flow_FIR_LPF(float raw_value, uint8_t restart){
miguelangel_2511 11:5cb7ae8bd831 188
miguelangel_2511 11:5cb7ae8bd831 189 const float b[FLOW_LPF_N] = {-0.00204445, 0.00170497, -0.00013853, -0.00430077, 0.01165634,
miguelangel_2511 11:5cb7ae8bd831 190 -0.01836386, 0.01729751, -0.00064490, -0.03547909, 0.08723292,
miguelangel_2511 11:5cb7ae8bd831 191 -0.14261912, 0.18533572, 0.80072653, 0.18533572, -0.14261912,
miguelangel_2511 11:5cb7ae8bd831 192 0.08723292, -0.03547909, -0.00064490, 0.01729751, -0.01836386,
miguelangel_2511 11:5cb7ae8bd831 193 0.01165634, -0.00430077, -0.00013853, 0.00170497, -0.00204445
miguelangel_2511 11:5cb7ae8bd831 194 };
miguelangel_2511 11:5cb7ae8bd831 195
miguelangel_2511 11:5cb7ae8bd831 196 static float x[FLOW_LPF_N];
miguelangel_2511 11:5cb7ae8bd831 197 float y;
miguelangel_2511 11:5cb7ae8bd831 198
miguelangel_2511 11:5cb7ae8bd831 199 int16_t i;
miguelangel_2511 11:5cb7ae8bd831 200
miguelangel_2511 11:5cb7ae8bd831 201 if(restart){
miguelangel_2511 11:5cb7ae8bd831 202 for(i = 0; i < FLOW_LPF_N; i++){
miguelangel_2511 11:5cb7ae8bd831 203 x[i] = 0.0;
miguelangel_2511 11:5cb7ae8bd831 204 }
miguelangel_2511 11:5cb7ae8bd831 205 }else{
miguelangel_2511 11:5cb7ae8bd831 206
miguelangel_2511 11:5cb7ae8bd831 207 /* Update recorded input values */
miguelangel_2511 11:5cb7ae8bd831 208 for(i = FLOW_LPF_N - 1; i > 0; i--){
miguelangel_2511 11:5cb7ae8bd831 209 x[i] = x[i - 1];
miguelangel_2511 11:5cb7ae8bd831 210 }
miguelangel_2511 11:5cb7ae8bd831 211
miguelangel_2511 11:5cb7ae8bd831 212 /* Update current input and output values */
miguelangel_2511 11:5cb7ae8bd831 213 x[0] = raw_value;
miguelangel_2511 11:5cb7ae8bd831 214 y = 0;
miguelangel_2511 11:5cb7ae8bd831 215 for(i = 0; i < FLOW_LPF_N; i++){
miguelangel_2511 11:5cb7ae8bd831 216 y += (b[i] * x[i]);
miguelangel_2511 11:5cb7ae8bd831 217 }
miguelangel_2511 11:5cb7ae8bd831 218
miguelangel_2511 11:5cb7ae8bd831 219 }
miguelangel_2511 11:5cb7ae8bd831 220
miguelangel_2511 11:5cb7ae8bd831 221 return y;
miguelangel_2511 11:5cb7ae8bd831 222
miguelangel_2511 11:5cb7ae8bd831 223 }
miguelangel_2511 11:5cb7ae8bd831 224
miguelangel_2511 11:5cb7ae8bd831 225