![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
filter_func.cpp
- Committer:
- bluefish
- Date:
- 2018-04-20
- Revision:
- 6:a5f674c2f262
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; }