For one thing, it appears the code doesn't quite match the struct, specifically on calculating the z position. According to that struct the z bits are Position2 & 0x7FFFF. Heading calculation is clearly incorrect so I'm going to guess the ZPos is too (or for a different version of the packet?).
How to figure out where the bits are? Group the bit fields into 32 bit integers since the type of each bit field is int.
First 32 bits is posData[0] Position1
unsigned pitch:12;
signed deltaX:13; // change in x
unsigned padding01:7;
Second 32 bits, posData[1] Position2
signed z:19; // z coord (3rd loc value)
signed deltaHeading:10; // change in heading
unsigned padding02:3;
z is listed first so it is simply the lowest 19 bits of this 32 bit int, posData[1] & 0x7FFFF
Third posData[2] Position3
signed x:19; // x coord (1st loc value)
signed deltaZ:13; // change in z
again, x listed first, lowest 19 bits, posData[2] & 0x7FFFF
Fourth posData[3] Position4
unsigned heading:12; // heading
signed deltaY:13; // change in y
unsigned padding03:7;
nothing here
Fifth posData[4] Position5
signed animation:10; // velocity
signed y:19; // y coord (2nd loc value)
unsigned padding04:3;
animation is stored in the lowest 10 bits, y is stored in the next 19 bits, that's why it needs to be shifted right by 10. (posData[4]>>10) & 0x7FFFF
|
Threaded Mode
