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