C# GearVR Packet Decryption

Question

Working on a Windows application that communicates with the GearVR controller, and I'm having some trouble decrypting the Magnetometer data. While I'm getting Gryo/Accel data, it seems a bit off.

// Work in Progress
var Magnometer = new Vector3(
    data[48] << 8 | data[49],
    data[50] << 8 | data[51],
    data[52] << 8 | data[53]
    );
var Touch = new Vector2(
    (((data[54] & 0xF) << 6) + ((data[55] & 0xFC) >> 2)) & 0x3FF,
    (((data[55] & 0x3) << 8) + ((data[56] & 0xFF) >> 0)) & 0x3FF
    );
var TriggerPressed = (data[58] & 1) == 1;
var HomePressed = (data[58] & 2) == 2;
var BackPressed = (data[58] & 4) == 4;
var TouchPressed = (data[58] & 8) == 8;
var VolumeUpPressed = (data[58] & 0x10) == 0x10;
var VolumeDownPressed = (data[58] & 0x20) == 0x20;
var Temperature = data[57];
var Battery = data[59];

double[] TimeStamp = new double[3];
Vector3[] Accel = new Vector3[3];
Vector3[] Gyro = new Vector3[3];
for (int t = 0; t < 3; t++)
{
    TimeStamp[t] = BitConverter.ToInt32(data, 0+t * 16) / 1000 * 0.001;
    Accel[t] = new Vector3(
        BitConverter.ToInt16(data, 4 + t * 16) * 10000f * 9.80665f / 2048.0f,
        BitConverter.ToInt16(data, 6 + t * 16) * 10000f * 9.80665f / 2048.0f,
        BitConverter.ToInt16(data, 8 + t * 16) * 10000f * 9.80665f / 2048.0f
        ) * 0.0001f / 9.8f;
    Gryo[t] = new Vector3(
        BitConverter.ToInt16(data, 10 + t * 16) * 10000f * 0.017453292f / 14.285f,
        BitConverter.ToInt16(data, 12 + t * 16) * 10000f * 0.017453292f / 14.285f,
        BitConverter.ToInt16(data, 14 + t * 16) * 10000f * 0.017453292f / 14.285f
        ) * 0.00001f;
}

Some examples of data in orientations:

North:

DATA: [[1c6a4601][a2ff140038f8][02000400fcff]][[337b4601][a1ff160034f8][02000500fcff]][[da8d4601][9dff16003af8][03000300fcff]][f6d6460ef2fa][200000][18][40][3c]
DATA: [[68a04601][9dff140035f8][02000400fcff]][[f6b24601][9fff150036f8][02000400fdff]][[9dc54601][9cff150034f8][02000300fcff]][f5d6460ef1fa][200000][18][40][3c]
DATA: [[2bd84601][a0ff150037f8][03000500fdff]][[b9ea4601][9dff130037f8][02000300fcff]][[60fd4601][a0ff150035f8][02000500fdff]][f5d6460eeefa][200000][18][40][3c]

West:

DATA: [[55226604][99ffffff34f8][01000400fdff]][[fc346604][92fffdff33f8][02000400fcff]][[8a476604][95fffcff35f8][02000300fcff]][18d5960f95fb][200000][18][40][3c]
DATA: [[185a6604][95fffdff32f8][02000300fcff]][[a66c6604][98fffcff36f8][02000300fcff]][[4d7f6604][97fffcff33f8][02000400fdff]][17d5970f95fb][200000][18][40][3c]
DATA: [[db916604][97ffffff35f8][02000400fcff]][[69a46604][99ff030034f8][01000400fcff]][[10b76604][96fffdff35f8][02000400fbff]][17d5970f95fb][200000][18][40][3c]

South:

DATA: [[b647c005][b0fff1ff31f8][02000400fcff]][[445ac005][aefff2ff33f8][02000400fcff]][[d26cc005][aefff7ff31f8][02000400fbff]][3cd63611c5fc][200000][18][40][3c]
DATA: [[797fc005][adffefff34f8][02000400fdff]][[0792c005][adffeeff35f8][02000400fdff]][[95a4c005][aefff2ff32f8][02000400fcff]][3bd63611c6fc][200000][18][40][3c]
DATA: [[3cb7c005][b0fff2ff34f8][02000400fcff]][[cac9c005][adfff4ff3af8][02000400fcff]][[58dcc005][aefff2ff33f8][02000400fdff]][3bd63611c3fc][200000][18][40][3c]

East:

DATA: [[69ffb306][b7ff080033f8][02000400fcff]][[de11b406][b5ff0b0034f8][02000400fbff]][[6c24b406][b3ff080030f8][01000400fbff]][2dd8ba0f3efc][200000][18][40][3c]
DATA: [[4537b406][b4ff0b0031f8][03000400fcff]][[a149b406][b3ff070032f8][02000400fcff]][[2f5cb406][b2ff070036f8][02000400fcff]][2dd8bc0f3dfc][200000][18][40][3c]
DATA: [[d66eb406][b6ff080032f8][02000400fbff]][[6481b406][b5ff070035f8][02000400fbff]][[f293b406][b8ff090039f8][02000400fcff]][2ed8bb0f3ffc][200000][18][40][3c]

Using the packet format

[[Time1][Accel1][Gyro1]][[Time2][Accel2][Gyro2]][[Time3][Accel3][Gyro3]][Magnetometer?][Touch][Temperature][Buttons][Battery]

I've pulled most of the data successfully enough to work with, but the Magnetometer data is confusing at best. I've tried every variation of every algorithm I could find to pull the data in sensible forms, but nothing looks right. For the data above, I had the controller on it's face (calibration standard when using an Android headset) with the device in EnableVR+SensorMode.

Original Source: jsyang Hackaday