filter_func.cpp

00001 #include "filter_func.h"
00002 
00003 // initialize complimentary filter
00004 void init_comp_filter( STCOMPFILTER* filter, float param ){
00005     filter->_int        =   0.0f;
00006     filter->_adj        =   0.0f;
00007     filter->_adj_temp   =   0.0f;
00008     filter->_param      =   param;
00009     return;    
00010 }
00011 
00012 // process complimentary filter
00013 void proc_comp_filter( STCOMPFILTER* filter, float dt, float angle_raw, float angle_vec_raw, float* angle, float* angle_vec ){
00014     if( filter->_param == 0.0f ){ return; }
00015     filter->_adj_temp  =    filter->_adj;
00016     filter->_int      +=    angle_vec_raw * dt;                              // integral gyro
00017     filter->_adj       =    angle_raw - filter->_int;                        // 
00018     filter->_adj       =    filter->_adj_temp + dt * ( filter->_adj - filter->_adj_temp ) / filter->_param;   // calc correction via LPF
00019     
00020     *angle             =    filter->_int + filter->_adj;
00021     *angle_vec         =    angle_vec_raw;
00022     return;    
00023 }
00024 
00025 // initialize BiQuad filter
00026 void init_biquad_filter( STBIQUADFILTER* filter, float freq_sample, float freq, float param, BIQUADTYPE filter_pattern ){
00027     float omega;
00028     float alpha;
00029     
00030     if( freq_sample == 0.0f ){ return; }
00031     omega = 2.0f * 3.14159265f *  freq / freq_sample;
00032     
00033     switch( filter_pattern ){
00034         case FILTER_LPF:
00035             alpha           =   sinf(omega)/(2.0f*param);
00036             filter->b[0]    =   (1.0f - cos(omega))/2.0f;
00037             filter->b[1]    =    1.0f - cos(omega);
00038             filter->b[2]    =   (1.0f - cos(omega))/2.0f;      
00039             break;
00040         case FILTER_HPF:
00041             alpha           =   sinf(omega)/(2.0f*param);
00042             filter->b[0]    =   (1.0f + cos(omega))/2.0f;
00043             filter->b[1]    =   -1.0f - cos(omega);
00044             filter->b[2]    =   (1.0f + cos(omega))/2.0f;      
00045             break;          
00046         case FILTER_BPF:
00047             alpha           =   sinf(omega)*sinhf( 0.15051500f * param * omega / sinf(omega) );
00048             filter->b[0]    =   alpha;
00049             filter->b[1]    =   0.0f;
00050             filter->b[2]    =  -alpha;
00051             break;
00052         case FILTER_NF:
00053             alpha           =   sinf(omega)*sinhf( 0.15051500f * param * omega / sinf(omega) );                     
00054             filter->b[0]    =   1.0f;
00055             filter->b[1]    =  -2.0f * cosf(omega);
00056             filter->b[2]    =   1.0f;     
00057             break;
00058         default:
00059                 break;
00060     }
00061     filter->a[0]    =   1.0f + alpha;
00062     filter->a[1]    =  -2.0f * cosf(omega);
00063     filter->a[2]    =   1.0f - alpha;
00064     
00065     filter->in[0]   =   0.0f;
00066     filter->in[1]   =   0.0f;
00067     filter->out[0]  =   0.0f;
00068     filter->out[1]  =   0.0f;
00069 
00070     return;    
00071 }
00072 
00073 // process BiQuad Filter
00074 float proc_biquad_filter( STBIQUADFILTER* filter, float input ){
00075     float output;
00076     if( filter->a[0] == 0.0f ){ return 0.0f; }
00077     
00078     output = 
00079              (filter->b[0]/filter->a[0]) * input
00080         + (filter->b[1]/filter->a[0] )* filter->in[0]
00081         + (filter->b[2]/filter->a[0]) * filter->in[1]
00082         - (filter->a[1]/filter->a[0]) * filter->out[0] 
00083         - (filter->a[2]/filter->a[0]) * filter->out[1];
00084 
00085     filter->in[1]       =   filter->in[0];
00086     filter->in[0]       =   input;
00087 
00088     filter->out[1]  =   filter->out[0];
00089     filter->out[0]  =   output;
00090 
00091     return output;  
00092 }