Diff: filter_func.cpp

Revision:

6:a5f674c2f262

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/filter_func.cpp	Fri Apr 20 18:14:15 2018 +0000
@@ -0,0 +1,92 @@
+#include "filter_func.h"
+
+// initialize complimentary filter
+void init_comp_filter( STCOMPFILTER* filter, float param ){
+    filter->_int        =   0.0f;
+    filter->_adj        =   0.0f;
+    filter->_adj_temp   =   0.0f;
+    filter->_param      =   param;
+    return;    
+}
+
+// process complimentary filter
+void proc_comp_filter( STCOMPFILTER* filter, float dt, float angle_raw, float angle_vec_raw, float* angle, float* angle_vec ){
+    if( filter->_param == 0.0f ){ return; }
+    filter->_adj_temp  =    filter->_adj;
+    filter->_int      +=    angle_vec_raw * dt;                              // integral gyro
+    filter->_adj       =    angle_raw - filter->_int;                        // 
+    filter->_adj       =    filter->_adj_temp + dt * ( filter->_adj - filter->_adj_temp ) / filter->_param;   // calc correction via LPF
+    
+    *angle             =    filter->_int + filter->_adj;
+    *angle_vec         =    angle_vec_raw;
+    return;    
+}
+
+// initialize BiQuad filter
+void init_biquad_filter( STBIQUADFILTER* filter, float freq_sample, float freq, float param, BIQUADTYPE filter_pattern ){
+    float omega;
+    float alpha;
+    
+    if( freq_sample == 0.0f ){ return; }
+    omega = 2.0f * 3.14159265f *  freq / freq_sample;
+    
+    switch( filter_pattern ){
+        case FILTER_LPF:
+            alpha           =   sinf(omega)/(2.0f*param);
+            filter->b[0]    =   (1.0f - cos(omega))/2.0f;
+            filter->b[1]    =    1.0f - cos(omega);
+            filter->b[2]    =   (1.0f - cos(omega))/2.0f;      
+            break;
+        case FILTER_HPF:
+            alpha           =   sinf(omega)/(2.0f*param);
+            filter->b[0]    =   (1.0f + cos(omega))/2.0f;
+            filter->b[1]    =   -1.0f - cos(omega);
+            filter->b[2]    =   (1.0f + cos(omega))/2.0f;      
+            break;          
+        case FILTER_BPF:
+            alpha           =   sinf(omega)*sinhf( 0.15051500f * param * omega / sinf(omega) );
+            filter->b[0]    =   alpha;
+            filter->b[1]    =   0.0f;
+            filter->b[2]    =  -alpha;
+            break;
+        case FILTER_NF:
+            alpha           =   sinf(omega)*sinhf( 0.15051500f * param * omega / sinf(omega) );                     
+            filter->b[0]    =   1.0f;
+            filter->b[1]    =  -2.0f * cosf(omega);
+            filter->b[2]    =   1.0f;     
+            break;
+        default:
+                break;
+    }
+    filter->a[0]    =   1.0f + alpha;
+    filter->a[1]    =  -2.0f * cosf(omega);
+    filter->a[2]    =   1.0f - alpha;
+    
+    filter->in[0]   =   0.0f;
+    filter->in[1]   =   0.0f;
+    filter->out[0]  =   0.0f;
+    filter->out[1]  =   0.0f;
+
+    return;    
+}
+
+// process BiQuad Filter
+float proc_biquad_filter( STBIQUADFILTER* filter, float input ){
+    float output;
+    if( filter->a[0] == 0.0f ){ return 0.0f; }
+    
+    output = 
+             (filter->b[0]/filter->a[0]) * input
+        + (filter->b[1]/filter->a[0] )* filter->in[0]
+        + (filter->b[2]/filter->a[0]) * filter->in[1]
+        - (filter->a[1]/filter->a[0]) * filter->out[0] 
+        - (filter->a[2]/filter->a[0]) * filter->out[1];
+
+    filter->in[1]       =   filter->in[0];
+    filter->in[0]       =   input;
+
+    filter->out[1]  =   filter->out[0];
+    filter->out[0]  =   output;
+
+    return output;  
+}