I am also using the same gyro L3G4200D.

I am trying to get angle from it...

I've used ADXL 335 accelerometer.

I am trying to get angles with respect to roll, yaw & pitch

Here is my code:

Code:

float Rx,Ry,Rz,Yd,Xd,Zd,Rxe,Rye,Rze,Xavg,Yavg,Zavg,T,Rxg,Ryg,Rzg,c,Re,R,Rx1,Ry1,Rz1;
Th = 5;
c = 180/3.14;
T = 0.0155; //15.5 ms (verified by using counter)
while(1)
{
get_accl_data(); // Accl data from ADC
accelerometer(); // Converting ADC values in "g" - Rx, Ry, Rz
inclination(); // Normalizing Rx, Ry, Rz
gyro(); // Computing Xavg, Yavg, Zavg by using two consecutive readings
if((Xavg >= Th)||(Xavg <= -Th))
{
Xd = atan2(Rye, Rze);
Xd =(Xd + (Xavg*T));
Xd *= c;
ftoal(Xd,str);
UARTprintf("Xd : %s \n\r",str);
Xd /= c;
}
if((Yavg >= Th)||(Yavg <= -Th))
{
Yd = atan2(Rxe, Rze);
Yd =(Yd + (Yavg*T));
Yd *= c;
ftoal(Yd,str1);
UARTprintf(" Yd : %s \n\r",str1);
Yd /= c;
}
if((Zavg >= Th)||(Zavg <= -Th))
{
Zd = atan2(Rye, Rxe);
Zd =Zd + (Zavg*T);
Zd *= c;
ftoal(Zd,str2);
UARTprintf(" Zd : %s\n\r",str2);
Zd /= c;
}
Rxg = sin(Yd) / (sqrt(1 + ( (cos(Yd)*cos(Yd) )*(tan(Xd) * tan(Xd) ))));
Ryg = sin(Xd) / (sqrt(1 + ( (cos(Xd) * cos(Xd) ) *( tan(Yd) * tan(Yd) ))));
Rzg = ((Rzg > 0) ? 1 : ((Rzg < 0) ? -1 : 1))*(sqrt(1 - (Rxg*Rxg)-(Ryg*Ryg)));
Rxe = (Rx*0.02)+(Rxg*0.98);
Rye = (Ry*0.02)+(Ryg*0.98);
Rze = (Rz*0.02)+(Rzg*0.98);
Re = sqrt((Rxe*Rxe) + (Rye*Rye) + (Rze*Rze) );
Rxe = Rxe/Re;
Rye = Rye/Re;
Rze = Rze/Re;
SysCtlDelay(800000); // 10 ms delay
}

I am not getting the accurate angles...

I am getting some angles proportional to acceleration at a time when I move it.

And after moving it from angle 0 to angle 45 (for example) it stabilizes after a 1-2 sec. and giving stable values of Xd, Yd, Zd again!!

What I am missing??

Please help me

## Bookmarks