Euler angle conversion code wrt ZYX transformation

[agcaahmet]

Quaternion to euler angle conversion function "uint8_t MPU6050::dmpGetEuler(float data, Quaternion q)" seems to be wrong placed in "MPU6050_6Axis_MotionApps20.h" if standart Tait Bryan angles is used as transformation convention (ZYX rotation). We may refer to wikipedia link below: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

Corrected function should be probably as follows:

`uint8_t MPU6050::dmpGetEuler(float data, Quaternion q) { //data[0] = atan2(2 q->xq->y - 2 q->wq->z, 2 q->wq->w + 2 q->xq->x - 1); // psi //data[1] = -asin(2 q->xq->z + 2 q->wq->y); // theta //data[2] = atan2(2 q->yq->z - 2 q->wq->x, 2 q->wq->w + 2 q->zq->z - 1); // phi

//Edit referenced with wiki quaternion to euler angles information
data[0] = atan2(2*(q->w*q->z + q->x*q->y), 1-2*(q->y*q->y + q->z*q->z));   // psi
data[1] = asin(2*(q->w*q->y - q->z*q->x));    // theta
data[2] = atan2(2*(q->w*q->x + q->y*q->z),1-2*(q->x*q->x+q->y*q->y));   // phi
return 0;

}`

[jrowberg]

Have you tested this code and observed the output? I am not intimately familiar with the rationale behind these calculations, and I don't have a way of validating them without direct testing.

[agcaahmet]

I wanted to use euler angles as sensor inputs for PID algorithm instead of using tilt angles. I realized that euler angles are not behaving like they should be, after that I compared the equations in code and wiki. After modifying the equations as in my previous message, euler (Z-Y-X) angles seems to be correct. You can also test the angles, they should behave as follows (step order is important):

1. Starting with a level position (x-y plane parallel to earth)
2. Rotating w.r.t z-axis should change euler psi angle
3. Then rotating w.r.t y-axis should change euler theta angle
4. Then rotating w.r.t x-axis should change euler phi angle

For the above steps one by one; if you are rotating only one axis while keeping others exactly fixed, you should not see any change for other two angles.

[jrowberg]

Awesome. This will undoubtedly make quite a few people happy, as I've seen some inquiries about odd Euler angle behavior. I didn't have the opportunity to look into it, let alone identify a solution if/when I pinned down a reproducible problem. I don't even remember where the original conversion code came from. Thanks!

Do you want to fork, modify, commit, and submit a pull request?

[agcaahmet]

I follow-up your repo in any case. You can modify the equations as in your way after doing required checks :)