IMU.cpp - Attitude of the Estimated Vector

[FlavioLD]

Regards!

I am studying the Multiwii code (v2.3), but I have a doubt that I still could not resolve and would like some help.

The question is,
why, in the code below, att.angle[PITCH] is not calculated similarly to att.angle[ROLL]?

Ex:
att.angle [PITCH] = _atan2 (EstG32.V.Y, EstG32.V.Z);


The code is at IMU.cpp file, function getEstimatedAttitude, and was changed from version 2.1 to 2.2:

// Attitude of the estimated vector
int32_t sqGX_sqGZ = sq(EstG32.V.X) + sq(EstG32.V.Z);
invG = InvSqrt(sqGX_sqGZ + sq(EstG32.V.Y));
att.angle[ROLL] = _atan2(EstG32.V.X, EstG32.V.Z);
att.angle[PITCH] = _atan2(EstG32.V.Y, InvSqrt(sqGX_sqGZ)*sqGX_sqGZ);
Flaviold is online now Send a private message to Flaviold Find More Posts by Flaviold

[Plüschi]

Code: Select all

Flaviold is online now  Send a private message to Flaviold  Find More Posts by Flaviold

I dont understand that part of the code. /t

[Lecostarius]

Hi,

this is related to the fact that the angles measure two rotations where one is executed after the other.

The order counts: if you first do the pitch rotation, and then the roll you get a different result than if you first rotate around roll and then pitch.

Therefore, the formulas for the inversion of the angles are different. One is just an arc tangent of two accelerations divided by each other, since no rotation has been done so far. The other, however, is performed after the first one is done, and the relationship between the acceleration vectors and that angle is more tricky. This is the root cause of the asymmetry between the roll and the pitch angle computations.

Check out "Euler Angles" in Wikipedia for more details on the subject. But, the formula in IMU.cpp is correct to the approximation that sin(a)=a and cos(a)=1. This is the "small angle" approximation. Beware that the code will not behave well for larger angles.

Cheers, Leco