File content as of revision 6:a5f674c2f262:

#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;  
}