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\$
  • \$\begingroup\$ You need to include the variable type declarations in your code excerpt, as choices inconsistent with your operations could be a problem. \$\endgroup\$ – Chris Stratton Mar 22 '13 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\$ – Anindo Ghosh Mar 22 '13 at 11:39
  • \$\begingroup\$ Link to the data sheet, please. \$\endgroup\$ – Brian Carlton Apr 3 '13 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\$ – Damien Oct 3 '14 at 6:03
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\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.