Euler calculation improvements

[dahlsrud]

SparkFunMPU9250-DMP.cpp line #641 seems to have an error: The multiplication with 2 is superflous.
Apart from that, the Euler angle calculation results in "strange" angles. See discussion here:
https://forum.sparkfun.com/viewtopic.php?f=14&t=45516
With the following code I get expected results:

`static void toEulerianAngle(float w, float x, float y, float z, double& roll, double& pitch, double& yaw)
{
double ysqr = y * y;

// roll (x-axis rotation)
double t0 = +2.0 * (w * x + y * z);
double t1 = +1.0 - 2.0 * (x * x + ysqr);
roll = atan2(t0, t1);

// pitch (y-axis rotation)
double t2 = +2.0 * (w * y - z * x);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
pitch = asin(t2);

// yaw (z-axis rotation)
double t3 = +2.0 * (w * z + x * y);
double t4 = +1.0 - 2.0 * (ysqr + z * z);
yaw = atan2(t3, t4);
}
`
This is based on code from here:
https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

[theonteg]

I can confirm this.

Opened issue before reading yours in the RAZOR IMU M0 repository sparkfun/9DOF_Razor_IMU#9

I also noticed that the yaw values change every time depending on the heading of the board every time it turns on. Is this also a bug?

[hbob]

I experienced the same bug. As noted in the Sparkfun forum there are still some problems with the proposed computation from Wikipedia. In fact quaternions when representing rotations, must be unit quaternions. As the MPU 9250 uses fixed point calculations this cannot be guaranteed when converting back to floating point numbers and one has to do it manually. So the corrected code would be:

void MPU9250_DMP::computeEulerAngles(bool degrees)
{

float dqw = qToFloat(qw, 30);
float dqx = qToFloat(qx, 30);
float dqy = qToFloat(qy, 30);
float dqz = qToFloat(qz, 30);

float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz);
dqw = dqw/norm;
dqx = dqx/norm;
dqy = dqy/norm;
dqz = dqz/norm;

float ysqr = dqy * dqy;

// roll (x-axis rotation)
float t0 = +2.0 * (dqw * dqx + dqy * dqz);
float t1 = +1.0 - 2.0 * (dqx * dqx + ysqr);
roll = atan2(t0, t1);

// pitch (y-axis rotation)
float t2 = +2.0 * (dqw * dqy - dqz * dqx);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
pitch = asin(t2);

// yaw (z-axis rotation)
float t3 = +2.0 * (dqw * dqz + dqx * dqy);
float t4 = +1.0 - 2.0 * (ysqr + dqz * dqz);  
yaw = atan2(t3, t4);

}

[hmhalvorsen]

This helped a lot hbob, thank you!

[TaylorHawkes]

Above code works for me as well, however I had to change the qToFloat conversion to below code, otherwise I was getting random strange results.

 MPU9250_DMP::qToFloat(long number, unsigned char q)
{
    return (float)((double)number / (double)(1 << q));
}