I2Cdevlib Forums

Yaw Pitch and Roll readings problem

Recommended Posts

Thank you for the amazing library. I’m using it to measure rotations in the 3 planes individually so the Euler angles don’t really work for me but the yaw pitch and roll do. I’ve noticed the following problem and it interferes with my measurements. If I’m having a pitch of around 90 degrees the Yaw readings go completely crazy and with the increment of simply 2 degrees of roll (without changing Yaw), the yaw measurement can go from 68.6 to -86.2. BUT with the Teapot example in processing the visualization of the 3D object doesn’t. So this means that they are being calculated differently: the yaw pitch and raw readings on the images come from:

```                // calculate yaw/pitch/roll angles
ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));```

And the 3D visualization i guess it comes from somewhere this:

```    // toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!)
// (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of
// different coordinate system orientation assumptions between Processing
// and InvenSense DMP)
float[] axis = quat.toAxisAngle();
rotate(axis[0], axis[2], axis[3], axis[1]);```

The image showing my problem is this:

How can I get the readings processing is doing and do the same in Arduino IDE? and is there anyway to make the yaw pitch and roll readings go from 0 to 180 and 0 to -180 instead of going to 90 and start decreasing even though the actual angle is increasing? Thank you very much for the help