Jigar4Electronics

03-22-2013, 01:39 AM

Hello all,

I am making Autonomous Quad.

I am programming my IMU using ADXL335 -3 Axis analog Accelerometer & L3G4200D 3-axis digital Gyro.

I am using ARM Cortex M4F based TI's stellaris launchpad LM4F120XL.

I've a problem while implementing IMU.

Here is my algorithm of IMU...

By ADC I am getting reading of accelerometer in Dx, Dy & Dz.

Now as I've 12bit ADC I am using the following formula to get the readings in "g"

Rx = [(Dx*(3.3/4095) - Tx] / 0.3

Ry = [(Dy*(3.3/4095) - Ty] / 0.3

Rz = [(Dz*(3.3/4095) - Tz] / 0.3

(In stable condition I've checked the Tx, Ty, Tz : Tx = 1.630820, Ty = 1.658477, Tz = 1.997728)

For Gyro (at 250 dps with 8.75 mdps/digit)

To get reading in dps I am using following formula

X = Gx*8.75/1000

Y = Gy*8.75/1000

Z = Gz*8.75/1000

(Here Gx, Gy & Gz are data from gyro... I've checked them- they are signed values and also accurate (min. noise) )

I want to measure angles with respect to all axis (pitch, roll & yaw). For that I am doing as per following

Th = 5;

T = 0.05; // approx. 5ms I've checked it using counter

c = 180/3.14;

while(1)

{

Xd = atan2(Rxe, sqrt( (Rze*Rze) + (Rye*Rye) )); // initial values of Rxe, Rye, Rze are same as Rx, Ry, Rz

Yd = atan2(Rye, sqrt( (Rze*Rze) + (Rxe*Rxe) ));

Zd = atan2(Rze, sqrt( (Rye*Rye) + (Rxe*Rxe) ));

gyro(); // To calculate Xavg, Yavg, Zavg from two consecutive X, Y, Z values respectively

if((Xavg >= Th)||(Xavg <= Th))

{

Xd = Xd + (Xavg*T);

}

if((Yavg >= Th)||(Yavg <= Th))

{

Yd = Yd + (Yavg*T);

}

if((Zavg >= Th)||(Zavg <= Th))

{

Zd = Zd + (Zavg*T);

}

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)));

get_accl_data(); // To get ADC values

accelerometer(); // To convert ADC valuse in "g" - Rx, Ry, Rz

Rxe = (Rx*0.015)+(Rxg*0.985); // weighted average

Rye = (Ry*0.015)+(Ryg*0.985);

Rze = (Rz*0.015)+(Rzg*0.985);

Xd *= c; // converting in degree

Yd *= c;

Zd *= c;

Print(Xd);

Print(Xd);

Print(Xd);

Xd /*=c; // converting in radians again

Yd /*=c;

Zd /*=c;

}

/* I am printing Xd, Yd, Zd... It is okay while satble or within +/- 10 degree...

But for more than 10 degree I am just getting fault values. I mean It is informing whether

the device is satable or not OR whether it is moving on positive side(counter clock wise) or negative

side (clockwise) BUT not giving the exact +/- 40 degree output while it is actually on 40 angle with respect to ground! */

What I am missing? Please help me ...

I've done it by referring some docs and blogs.

// al variables type are taken proper (as per my knowledge).

One more question : Do I really need to know these angles? or can I use the current values also to make my quad stable ?

What else data I'll require to make my quad stable?

Thank you for your time.

I am making Autonomous Quad.

I am programming my IMU using ADXL335 -3 Axis analog Accelerometer & L3G4200D 3-axis digital Gyro.

I am using ARM Cortex M4F based TI's stellaris launchpad LM4F120XL.

I've a problem while implementing IMU.

Here is my algorithm of IMU...

By ADC I am getting reading of accelerometer in Dx, Dy & Dz.

Now as I've 12bit ADC I am using the following formula to get the readings in "g"

Rx = [(Dx*(3.3/4095) - Tx] / 0.3

Ry = [(Dy*(3.3/4095) - Ty] / 0.3

Rz = [(Dz*(3.3/4095) - Tz] / 0.3

(In stable condition I've checked the Tx, Ty, Tz : Tx = 1.630820, Ty = 1.658477, Tz = 1.997728)

For Gyro (at 250 dps with 8.75 mdps/digit)

To get reading in dps I am using following formula

X = Gx*8.75/1000

Y = Gy*8.75/1000

Z = Gz*8.75/1000

(Here Gx, Gy & Gz are data from gyro... I've checked them- they are signed values and also accurate (min. noise) )

I want to measure angles with respect to all axis (pitch, roll & yaw). For that I am doing as per following

Th = 5;

T = 0.05; // approx. 5ms I've checked it using counter

c = 180/3.14;

while(1)

{

Xd = atan2(Rxe, sqrt( (Rze*Rze) + (Rye*Rye) )); // initial values of Rxe, Rye, Rze are same as Rx, Ry, Rz

Yd = atan2(Rye, sqrt( (Rze*Rze) + (Rxe*Rxe) ));

Zd = atan2(Rze, sqrt( (Rye*Rye) + (Rxe*Rxe) ));

gyro(); // To calculate Xavg, Yavg, Zavg from two consecutive X, Y, Z values respectively

if((Xavg >= Th)||(Xavg <= Th))

{

Xd = Xd + (Xavg*T);

}

if((Yavg >= Th)||(Yavg <= Th))

{

Yd = Yd + (Yavg*T);

}

if((Zavg >= Th)||(Zavg <= Th))

{

Zd = Zd + (Zavg*T);

}

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)));

get_accl_data(); // To get ADC values

accelerometer(); // To convert ADC valuse in "g" - Rx, Ry, Rz

Rxe = (Rx*0.015)+(Rxg*0.985); // weighted average

Rye = (Ry*0.015)+(Ryg*0.985);

Rze = (Rz*0.015)+(Rzg*0.985);

Xd *= c; // converting in degree

Yd *= c;

Zd *= c;

Print(Xd);

Print(Xd);

Print(Xd);

Xd /*=c; // converting in radians again

Yd /*=c;

Zd /*=c;

}

/* I am printing Xd, Yd, Zd... It is okay while satble or within +/- 10 degree...

But for more than 10 degree I am just getting fault values. I mean It is informing whether

the device is satable or not OR whether it is moving on positive side(counter clock wise) or negative

side (clockwise) BUT not giving the exact +/- 40 degree output while it is actually on 40 angle with respect to ground! */

What I am missing? Please help me ...

I've done it by referring some docs and blogs.

// al variables type are taken proper (as per my knowledge).

One more question : Do I really need to know these angles? or can I use the current values also to make my quad stable ?

What else data I'll require to make my quad stable?

Thank you for your time.