Diff: filters.cpp

Revision:

11:5cb7ae8bd831

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/filters.cpp	Thu May 07 03:52:31 2020 +0000
@@ -0,0 +1,225 @@
+
+/* File inclusion */
+#include "stdint.h"
+#include "project_defines.h"
+
+
+float RC_LPF_01(float raw_value, uint8_t restart){
+    
+    static float lpf_prev_out = 0.0;
+    float lpf_out, sample_period_sec, div;
+    
+    if(restart){
+        lpf_prev_out = 0.0;
+        lpf_out = 0.0;    
+    }else{
+        sample_period_sec = 0.001 * ((float)PRESSURE_SENSOR_READ_PERIOD_MS);
+        div = sample_period_sec + P1_LPF_RC_CONSTANT_SEC;
+        
+        lpf_out = (raw_value * (sample_period_sec / div)) +  (lpf_prev_out * (P1_LPF_RC_CONSTANT_SEC / div));
+        lpf_prev_out = lpf_out;        
+    }
+    
+    return lpf_out;
+
+}
+
+
+float RC_HPF_01(float raw_value, uint8_t restart){
+        
+    
+    static float prev_raw_value = 0.0, hpf_prev_out = 0.0;
+    float hpf_out, sample_period_sec, div;
+ 
+    if(restart){
+        prev_raw_value = 0.0;
+        hpf_prev_out = 0.0;
+        hpf_out = 0.0;    
+    }else{ 
+        sample_period_sec = 0.001 * ((float)PRESSURE_SENSOR_READ_PERIOD_MS);
+        div = sample_period_sec + F_HPF_RC_CONSTANT_SEC;
+        
+        hpf_out = (raw_value - prev_raw_value + hpf_prev_out) * (F_HPF_RC_CONSTANT_SEC / div);
+        hpf_prev_out = hpf_out;
+        prev_raw_value = raw_value;
+    }   
+    
+    return hpf_out;
+
+}
+
+
+
+float RC_LPF_02(float raw_value, uint8_t restart){
+ 
+    static float lpf_prev_out = 0;
+    float lpf_out, sample_period_sec, div;
+    
+    if(restart){
+        lpf_prev_out = 0.0;
+        lpf_out = 0.0;    
+    }else{
+        sample_period_sec = 0.001 * ((float)PRESSURE_SENSOR_READ_PERIOD_MS);
+        div = sample_period_sec + P2_LPF_RC_CONSTANT_SEC;
+        
+        lpf_out = (raw_value * (sample_period_sec / div)) +  (lpf_prev_out * (P2_LPF_RC_CONSTANT_SEC / div));
+        lpf_prev_out = lpf_out;
+    }
+    
+    return lpf_out;    
+
+}
+
+
+
+float Pressure_01_Cheby2_LPF(float raw_value, uint8_t restart){
+    
+   const float b[PRESSURE_01_LPF_N] = {0.20077,   0.47142,   0.47142,   0.20077};                        
+   const float a[PRESSURE_01_LPF_N] = {1.0000000,  -0.0398709,   0.3747845,   0.0094542};
+   
+   static float x[PRESSURE_01_LPF_N];
+   static float y[PRESSURE_01_LPF_N];
+   
+   int16_t i;  
+
+    if(restart){
+        for(i = 0; i < PRESSURE_01_LPF_N; i++){
+           x[i] = 0.0; 
+           y[i] = 0.0; 
+        }
+    }else{
+        
+        /* Update recorded input and output values */
+        for(i = PRESSURE_01_LPF_N - 1; i > 0; i--){
+           x[i] = x[i - 1]; 
+           y[i] = y[i - 1]; 
+        }
+   
+        /* Update current input and output values */
+        x[0] = raw_value;
+        y[0] = b[0] * x[0];
+        for(i = 1; i < PRESSURE_01_LPF_N; i++){
+           y[0] += (b[i] * x[i]) - (a[i] * y[i]); 
+        } 
+        
+    }
+
+    return y[0];
+}
+
+
+
+float Pressure_02_Cheby2_LPF(float raw_value, uint8_t restart){
+    
+   const float b[PRESSURE_02_LPF_N] = {0.20077,   0.47142,   0.47142,   0.20077};                        
+   const float a[PRESSURE_02_LPF_N] = {1.0000000,  -0.0398709,   0.3747845,   0.0094542};
+   
+   static float x[PRESSURE_02_LPF_N];
+   static float y[PRESSURE_02_LPF_N];
+   
+   int16_t i;  
+
+    if(restart){
+        for(i = 0; i < PRESSURE_02_LPF_N; i++){
+           x[i] = 0.0; 
+           y[i] = 0.0; 
+        }
+    }else{
+        
+        /* Update recorded input and output values */
+        for(i = PRESSURE_02_LPF_N - 1; i > 0; i--){
+           x[i] = x[i - 1]; 
+           y[i] = y[i - 1]; 
+        }
+   
+        /* Update current input and output values */
+        x[0] = raw_value;
+        y[0] = b[0] * x[0];
+        for(i = 1; i < PRESSURE_02_LPF_N; i++){
+           y[0] += (b[i] * x[i]) - (a[i] * y[i]); 
+        } 
+        
+    }
+
+    return y[0];
+}
+
+
+
+
+
+float Flow_Cheby2_HPF(float raw_value, uint8_t restart){
+    
+   const float b[FLOW_HPF_N] = {0.97692,  -1.95383,   0.97692};                        
+   const float a[FLOW_HPF_N] = {1.00000,  -1.95330,   0.95438};
+   
+   static float x[FLOW_HPF_N];
+   static float y[FLOW_HPF_N];
+   
+   int16_t i;  
+
+    if(restart){
+        for(i = 0; i < FLOW_HPF_N; i++){
+           x[i] = 0.0; 
+           y[i] = 0.0; 
+        }
+    }else{
+        
+        /* Update recorded input and output values */
+        for(i = FLOW_HPF_N - 1; i > 0; i--){
+           x[i] = x[i - 1]; 
+           y[i] = y[i - 1]; 
+        }
+   
+        /* Update current input and output values */
+        x[0] = raw_value;
+        y[0] = b[0] * x[0];
+        for(i = 1; i < FLOW_HPF_N; i++){
+           y[0] += (b[i] * x[i]) - (a[i] * y[i]); 
+        } 
+        
+    }
+
+    return y[0];
+}
+
+
+float Flow_FIR_LPF(float raw_value, uint8_t restart){
+
+    const float b[FLOW_LPF_N] = {-0.00204445,   0.00170497,  -0.00013853,  -0.00430077,   0.01165634,
+                                 -0.01836386,   0.01729751,  -0.00064490,  -0.03547909,   0.08723292,
+                                 -0.14261912,   0.18533572,  0.80072653,    0.18533572,  -0.14261912,
+                                  0.08723292,  -0.03547909,  -0.00064490,   0.01729751,  -0.01836386,
+                                  0.01165634,  -0.00430077,  -0.00013853,   0.00170497,  -0.00204445
+                                 };
+                                 
+    static float x[FLOW_LPF_N];
+    float y; 
+        
+    int16_t i; 
+    
+     if(restart){
+        for(i = 0; i < FLOW_LPF_N; i++){
+           x[i] = 0.0; 
+        }
+    }else{
+        
+        /* Update recorded input values */
+        for(i = FLOW_LPF_N - 1; i > 0; i--){
+           x[i] = x[i - 1]; 
+        }
+   
+        /* Update current input and output values */
+        x[0] = raw_value;
+        y = 0;
+        for(i = 0; i < FLOW_LPF_N; i++){
+           y += (b[i] * x[i]); 
+        } 
+        
+    }
+
+    return y;   
+    
+}
+
+