Quaternion Parsing

#1

Hi All,

I’m working on a project where I need to know the position and orientation of each tag. I’ve correctly handled position without much issue, but I’m struggling to get the orientation from the quaternion packet.

I can parse the packet but I’m struggling to get what I consider to be valid data. I get values like:
X: 98988655
Y: 603887317
Z: 227804112
W: 853384514

I don’t have much experience with quaternions so I don’t know if these are valid or not. When I parse them with equations found here All three values (yaw, pitch, roll) oscillate pretty heavily.

Does anybody have any experience using the quaternion packet and could point me in the right direction?

EDIT: On more investigation I think I may be using the wrong datatypes for the quaternion values. Following what I did for the position_v3 packets I have uint32_t’s for all the quaternion values. Could this be wrong?

Thanks!

#2

Thank you for writing in.

The XYZ and W fields are floating point fields between -1 and 1. If you are looking at the data as 32 bit signed integers, the MAX INT (2^31 - 1) is +1 and MIN INT (0 - 2^31) is -1.

If you are using python, you can also use our CDP-Py PIP to directly access the X,Y,Z, and W values as floats.

#3

Awesome. That helps a bunch. Thanks!

#4

Does anybody know the frame of reference? For example where yaw, pitch, and roll are 0?

Thanks!

#5

After getting those quaternion values, use the following equation:

tmp = Math.Sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
qx /= tmp;
qy /= tmp;
qz /= tmp;
qw /= tmp;

            roll = Math.Atan2(2 * qx * qw + 2 * qy * qz, 1 - 2 * qx * qx - 2 * qy * qy);
            pitch = Math.Asin(2 * (qz * qx - qw * qy));
            yaw = Math.Atan2(2 * (qz * qw + qx * qy), 1 - 2 * (qy * qy + qz * qz));

then you can get the angles