public static void Parse(PSVRSensorReport Report)
{
if (recalibrate)
{
samplesLeft = 2000;
accelOffset = Vector3.Zero;
gyroOffset = Vector3.Zero;
recalibrate = false;
}
Report.LinearAcceleration1 = new Vector3(Report.RawMotionX1 * aRes, Report.RawMotionY1 * aRes, Report.RawMotionZ1 * aRes);
Report.AngularAcceleration1 = new Vector3(Report.RawGyroYaw1 * gRes, Report.RawGyroPitch1 * gRes, Report.RawGyroRoll1 * gRes);
Integrate(Report.LinearAcceleration1, Report.AngularAcceleration1, Report.Timestamp1);
Report.LinearAcceleration2 = new Vector3(Report.RawMotionX2 * aRes, Report.RawMotionY2 * aRes, Report.RawMotionZ2 * aRes);
Report.AngularAcceleration2 = new Vector3(Report.RawGyroYaw2 * gRes, Report.RawGyroPitch2 * gRes, Report.RawGyroRoll2 * gRes);
Report.Pose = Integrate(Report.LinearAcceleration2, Report.AngularAcceleration2, Report.Timestamp2);
//Report.LinearAcceleration1 = new Vector3(-Report.RawMotionY1 * aRes, Report.RawMotionX1 * aRes, Report.RawMotionZ1 * aRes);
//Report.AngularAcceleration1 = new Vector3(-Report.RawGyroPitch1 * gRes, Report.RawGyroYaw1 * gRes, Report.RawGyroRoll1 * gRes);
//Integrate(Report.LinearAcceleration1, Report.AngularAcceleration1, Report.Timestamp1);
//Report.LinearAcceleration2 = new Vector3(-Report.RawMotionY2 * aRes, Report.RawMotionX2 * aRes, Report.RawMotionZ2 * aRes);
//Report.AngularAcceleration2 = new Vector3(-Report.RawGyroPitch2 * gRes, Report.RawGyroYaw2 * gRes, Report.RawGyroRoll2 * gRes);
//Report.Pose = Integrate(Report.LinearAcceleration2, Report.AngularAcceleration2, Report.Timestamp2);
Report.Orientation = ToEuler(ref Report.Pose);
}
public void Feed(PSVRSensorReport Report)
{
if (intervalMeasure.ElapsedMilliseconds < intervalLapse)
{
return;
}
intervalMeasure.Restart();
Vector3 forceVector = new Vector3((float)Report.LinearAcceleration1.X, (float)Report.LinearAcceleration1.Y, (float)Report.LinearAcceleration1.Z);
float magnitude = forceVector.Length();
if (calCycles > 0)
{
calCycles--;
zeroForce += magnitude;
return;
}
else if (calCycles == 0)
{
calCycles--;
zeroForce /= 200;
return;
}
magnitude = magnitude - zeroForce;
if (magnitude < sensibility)
{
onSpike = false;
return;
}
if (onSpike)
{
return;
}
if (!onTap)
{
onTap = true;
spikeCount = 1;
tapMeasure.Restart();
}
else
{
spikeCount++;
}
debounceEvent.Change(debounceLapse, Timeout.Infinite);
onSpike = true;
}
private void Reader_DataReceived(object sender, LibUsbDotNet.Main.EndpointDataEventArgs e)
{
var rep = PSVRSensorReport.parseSensor(e.Buffer);
if (SensorDataUpdate == null)
{
return;
}
SensorDataUpdate(this, new PSVRSensorEventArgs {
SensorData = rep
});
}
public static PSVRSensorReport parseSensor(byte[] data)
{
PSVRSensorReport sensor = new PSVRSensorReport();
if (data == null)
{
return(sensor);
}
sensor.Buttons = (HeadsetButtons)data[0];
sensor.Volume = data[2];
sensor.Worn = (data[8] & 0x1) == 0x1 ? true : false; //confirmed
sensor.DisplayActive = (data[8] & 0x2) == 0x2 ? false : true;
sensor.Muted = (data[8] & 0x8) == 0x8 ? true : false; //confirmed
sensor.EarphonesConnected = (data[8] & 0x10) == 0x10 ? true : false; //confirmed
sensor.Timestamp1 = getUIntFromUInt32(data, 16);
sensor.RawGyroYaw1 = getIntFromInt16(data, 20);
sensor.RawGyroPitch1 = getIntFromInt16(data, 22);
sensor.RawGyroRoll1 = getIntFromInt16(data, 24);
sensor.RawMotionX1 = getAccelShort(data, 26);
sensor.RawMotionY1 = getAccelShort(data, 28);
sensor.RawMotionZ1 = getAccelShort(data, 30);
sensor.Timestamp2 = getUIntFromUInt32(data, 32);
sensor.RawGyroYaw2 = getIntFromInt16(data, 36);
sensor.RawGyroPitch2 = getIntFromInt16(data, 38);
sensor.RawGyroRoll2 = getIntFromInt16(data, 40);
sensor.RawMotionX2 = getAccelShort(data, 42);
sensor.RawMotionY2 = getAccelShort(data, 44);
sensor.RawMotionZ2 = getAccelShort(data, 46);
sensor.CalStatus = data[48];
sensor.Ready = data[49];
sensor.A = data[50];
sensor.B = data[51];
sensor.C = data[52];
sensor.VoltageValue = data[53];
sensor.VoltageReference = data[54];
sensor.IRSensor = getIntFromInt16(data, 55);
sensor.D = data[58];
sensor.E = data[59];
sensor.F = data[60];
sensor.G = data[61];
sensor.H = data[62];
sensor.PacketSequence = data[63];
//BMI055Integrator.Parse(data, 26, 20, sensor.Timestamp1);
//sensor.Pose = BMI055Integrator.Parse(data, 42, 36, sensor.Timestamp2);
BMI055Integrator.Parse(sensor);
return(sensor);
}
