Hello everyone. I’m trying to calculate the 3 angles of ROLL, PITCH and YAW to stabilize a drone.

Currently I can get: ROOL and PITCH with the following formulas:

pitch = 180 * atan (accelerationX / sqrt (accelerationY * accelerationY + accelerationZ * accelerationZ)) / M_PI;

roll = 180 * atan (accelerationY / sqrt (accelerationX * accelerationX + accelerationZ * accelerationZ)) / M_PI;

but if I use this expression, with the values of the magnometer, for the Yaw are incorrect values:

Yaw = atan2 ((-YY * cos (Roll) + MyZ * sin (Roll)), (MyX * cos (Pitch) + MyY * sin (Pitch) * sin (Roll) + MyZ * sin (Pitch) * cos ( Roll)))

I am attaching my code, you know the right expression to get the angle of YAW ?? or some solution for my problem

thank you

MPU9250_01.ino (3.16 KB)