Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Euler calculation improvements #5

Open
dahlsrud opened this issue Mar 13, 2017 · 4 comments
Open

Euler calculation improvements #5

dahlsrud opened this issue Mar 13, 2017 · 4 comments

Comments

@dahlsrud
Copy link

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
Copy link

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
Copy link

hbob commented Jun 6, 2017

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
Copy link

This helped a lot hbob, thank you!

@TaylorHawkes
Copy link

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));
}

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

5 participants