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; }