MPU9250-CA-SDK - Python Script - Quaternion Calculation

By cableyang , 24 April 2014

Hello!

I have a MPU9250-CA-SDK connected to PC via USB cable (Embedded MotionApps CDC COM5).

I launched the rotational cube demo:

CMD:>> python ".../Release CA SDK Firmware 08212013/core/eMPL-hal/eMPL-client/eMA511-client.py" 5

The demo works perfectly. I put SDK in a certain position to see quaternion components (select the pygame window and send the command "inv q"):

q0 = 0,74 q1 = 0,01 q2 = -0,05 q3 = 0,67

I want to understand the quaternion's world. With that in mind, I analyze the serial packet received by PC using USBlyzer software http://www.usblyzer.com/. As expected, the platform sends packets composed by 23 bytes.

The first received byte is always the char $ (0x24) which marks the beggining of new packet.

The second byte is an handler byte (pkt_code for python script):
debug - 0x01
quat - 0x02
data - 0x03

If there is no any log (no printed data information), I receive continuously only a quat handler. If there is a printed information, I receive also continuously a quat handler preceeded by a data handler but the rest of the packet is exactly the same.

According to the quat handler, the python script (eMA511-client.py) calculate the 4 components of the unit quaternion (normalizing for put the values between -1 and 1).

Bytes 4, 5, 6 and 7: q0
Bytes 8, 9, 10 and 11: q1
Bytes 12, 13, 14 and 15: q2
Bytes 16, 17, 19 and 19: q3

Each component is calculated using the following code:

self.q0 = four_bytes(l[3],l[4],l[5],l[6]) * 1.0 / (1<<30)
self.q1 = four_bytes(l[7],l[8],l[9],l[10]) * 1.0 / (1<<30)
self.q2 = four_bytes(l[11],l[12],l[13],l[14]) * 1.0 / (1<<30)
self.q3 = four_bytes(l[15],l[16],l[17],l[18]) * 1.0 / (1<<30)

... where

four_bytes(d1, d2, d3, d4):
d = ord(d1)*(1<<24) + ord(d2)*(1<<16) + ord(d3)*(1<<8) + ord(d4)
if d > 2147483648:
d-= 4294967296
return d

I transposed the code to an excel document where I made all the calculations according to an example of a captured packet:
http://speedy.sh/gABzd/quaternion-calcs.xlsx

As you can see, the 4 components of the unit quaternion printed by the python script and those calculated are the same.

My question is: why is the magnitude considered 1<<30 = 1073741824 ?

By theory, the magnitude would be, considering this example:
sqrt(795546630^2 + 14015731^2 + -53580400^2 + 718999215^2) = 1073741874, which is very close of 1<<30, but isn't exactly the same.

Can anyone explain me that?!


Best regards,
BR

phpbb Topic ID
16089