public void Broadcast(PSVRSensor SensorData)
{
byte[] data = Encoding.UTF8.GetBytes(JsonConvert.SerializeObject(SensorData));
client.Send(data, data.Length, ep);
}
public void Update(PSVRSensor Data)
{
if (samples < 1000)
{
drift[0] += ParseAccelerometerData((short)Data.MotionY1);
drift[1] += ParseAccelerometerData((short)Data.MotionX1);
drift[2] += ParseAccelerometerData((short)Data.MotionZ1);
drift[3] += ParseAccelerometerData((short)Data.GyroPitch1);
drift[4] += ParseAccelerometerData((short)Data.GyroYaw1);
drift[5] += ParseAccelerometerData((short)Data.GyroRoll1);
samples++;
return;
}
else if (samples == 1000)
{
samples++;
for (int buc = 0; buc < 6; buc++)
{
drift[buc] = Math.Abs(drift[buc] / 1000.0f);
}
drift[1] -= 1;
}
SamplePeriod = 1 / (sw.ElapsedTicks / (float)Stopwatch.Frequency);
sw.Restart();
float accelX = ParseAccelerometerData((short)Data.MotionY1);
float accelY = ParseAccelerometerData((short)Data.MotionX1);
float accelZ = ParseAccelerometerData((short)Data.MotionZ1);
//float gyroX = ParseGyroscopeData((short)Data.GyroPitch1);
//float gyroY = ParseGyroscopeData((short)Data.GyroRoll1);
//float gyroZ = ParseGyroscopeData((short)Data.GyroYaw1);
//float gyroX = ParseGyroscopeData((short)Data.GyroPitch1);
//float gyroY = ParseGyroscopeData((short)Data.GyroYaw1);
//float gyroZ = ParseGyroscopeData((short)Data.GyroRoll1);
//float gyroX = ParseGyroscopeData((short)Data.GyroRoll1);
//float gyroY = ParseGyroscopeData((short)Data.GyroPitch1);
//float gyroZ = ParseGyroscopeData((short)Data.GyroYaw1);
//float gyroX = ParseGyroscopeData((short)Data.GyroRoll1);
//float gyroY = ParseGyroscopeData((short)Data.GyroYaw1);
//float gyroZ = ParseGyroscopeData((short)Data.GyroPitch1);
//float gyroX = ParseGyroscopeData((short)Data.GyroYaw1);
//float gyroY = ParseGyroscopeData((short)Data.GyroPitch1);
//float gyroZ = ParseGyroscopeData((short)Data.GyroRoll1);
float gyroX = ParseGyroscopeData((short)Data.GyroPitch1);
float gyroY = ParseGyroscopeData((short)Data.GyroYaw1);
float gyroZ = ParseGyroscopeData((short)Data.GyroRoll1);
accelX = Math.Sign(accelX) * (Math.Abs(accelX) - drift[0]);
accelX = Math.Sign(accelY) * (Math.Abs(accelY) - drift[1]);
accelX = Math.Sign(accelZ) * (Math.Abs(accelZ) - drift[2]);
accelX = Math.Sign(gyroX) * (Math.Abs(gyroX) - drift[3]);
accelX = Math.Sign(gyroY) * (Math.Abs(gyroY) - drift[4]);
accelX = Math.Sign(gyroZ) * (Math.Abs(gyroZ) - drift[5]);
Update(gyroX, gyroY, gyroZ, accelX, accelY, accelZ);
//accelX = ParseAccelerometerData((short)Data.MotionY2);
//accelY = ParseAccelerometerData((short)Data.MotionX2);
//accelZ = ParseAccelerometerData((short)Data.MotionZ2);
//gyroX = ParseGyroscopeData((short)Data.GyroYaw2);
//gyroY = ParseGyroscopeData((short)Data.GyroRoll2);
//gyroZ = ParseGyroscopeData((short)Data.GyroPitch2);
//Update(gyroX, gyroY, gyroZ, accelX, accelY, accelZ);
}
