Guide to gyro and accelerometer with Arduino including Kalman filtering

Using Arduino Sensors
420

Lauszus:
@brianb00
I haven't got much experience with magnetometers, sorry.

@doantrungnghia05
The Kalman I filter I wrote can't be used without an accelerometer. The easiest thing would be to use a digital high-pass filter.

Thank Lauszus, here is my board, i bought it because in the internet many people use this board and they also buid their quad fly, but they just assembly these things like: motors, rf-controller, ...together without writing any code for the uController.

1.Kk_board datasheet: kkmulticopter.com

2.The angular rate sensor use in kk board: (ENC series) http://www.murata.com/products/catalog/pdf/s42e.pdf

My code is C++ for atmega168pa and i know this forum is about adruino, but i think my code is very cleary and easy to understand. Here is it:

/************************************************************************/
/*  MAIN FUNTION                                                       */
/************************************************************************/
int main(void)
{
	CLKPR=0x80;
	CLKPR=0x00;
	
	//----------- system init --------------------
	sei();
	usart_init();
	adc_sensor_init();
	led_init();
	motor_control_init();
	motor_start();
	DDRB|=(1<<PINB3);// measure loop time for cheking
	//-----------------------------------------------------------
	//---------- update the zero value ---------
	adc_sensor_get_value();
	roll_zero=roll_adc;//~=824
	yaw_zero=yaw_adc;//~=816
	pitch_zero=pitch_adc;//~=846
	roll_angle=360;
	yaw_angle=360;
	pitch_angle=360;
	//----------------------------------------------------------
	timer2_start();//start measure system time by timer2
	_delay_ms(2000);
	led_on();
	 while(1)
    {
		//--------------   get value in 3 sensors -----------------------
		PORTB^=(1<<PINB3);// toogle this pin per main loop
		adc_sensor_get_value();//
		
		//-------- just process roll gyro--------------------------------------------------------------
		if(roll_adc<roll_zero)
		{
			roll_rate=(roll_zero-roll_adc)/sensitivity;//tinh toc do goc theo phuong roll [degree/s]
			roll_angle-=roll_rate*dtime/1000;//tra ve gia tri degree theo phuong roll [degree]	
		}
		
		if(roll_adc>roll_zero)
		{
			roll_rate=(roll_adc-roll_zero)/sensitivity;//tinh toc do goc theo phuong roll [degree/s]
			roll_angle+=roll_rate*dtime/1000;//tra ve gia tri degree theo phuong roll	[degree]
		}
		//---------------------------------------------------------------------------------------------
		
		usart_clr();		
		usart_print_nb(roll_angle);//print roll_angle to computer screen
		
		//----------------------- creat fixed_loop_time= 25ms per loop---------------------------------	
		loop_usefull_time=sys_time-last_time;
		if(loop_usefull_time<fixed_loop_time)
		{
			for(i=0;i<(fixed_loop_time-loop_usefull_time);i++)
			{
				_delay_ms(1);
			}
		}		
		dtime=sys_time-last_time;//dtime=time per main loop (ms)
		last_time=sys_time;//update the last_time value for the next loop
		//------------------------------------------------------------------------------------------	
    }
}

ISR(TIMER2_OVF_vect)//when the timer2 over flow we have: sys_time=sys_time +1(ms)
{				// so we can caculate the dtime( time per main loop)
	sys_time+=1;  
	TCNT2=130;
}

In my board, the three gyro sensors also have the high-pass filter as you say, but it still drifs.
Could you take a look at my "gyro drift" problem and show me how to fix it ?

Regrard,
Nghia