Quote:
Originally Posted by ayeckley
Thanks for the response. It's going to take us a while to digest that. Doing our own integration in real time does not seem like an attractive option, at least at first blush.
|
I may not understand why "total rotation" is needed - however my intuition is that transforming the quaternion to yaw angle is all you need. Quaternions were developed to avoid gimbal lock and can easily yield a yaw angle that is 0 to 360 rather than -180 to 180; my thinking is this completely eliminates the need for a total rotation value.
The code to do this is pretty trivial:
float q[4], yaw_radians, yaw_degrees;
// Convert navX Quaternions to a +/- 2 pi radians range
q[0] = ((float)quat_w) / 16384.0f;
q[1] = ((float)quat_x) / 16384.0f;
q[2] = ((float)quat_y) / 16384.0f;
q[3] = ((float)quat_z) / 16384.0f;
// Range-check quaternion values
for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];
// calculate yaw angle (0-360 degrees)
yaw_radians = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
yaw_degrees = yaw_radians * (180.0/3.1415926);