static public async void SendData(OpenTrackUDPData dataSource, string host, string prot)
{
if (_busy)
{
return;
}
_busy = true;
var data = new double[]
{
dataSource.X,
dataSource.Y,
dataSource.Z,
dataSource.Yaw,
dataSource.Pitch,
dataSource.Roll
}.SelectMany(BitConverter.GetBytes).ToArray();
var socket = new DatagramSocket();
try
{
using (var stream = await socket.GetOutputStreamAsync(new HostName(host), prot))
{
await stream.WriteAsync(data.AsBuffer());
await stream.FlushAsync();
}
}
catch
{
Debug.WriteLine("UDP DRROR");
}
_busy = false;
}
private void _data_characteristic_ValueChanged(GattCharacteristic sender, GattValueChangedEventArgs args)
{
var movementData = ConvertData(args.CharacteristicValue);
//Debug.WriteLine("{0},{1},{2},{3},{4},{5},{6},{7},{8}", movementData.AccelX.ToString("0.00"),
// movementData.AccelY.ToString("0.00"),
// movementData.AccelZ.ToString("0.00"),
// movementData.GyroX.ToString("0.00"),
// movementData.GyroY.ToString("0.00"),
// movementData.GyroZ.ToString("0.00"),
// movementData.MagX.ToString("0.00"),
// movementData.MagY.ToString("0.00"),
// movementData.MagZ.ToString("0.00"));
if (movementData.AccelZ == 0 && movementData.AccelY == 0 && movementData.AccelX == 0)
{
return;
}
if (movementData.MagX == 0 && movementData.MagY == 0 && movementData.MagZ == 0)
{
return;
}
//Debug.WriteLine("[{0},{1},{2}],", movementData.MagX, movementData.MagY, movementData.MagZ);
if (_isGetData == false)
{
_magCalibrator.Update(movementData);
return;
}
OpenTrackUDPData result = new OpenTrackUDPData();
if (Status == TrackerStatus.Connected)
{
_accXCalibrator.Reset();
_accYCalibrator.Reset();
_yawCalibrator.Reset();
_accXFiliter.Reset();
_accYFiliter.Reset();
_magYFiliter.Reset();
_magXFiliter.Reset();
UpdateStatue(TrackerStatus.Calibrate, "Calibrate accelerometer, keep stable");
}
var magBias = _magCalibrator.GetBias();
Debug.WriteLine("[{0},{1},{2}],", movementData.MagX - magBias.X, movementData.MagY - magBias.Y, movementData.MagZ - magBias.Z);
if (_accXCalibrator.Count < _accXCalibrator.Range)
{
result.Yaw = CalaAngle(movementData.MagY - magBias.Y, movementData.MagX - magBias.X);
_accXCalibrator.Update(movementData.AccelX);
_accYCalibrator.Update(movementData.AccelX);
_yawCalibrator.Update(result.Yaw);
}
else
{
UpdateStatue(TrackerStatus.Woriking, "Working..");
}
movementData.AccelX = _accXFiliter.GetVal(movementData.AccelX);
movementData.AccelY = _accYFiliter.GetVal(movementData.AccelY);
movementData.MagX = _magXFiliter.GetVal(movementData.MagX);
movementData.MagY = _magYFiliter.GetVal(movementData.MagY);
result.Yaw = CalaAngle(movementData.MagY - magBias.Y, movementData.MagX - magBias.X);
result.Yaw = result.Yaw - _yawCalibrator.Bias;
if (result.Yaw < 0)
{
result.Yaw = 360 + result.Yaw;
}
result.Pitch = CalaAngle(movementData.AccelZ, (movementData.AccelX - _accXCalibrator.Bias) * -1);
result.Roll = CalaAngle(movementData.AccelZ, (movementData.AccelY - _accYCalibrator.Bias));
if (ValueChanged != null)
{
ValueChanged(this, result);
}
}
