private void OnTrackerMessage(RiftInputReport report)
{
const float timeUnit = (1.0f / 1000.0f);
if (SequenceValid) {
uint timestampDelta;
if (report.Timestamp < LastTimestamp)
timestampDelta = (uint)((((int)report.Timestamp) + 0x10000) - (int)LastTimestamp);
else
timestampDelta = (uint)(report.Timestamp - LastTimestamp);
// If we missed a small number of samples, replicate the last sample.
if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254)) {
MessageBodyFrame sensors = new MessageBodyFrame(this);
sensors.TimeDelta = (timestampDelta - LastSampleCount) * timeUnit;
sensors.Acceleration = LastAcceleration;
sensors.RotationRate = LastRotationRate;
sensors.MagneticField = LastMagneticField;
sensors.Temperature = LastTemperature;
Sensor.OnMessage(sensors);
}
} else {
LastAcceleration = new Vector3f();
LastRotationRate = new Vector3f();
LastMagneticField = new Vector3f();
LastTemperature = 0;
SequenceValid = true;
}
LastSampleCount = report.SampleCount;
LastTimestamp = report.Timestamp;
bool convertHMDToSensor = (Coordinates == CoordinateFrame.Sensor) && (HWCoordinates == CoordinateFrame.HMD);
//if (HandlerRef.GetHandler())
{
MessageBodyFrame sensors = new MessageBodyFrame(this);
Byte iterations = report.SampleCount;
if (report.SampleCount > 3) {
iterations = 3;
sensors.TimeDelta = (report.SampleCount - 2) * timeUnit;
} else {
sensors.TimeDelta = timeUnit;
}
for (Byte i = 0; i < iterations; i++) {
sensors.Acceleration = AccelFromBodyFrameUpdate(report, i, convertHMDToSensor);
sensors.RotationRate = EulerFromBodyFrameUpdate(report, i, convertHMDToSensor);
sensors.MagneticField = MagFromBodyFrameUpdate(report, convertHMDToSensor);
sensors.Temperature = report.Temperature * 0.01f;
Sensor.OnMessage(sensors);
// TimeDelta for the last two sample is always fixed.
sensors.TimeDelta = timeUnit;
}
LastAcceleration = sensors.Acceleration;
LastRotationRate = sensors.RotationRate;
LastMagneticField = sensors.MagneticField;
LastTemperature = sensors.Temperature;
}
//else
//{
// UByte i = (report.SampleCount > 3) ? 2 : (report.SampleCount - 1);
// LastAcceleration = AccelFromBodyFrameUpdate(report, i, convertHMDToSensor);
// LastRotationRate = EulerFromBodyFrameUpdate(report, i, convertHMDToSensor);
// LastMagneticField = MagFromBodyFrameUpdate(report, convertHMDToSensor);
// LastTemperature = report.Temperature * 0.01f;
//}
}
Vector3f EulerFromBodyFrameUpdate(RiftInputReport update, Byte sampleNumber, bool convertHMDToSensor = false)
{
TrackerSample sample = update.Samples[sampleNumber];
float gx = (float)sample.Gyro.X;
float gy = (float)sample.Gyro.Y;
float gz = (float)sample.Gyro.Z;
Vector3f val = convertHMDToSensor ? new Vector3f(gx, gz, -gy) : new Vector3f(gx, gy, gz);
return val * 0.0001f;
}
Vector3f MagFromBodyFrameUpdate(RiftInputReport update, bool convertHMDToSensor = false)
{
// Note: Y and Z are swapped in comparison to the Accel.
// This accounts for DK1 sensor firmware axis swap, which should be undone in future releases.
if (!convertHMDToSensor)
{
return new Vector3f( (float)update.MagX,
(float)update.MagZ,
(float)update.MagY) * 0.0001f;
}
return new Vector3f((float)update.MagX,
(float)update.MagY,
-(float)update.MagZ) * 0.0001f;
}
// Sensor reports data in the following coordinate system:
// Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down.
// Gyro: 10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive yaw right.
// We need to convert it to the following RHS coordinate system:
// X right, Y Up, Z Back (out of screen)
//
Vector3f AccelFromBodyFrameUpdate(RiftInputReport update, Byte sampleNumber, bool convertHMDToSensor = false)
{
TrackerSample sample = update.Samples[sampleNumber];
float ax = (float)sample.Accel.X;
float ay = (float)sample.Accel.Y;
float az = (float)sample.Accel.Z;
Vector3f val = convertHMDToSensor ? new Vector3f(ax, az, -ay) : new Vector3f(ax, ay, az);
return val * 0.0001f;
}