Hi there,
I’ve found a bug connected to compute euler angles:
sometimes I’m getting some of Yaw/Pitch/Roll angles which is greater than 359, for example 655. That’s well known problem associated with radian-degrees conversion (angle can be -655 or 1028).
I’d offer you to make following changes in SparkFunMPU9250-DMP lib:
Instead of:
if (degrees)
{
pitch *= (180.0 / PI);
roll *= (180.0 / PI);
yaw *= (180.0 / PI);
if (pitch < 0) pitch = 360.0 + pitch;
if (roll < 0) roll = 360.0 + roll;
if (yaw < 0) yaw = 360.0 + yaw;
}
portion of code in computeEulerAngles method, to use:
if (degrees)
{
yaw = NormalizeAngle(yaw * RAD_TO_DEGREE);
pitch = NormalizeAngle(pitch * RAD_TO_DEGREE);
roll = NormalizeAngle(roll * RAD_TO_DEGREE);
}
Where recursive NormalizeAngle method which normalizes angle to 0-359 range :
float NormalizeAngle(float angle)
{
return angle > 360 ?
NormalizeAngle(angle - 360) :
angle < 0 ? NormalizeAngle(angle + 360) : angle;
}
and pre-calculated RAD_TO_DEGREE (why to calculate division of constant values for this coefficient everytime triply in computeEulerAngles?)
static float RAD_TO_DEGREE = 180 / PI;