void AddRawAPIHistory(SFUAPI api)
{
rawTelemetryHistory.Add(api);
if (rawTelemetryHistory.Count > maxTelemetryHistory)
{
rawTelemetryHistory.RemoveAt(0);
}
}
void AddFilteredAPIHistory(SFUAPI api)
{
filteredTelemetryHistory.Add(api);
if (filteredTelemetryHistory.Count > maxTelemetryHistory)
{
filteredTelemetryHistory.RemoveAt(0);
}
}
public byte[] ToByteArray()
{
SFUAPI packet = this;
int num = Marshal.SizeOf <SFUAPI>(packet);
byte[] array = new byte[num];
IntPtr intPtr = Marshal.AllocHGlobal(num);
Marshal.StructureToPtr <SFUAPI>(packet, intPtr, false);
Marshal.Copy(intPtr, array, 0, num);
Marshal.FreeHGlobal(intPtr);
return(array);
}
public float GetAPIValue(SFUAPI api)
{
float value = 0.0f;
Type eleDataType = typeof(SFUAPI);
if (fieldInfo != null)
{
value = (float)fieldInfo.GetValue(api);
}
else
if (propertyInfo != null)
{
value = (float)propertyInfo.GetValue(api, null);
}
return(value);
}
public void CopyFields(SFUAPI other)
{
velX = other.velX;
velY = other.velY;
velZ = other.velZ;
accX = other.accX;
accY = other.accY;
accZ = other.accZ;
pitch = other.pitch;
yaw = other.yaw;
roll = other.roll;
pitchVel = other.pitchVel;
yawVel = other.yawVel;
rollVel = other.rollVel;
rollAcc = other.rollAcc;
pitchAcc = other.pitchAcc;
yawAcc = other.yawAcc;
slipAngle = other.slipAngle;
}
/// <summary>
/// The thread funktion to poll the telemetry data and send TelemetryUpdated events.
/// </summary>
private void Run()
{
SFUAPI lastTelemetryData = new SFUAPI();
lastTelemetryData.Reset();
Session session = new Session();
Stopwatch sw = new Stopwatch();
sw.Start();
UdpClient socket = new UdpClient();
socket.ExclusiveAddressUse = false;
socket.Client.Bind(new IPEndPoint(IPAddress.Any, PORTNUM));
Log("Listener started (port: " + PORTNUM.ToString() + ") SFUTelemetryProvider.Thread");
while (!isStopped)
{
try
{
// get data from game,
if (socket.Available == 0)
{
if (sw.ElapsedMilliseconds > 500)
{
IsRunning = false;
IsConnected = false;
Thread.Sleep(1000);
}
continue;
}
else
{
IsConnected = true;
}
Byte[] received = socket.Receive(ref _senderIP);
var alloc = GCHandle.Alloc(received, GCHandleType.Pinned);
SFUAPI telemetryData = (SFUAPI)Marshal.PtrToStructure(alloc.AddrOfPinnedObject(), typeof(SFUAPI));
// otherwise we are connected
IsConnected = true;
if (telemetryData.packetID != lastTelemetryData.packetID)
{
IsRunning = true;
sw.Restart();
TelemetryEventArgs args = new TelemetryEventArgs(
new SFUTelemetryInfo(telemetryData, lastTelemetryData));
RaiseEvent(OnTelemetryUpdate, args);
lastTelemetryData = telemetryData;
}
else if (sw.ElapsedMilliseconds > 500)
{
IsRunning = false;
}
}
catch (Exception e)
{
LogError("SFUTelemetryProvider Exception while processing data", e);
IsConnected = false;
IsRunning = false;
Thread.Sleep(1000);
}
}
socket.Close();
IsConnected = false;
IsRunning = false;
}
public SFUTelemetryInfo(SFUAPI telemetryData, SFUAPI lastTelemetryData)
{
_telemetryData = telemetryData;
_lastTelemetryData = lastTelemetryData;
}
// Update is called once per frame
void LateUpdate()
{
if (!active)
{
return;
}
float deltaTime = Time.deltaTime;
if (deltaTime < 0.0f)
{
return;
}
float dtRecip = 1.0f / deltaTime;
Vector3 velocity = (telemetryTransform.position - lastPosition) * dtRecip;
Vector3 lastLocalVelocity = telemetryTransform.InverseTransformDirection(lastVelocity);
Vector3 localVelocity = telemetryTransform.InverseTransformDirection(velocity);
Vector3 localAcceleration = localVelocity - lastLocalVelocity;
dataPacket.accX = localAcceleration.x;
dataPacket.accY = localAcceleration.y;
dataPacket.accZ = localAcceleration.z;
Vector3 angles = telemetryTransform.rotation.eulerAngles;
dataPacket.pitch = LoopAngle(angles.x);
dataPacket.yaw = angles.y;
dataPacket.roll = LoopAngle(angles.z);
Vector3 angularVelocity = new Vector3(Mathf.DeltaAngle(lastAngles.x, angles.x), Mathf.DeltaAngle(lastAngles.y, angles.y), Mathf.DeltaAngle(lastAngles.z, angles.z)) * dtRecip;
dataPacket.pitchVel = angularVelocity.x;
dataPacket.yawVel = angularVelocity.y;
dataPacket.rollVel = angularVelocity.z;
Vector3 rotAcc = angularVelocity - lastAngularVelocity;
dataPacket.pitchAcc = rotAcc.x;
dataPacket.yawAcc = rotAcc.y;
dataPacket.rollAcc = rotAcc.z;
lastPosition = telemetryTransform.position;
lastRotation = telemetryTransform.rotation;
lastAngles = new Vector3(dataPacket.pitch, dataPacket.yaw, dataPacket.roll);
//loop packet id
if (dataPacket.packetID == int.MaxValue)
{
dataPacket.packetID = 0;
}
dataPacket.packetID++;
//copy raw packet to history
SFUAPI rawCopy = new SFUAPI();
rawCopy.CopyFields(dataPacket);
AddRawAPIHistory(rawCopy);
Filter();
//copy filtered packet to history
SFUAPI filteredCopy = new SFUAPI();
filteredCopy.CopyFields(dataPacket);
AddFilteredAPIHistory(filteredCopy);
byte[] bytes = dataPacket.ToByteArray();
telemetrySender.SendAsync(bytes);
}
