0
\$\begingroup\$

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) { // initial values of Rxe, Rye, Rze are same as Rx, Ry, Rz Xd = atan2(Rxe, sqrt( (Rze*Rze) + (Rye*Rye) )); Yd = atan2(Rye, sqrt( (Rze*Rze) + (Rxe*Rxe) )); Zd = atan2(Rze, sqrt( (Rye*Rye) + (Rxe*Rxe) )); // To calculate Xavg, Yavg, Zavg from two consecutive X, Y, Z values respectively gyro(); 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?

\$\endgroup\$
4
  • \$\begingroup\$ You need to include the variable type declarations in your code excerpt, as choices inconsistent with your operations could be a problem. \$\endgroup\$ Commented Mar 22, 2013 at 11:25
  • 2
    \$\begingroup\$ @Jigar4Electronics I suspect you are not receiving any answers because your question is too narrow, specific to a set of devices that may not be available to other members, and is not of sufficiently general interest to merit additional research effort by said members. Please consider whether you can slice the question down a bit. \$\endgroup\$ Commented Mar 22, 2013 at 11:39
  • \$\begingroup\$ Link to the data sheet, please. \$\endgroup\$ Commented Apr 3, 2013 at 18:11
  • \$\begingroup\$ For an IMU, a bit of math is inescapable. Have you written down what you want to achieve in terms of equations? You can use LaTeX on this site to communicate your intent. IMUs are a little more complex behind the scenes and the processing is somewhat more involved when it comes to fusing gyros with accelerometer data. \$\endgroup\$ Commented Oct 3, 2014 at 6:03

1 Answer 1

Reset to default
1
\$\begingroup\$

(This question has been posted more than a year ago)

You need these angles in order to apply a control, a PID for example, to stabilize the quadcopter in hovering mode. Your control is supposed to be active when you are not remote controlling your drone and you want it to be perfectly horizontal. This means that the theta(x axis) and phi(y axis) angles will be close to zero and will reach the desired reference value ( =0 ) thanks to the control.

I have a question for you: Have you recorded these values while the motors were spinning? I've noticed an important increase in noise amplitude caused by vibrations on the chassis.

\$\endgroup\$

Start asking to get answers

Find the answer to your question by asking.

Ask question

Explore related questions

See similar questions with these tags.