IMU.cpp - Attitude of the Estimated Vector

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
FlavioLD
Posts: 1
Joined: Mon Nov 02, 2015 6:06 pm

IMU.cpp - Attitude of the Estimated Vector

Post by 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
Last edited by FlavioLD on Sun Nov 15, 2015 8:37 pm, edited 1 time in total.

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: IMU.cpp - Attitude of the Estimated Vector

Post by Plüschi »

FlavioLD wrote:

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
Posts: 46
Joined: Mon Oct 20, 2014 11:29 pm

Re: IMU.cpp - Attitude of the Estimated Vector

Post by 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

Post Reply