void OnNewReceiveUdpServerEvent(object sender, NewEventReceiveUDPServerArgs e)
{
try
{
_sensorCalibration.Push(e.ReceiveUDPServerMessage);
GyroQuaternion gyroQuaternionInverse = GyroQuaternion.Inverse(e.ReceiveUDPServerMessage);
GyroQuaternion gyroQuaternionCalibrationResult = _sensorCalibration.GetCalibrationResult(e.ReceiveUDPServerMessage.sensorName);
if (gyroQuaternionCalibrationResult != null)
{
GyroQuaternion gyroQuaternion = GyroQuaternion.Multiply(gyroQuaternionInverse, gyroQuaternionCalibrationResult);
gyroQuaternion = GetSettingsAxis(gyroQuaternion);
if (_senderUdpClient.UdpSocket != null && _senderUdpClient.UdpSocket.Connected)
{
_senderUdpClient.Send(gyroQuaternion);
}
if (e.ReceiveUDPServerMessage.sensorName == GetSelectorSensorValue())
{
gyroQuaternion = _filterQuaternion.Filter(gyroQuaternion);
Cube3DRender(gyroQuaternion);
}
}
}
catch (Exception)
{
// ignored
}
}
public void Push(GyroQuaternion quat)
{
if (!calibrationDictionary.Any(x => x.Key == quat.sensorName))
{
calibrationDictionary.TryAdd(quat.sensorName, new GyroQuaternion(quat.sensorName, quat.qX, quat.qY, quat.qZ, quat.qW));
}
}
private void DoReceiveFrom(IAsyncResult iar)
{
try
{
EndPoint clientEp = new IPEndPoint(IPAddress.Any, 0);
try
{
var dataLen = UdpSocket.EndReceiveFrom(iar, ref clientEp);
var data = new byte[dataLen];
Array.Copy(_byteData, data, dataLen);
EndPoint newClientEp = new IPEndPoint(IPAddress.Any, 0);
UdpSocket.BeginReceiveFrom(_byteData, 0, _byteData.Length, SocketFlags.None, ref newClientEp, DoReceiveFrom, newClientEp);
if (!_clientList.Any(client => client.Equals(clientEp)))
{
_clientList.Add(clientEp);
}
GyroQuaternion gyroQuaternion = JsonConvert.DeserializeObject <GyroQuaternion>(Encoding.ASCII.GetString(data));
Eventing.eventReceiveUDPServer.InvokeOnNewReceiveUDPServer(gyroQuaternion);
}
catch (Exception ex)
{
Program.ExceptionLog(ex);
}
}
catch (ObjectDisposedException dex)
{
Program.ExceptionLog(dex);
}
}
public void InvokeOnNewReceiveUDPServer(GyroQuaternion quat)
{
NewReceiveUDPServer handler = OnNewReceiveUDPServer;
NewEventReceiveUDPServerArgs e = new NewEventReceiveUDPServerArgs(quat);
handler?.Invoke(this, e);
}
private GyroQuaternion GetSettingsAxis(GyroQuaternion gyroQuaternion)
{
float tmpX = 0, tmpY = 0, tmpZ = 0, tmpW = 0;
MethodInvoker methodInvokerDelegate = delegate()
{
switch (cbReplaceAxisX.SelectedIndex)
{
case 0: tmpX = gyroQuaternion.qX; break;
case 1: tmpX = gyroQuaternion.qY; break;
case 2: tmpX = gyroQuaternion.qZ; break;
}
switch (cbReplaceAxisY.SelectedIndex)
{
case 0: tmpY = gyroQuaternion.qX; break;
case 1: tmpY = gyroQuaternion.qY; break;
case 2: tmpY = gyroQuaternion.qZ; break;
}
switch (cbReplaceAxisZ.SelectedIndex)
{
case 0: tmpZ = gyroQuaternion.qX; break;
case 1: tmpZ = gyroQuaternion.qY; break;
case 2: tmpZ = gyroQuaternion.qZ; break;
}
tmpW = gyroQuaternion.qW;
if (chbInverseAxisX.Checked)
{
tmpX *= -1;
}
if (chbInverseAxisY.Checked)
{
tmpY *= -1;
}
if (chbInverseAxisZ.Checked)
{
tmpZ *= -1;
}
};
if (InvokeRequired)
{
Invoke(methodInvokerDelegate);
}
else
{
methodInvokerDelegate();
}
return(new GyroQuaternion(gyroQuaternion.sensorName, tmpX, tmpY, tmpZ, tmpW));
}
public static GyroQuaternion Multiply(GyroQuaternion q1, GyroQuaternion q2)
{
float qX = q1.qW * q2.qX + q1.qX * q2.qW + q1.qY * q2.qZ - q1.qZ * q2.qY;
float qY = q1.qW * q2.qY + q1.qY * q2.qW + q1.qZ * q2.qX - q1.qX * q2.qZ;
float qZ = q1.qW * q2.qZ + q1.qZ * q2.qW + q1.qX * q2.qY - q1.qY * q2.qX;
float qW = q1.qW * q2.qW - q1.qX * q2.qX - q1.qY * q2.qY - q1.qZ * q2.qZ;
return(new GyroQuaternion(q1.sensorName, qX, qY, qZ, qW));
}
private void Cube3DRender(GyroQuaternion quat)
{
Point3D point3D = GyroQuaternion.QuatToEuler(quat);
_cube3D.RotateX = point3D.X;
_cube3D.RotateY = point3D.Y;
_cube3D.RotateZ = point3D.Z;
Point origin = new Point(pbCube3D.Width / 2, pbCube3D.Height / 2);
pbCube3D.Image = _cube3D.DrawCube(origin);
}
public static GyroQuaternion Inverse(GyroQuaternion quat)
{
GyroQuaternion quaternion2 = new GyroQuaternion(quat.sensorName, 0, 0, 0, -1);
float num2 = quat.qX * quat.qX + quat.qY * quat.qY + quat.qZ * quat.qZ + quat.qW * quat.qW;
float num = 1f / num2;
quaternion2.qX = -quat.qX * num;
quaternion2.qY = -quat.qY * num;
quaternion2.qZ = -quat.qZ * num;
quaternion2.qW = quat.qW * num;
return(quaternion2);
}
public GyroQuaternion Filter(GyroQuaternion gyroQuaternion)
{
_quaternionQueue.Enqueue(gyroQuaternion);
GyroQuaternion smaValue = SimpleMovingAverageQuaternion.GetLastValue(_quaternionQueue.GetAllItems(), _period);
float qX = _kalmanFilterX.Filter(smaValue.qX);
float qY = _kalmanFilterY.Filter(smaValue.qY);
float qZ = _kalmanFilterZ.Filter(smaValue.qZ);
float qW = _kalmanFilterW.Filter(smaValue.qW);
GyroQuaternion result = new GyroQuaternion(gyroQuaternion.sensorName, qX, qY, qZ, qW);
return(result);
}
public static Point3D QuatToEuler(GyroQuaternion quat)
{
Point3D point3D = new Point3D();
float X = quat.qX;
float Y = quat.qY;
float Z = quat.qZ;
float W = quat.qW;
float singularityTest = Z * X - W * Y;
float yawY = 2f * (W * Z + X * Y);
float yawX = 1f - 2f * (Y * Y + Z * Z);
const float RAD_TO_DEG = (float)(180 / Math.PI);
if (singularityTest < -0.4999995f)
{
point3D.X = -90f;
point3D.Y = (float)Math.Atan2(yawY, yawX) * RAD_TO_DEG;
point3D.Z = NormalizeAxis((float)(-point3D.Y - 2f * Math.Atan2(X, W) * RAD_TO_DEG));
}
else if (singularityTest > 0.4999995f)
{
point3D.X = 90f;
point3D.Y = (float)Math.Atan2(yawY, yawX) * RAD_TO_DEG;
point3D.Z = NormalizeAxis((float)(point3D.Y - 2f * Math.Atan2(X, W) * RAD_TO_DEG));
}
else
{
point3D.X = (float)Math.Asin(2f * singularityTest) * RAD_TO_DEG;
point3D.Y = (float)Math.Atan2(yawY, yawX) * RAD_TO_DEG;
point3D.Z = (float)Math.Atan2(-2f * (W * X + Y * Z), 1f - 2f * (X * X + Y * Y)) * RAD_TO_DEG;
}
return(point3D);
}
public void Send(GyroQuaternion gyroQuaternion)
{
byte[] data = Encoding.ASCII.GetBytes(JsonConvert.SerializeObject(gyroQuaternion));
udpSocket.BeginSend(data, 0, data.Length, 0, new AsyncCallback(SendCallback), udpSocket);
sendDone.WaitOne();
}
public GyroQuaternionCalibrationModel(GyroQuaternion gyroQuaternion)
{
GyroQuaternion = gyroQuaternion;
}
public NewEventReceiveUDPServerArgs(GyroQuaternion quat)
{
ReceiveUDPServerMessage = quat;
}
