public void CopyFields(BNGAPI other)
{
posX = other.posX;
posY = other.posY;
posZ = other.posZ;
velX = other.velX;
velY = other.velY;
velZ = other.velZ;
accX = other.accX;
accY = other.accY;
accZ = other.accZ;
upVecX = other.upVecX;
upVecY = other.upVecY;
upVecZ = other.upVecZ;
rollPos = other.rollPos;
pitchPos = other.pitchPos;
yawPos = other.yawPos;
rollRate = other.rollRate;
pitchRate = other.pitchRate;
yawRate = other.yawRate;
rollAcc = other.rollAcc;
pitchAcc = other.pitchAcc;
yawAcc = other.yawAcc;
}
public byte[] ToByteArray()
{
BNGAPI packet = this;
int num = Marshal.SizeOf <BNGAPI>(packet);
byte[] array = new byte[num];
IntPtr intPtr = Marshal.AllocHGlobal(num);
Marshal.StructureToPtr <BNGAPI>(packet, intPtr, false);
Marshal.Copy(intPtr, array, 0, num);
Marshal.FreeHGlobal(intPtr);
return(array);
}
/// <summary>
/// The thread funktion to poll the telemetry data and send TelemetryUpdated events.
/// </summary>
private void Run()
{
BNGAPI lastTelemetryData = new BNGAPI();
lastTelemetryData.Reset();
Vector3 lastVelocity = Vector3.Zero;
Session session = new Session();
Stopwatch sw = new Stopwatch();
sw.Start();
KalmanFilter accXFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
KalmanFilter accYFilter = new KalmanFilter(1, 1, 0.05f, 1, 0.1f, 0.0f);
KalmanFilter accZFilter = new KalmanFilter(1, 1, 0.05f, 1, 0.05f, 0.0f); //heave noisy af
NoiseFilter accXSmooth = new NoiseFilter(6, 1.0f);
NoiseFilter accYSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter accZSmooth = new NoiseFilter(6, 1.0f);
KalmanFilter velXFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
KalmanFilter velYFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
NoiseFilter velXSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter velYSmooth = new NoiseFilter(6, 0.5f);
KalmanFilter yawRateFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
NoiseFilter yawRateSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter pitchFilter = new NoiseFilter(3);
NoiseFilter rollFilter = new NoiseFilter(3);
NoiseFilter yawFilter = new NoiseFilter(3);
NoiseFilter slipAngleSmooth = new NoiseFilter(6, 0.25f);
UdpClient socket = new UdpClient();
socket.ExclusiveAddressUse = false;
socket.Client.Bind(new IPEndPoint(IPAddress.Any, PORTNUM));
Log("Listener started (port: " + PORTNUM.ToString() + ") BNGTelemetryProvider.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);
BNGAPI telemetryData = (BNGAPI)Marshal.PtrToStructure(alloc.AddrOfPinnedObject(), typeof(BNGAPI));
// otherwise we are connected
IsConnected = true;
if (telemetryData.magic[0] == 'B' &&
telemetryData.magic[1] == 'N' &&
telemetryData.magic[2] == 'G' &&
telemetryData.magic[3] == '1')
{
IsRunning = true;
sw.Restart();
BNGAPI telemetryToSend = new BNGAPI();
telemetryToSend.Reset();
telemetryToSend.CopyFields(telemetryData);
telemetryToSend.accX = accXSmooth.Filter(accXFilter.Filter(telemetryData.accX));
telemetryToSend.accY = accYSmooth.Filter(accYFilter.Filter(telemetryData.accY));
telemetryToSend.accZ = accZSmooth.Filter(accZFilter.Filter(telemetryData.accZ));
telemetryToSend.pitchPos = pitchFilter.Filter(telemetryData.pitchPos);
telemetryToSend.rollPos = rollFilter.Filter(telemetryData.rollPos);
telemetryToSend.yawPos = yawFilter.Filter(telemetryData.yawPos);
telemetryToSend.velX = velXSmooth.Filter(velXFilter.Filter(telemetryData.velX));
telemetryToSend.velY = velYSmooth.Filter(velYFilter.Filter(telemetryData.velY));
telemetryToSend.yawRate = yawRateSmooth.Filter(yawRateFilter.Filter(telemetryData.yawRate));
telemetryToSend.yawAcc = slipAngleSmooth.Filter(telemetryToSend.CalculateSlipAngle());
TelemetryEventArgs args = new TelemetryEventArgs(
new BNGTelemetryInfo(telemetryToSend, lastTelemetryData));
RaiseEvent(OnTelemetryUpdate, args);
lastTelemetryData = telemetryData;
}
else if (sw.ElapsedMilliseconds > 500)
{
IsRunning = false;
}
}
catch (Exception e)
{
LogError("BNGTelemetryProvider Exception while processing data", e);
IsConnected = false;
IsRunning = false;
Thread.Sleep(1000);
}
}
socket.Close();
IsConnected = false;
IsRunning = false;
}
public BNGTelemetryInfo(BNGAPI telemetryData, BNGAPI lastTelemetryData)
{
_telemetryData = telemetryData;
_lastTelemetryData = lastTelemetryData;
}
