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); }