void CallbackGearVR(GattCharacteristic sender, GattValueChangedEventArgs eventArgs)
{
var buf = eventArgs.CharacteristicValue.ToArray();
if (buf.Length < 3)
{
return;
}
// The data array consist of multiple data which size is 16bytes and footer 12bytes
// On current version, the data array is 60 bytes and dataCount = 3
const int DATA_SIZE = 16;
const int FOOTER_SIZE = 6 + 3 + 1 + 1 + 1; // 12 bytes = 6(mag) + 3(touch) + 1(temperature) + 1(button) + 1(unknown)
int dataCount = (buf.Length - FOOTER_SIZE) / DATA_SIZE;
var mscale = 0.06 * 0.06;
magnetometer[0] = Get16(DATA_SIZE * dataCount + 0, buf) * mscale; // 48 ~ 54
magnetometer[1] = Get16(DATA_SIZE * dataCount + 2, buf) * mscale;
magnetometer[2] = Get16(DATA_SIZE * dataCount + 4, buf) * mscale;
var mmag = Math.Sqrt(magnetometer[0] * magnetometer[0]
+ magnetometer[1] * magnetometer[1] + magnetometer[2] * magnetometer[2]);
for (int i = 0; i < dataCount; i++)
{
var time2 = Get32(DATA_SIZE * i, buf);
double ascale = 9.80665 / 2048.0;
var ax = Get16(DATA_SIZE * i + 4, buf) * ascale;
var ay = Get16(DATA_SIZE * i + 6, buf) * ascale;
var az = Get16(DATA_SIZE * i + 8, buf) * ascale;
var amag = Math.Sqrt(ax * ax + ay * ay + az * az);
acceleration[0] = ax;
acceleration[1] = ay;
acceleration[2] = az;
double gscale = 0.017453292 / 14.285;
var gx = Get16(DATA_SIZE * i + 10, buf) * gscale;
var gy = Get16(DATA_SIZE * i + 12, buf) * gscale;
var gz = Get16(DATA_SIZE * i + 14, buf) * gscale;
var gmag = Math.Sqrt(gx * gx + gy * gy + gz * gz);
gyroscope[0] = gx;
gyroscope[1] = gy;
gyroscope[2] = gz;
AHRS.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az, (float)magnetometer[0], (float)magnetometer[1], (float)magnetometer[2]);
}
// We need to fix the difference of coordinate system between AHRS and VR app.
// I don't know how to introduce correct conversion, but it seems roughly right (working).
quaternion[0] = AHRS.Quaternion[1];
quaternion[1] = AHRS.Quaternion[3];
quaternion[2] = -AHRS.Quaternion[2];
quaternion[3] = AHRS.Quaternion[0];
position = ArmModel.CalculateModel(quaternion);
temperature = buf[DATA_SIZE * dataCount + 6 + 3]; // 57
int touchPos = DATA_SIZE * dataCount + 6; // 54
var touchFlag = ParseInt(touchPos * 8, touchPos * 8 + 4, false, buf);
var tx = ParseInt(touchPos * 8 + 4, touchPos * 8 + 14, false, buf);
var ty = ParseInt(touchPos * 8 + 14, touchPos * 8 + 24, false, buf);
double tscale = 1.0 / 320.0;
// from -1.0(left) to +1.0(right)
trackpad[0] = tx * tscale * 2.0 - 1.0;
// from -1.0(bottom) to +1.0(top)
trackpad[1] = 1.0 - ty * tscale * 2.0;
button[TOUCH] = touchFlag == 1;
// trigger, home, back, touch click, vol up, vol down
int buttonPos = DATA_SIZE * dataCount + 6 + 3 + 1; // 58;
button[TRIGGER] = (buf[buttonPos] & (1 << 0)) != 0;
button[HOME] = (buf[buttonPos] & (1 << 1)) != 0;
button[APP] = (buf[buttonPos] & (1 << 2)) != 0;
button[CLICK] = (buf[buttonPos] & (1 << 3)) != 0;
button[VOLUP] = (buf[buttonPos] & (1 << 4)) != 0;
button[VOLDOWN] = (buf[buttonPos] & (1 << 5)) != 0;
version = buf[DATA_SIZE * dataCount + 6 + 3 + 1 + 1]; // 59
}
void CallbackDaydream(GattCharacteristic sender, GattValueChangedEventArgs eventArgs)
{
// https://stackoverflow.com/questions/40730809/use-daydream-controller-on-hololens-or-outside-daydream/40753551#40753551
// https://github.com/mrdoob/daydream-controller.js/blob/master/DaydreamController.js
var buf = eventArgs.CharacteristicValue.ToArray();
// Orientation
double scale = 2.0 * Math.PI / 4095.0;
orientation[0] = ParseInt(14, 27, true, buf) * scale;
orientation[1] = ParseInt(27, 40, true, buf) * scale;
orientation[2] = ParseInt(40, 53, true, buf) * scale;
double abs = Math.Sqrt(orientation[0] * orientation[0]
+ orientation[1] * orientation[1] + orientation[2] * orientation[2]);
double sin = Math.Sin(abs / 2);
quaternion[0] = orientation[0] / abs * sin;
quaternion[1] = orientation[1] / abs * sin;
quaternion[2] = orientation[2] / abs * sin;
quaternion[3] = Math.Cos(abs / 2);
position = ArmModel.CalculateModel(quaternion);
// Acceleration
double ascale = 8.0 * 9.8 / 4095.0;
acceleration[0] = ParseInt(53, 66, true, buf) * ascale;
acceleration[1] = ParseInt(66, 79, true, buf) * ascale;
acceleration[2] = ParseInt(79, 92, true, buf) * ascale;
// Gyro
double gscale = 2048.0 / 180 * Math.PI / 4095.0;
gyroscope[0] = ParseInt(92, 105, true, buf) * gscale;
gyroscope[1] = ParseInt(105, 118, true, buf) * gscale;
gyroscope[2] = ParseInt(118, 131, true, buf) * gscale;
// Touch position
double tscale = 1 / 255.0;
int tx = ParseInt(131, 139, false, buf);
int ty = ParseInt(139, 147, false, buf);
// from -1.0(left) to +1.0(right)
trackpad[0] = tx * tscale * 2.0 - 1.0;
// from -1.0(bottom) to +1.0(top)
trackpad[1] = 1.0 - ty * tscale * 2.0;
button[TOUCH] = tx != 0 || ty != 0;
// Vol up
button[VOLUP] = ParseInt(147, 148, false, buf) != 0;
// Vol down
button[VOLDOWN] = ParseInt(148, 149, false, buf) != 0;
// App
button[APP] = ParseInt(149, 150, false, buf) != 0;
// Home
button[HOME] = ParseInt(150, 151, false, buf) != 0;
// Click
button[CLICK] = ParseInt(151, 152, false, buf) != 0;
// Version
version = ParseInt(152, 160, false, buf);
}