Hi every one, i am a last year student in Vietnam and i have my term project is ” Building a quadrotor”.
I have something confuse about my project and i really need your help.
The hardware which i use to build my quadroto is “kk multicoter controller black board v5.5”, in this board there are 3 angular rate sensors and the main uController is atmega168pa, and here is the datasheets.
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
I follow the way to calculate the value of three angular rate sensors in these website:
and
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;
}
I recognize that: the response of the angular rate sensor is 50Hz (according to the sensor datasheet) , it means that: in 1 second the sensor just can output the value 50 times ( true or false ???? ), so my fixed_loop_time is 25ms per loop =>per 1 second i read the adc value 40 times < 50 times.
I think i do it almost correct like the guide but my result is not true as the real angular. So i make the video for you to see my problems.
As you see in my video, the angle when the board in balance point is 360 degrees, when it rolls left the angle will increase and when it rolls right the angle will decrease. And after all, when the board comes back to the zero spot, the angular must be 360 degrees, but no. So i think that may be the sensitivity when it rolls left is different than when it rolls right although in one sensor.
Now i really need all off your help, please give me any ideal to solve this problem. Many thank to you.
P/s: I’m so sorry about my English !