public virtual void SendFilteredData() { byte[] bytes = filteredData.GetBytes(); mutex.WaitOne(); if (fillMMF) { using (MemoryMappedViewStream stream = filteredMMF.CreateViewStream()) { BinaryWriter writer = new BinaryWriter(stream); writer.Write(bytes); } } if (sendUDP) { telemetrySender.SendAsync(bytes); } lastFilteredData.Copy(filteredData); mutex.ReleaseMutex(); }
public void Filter(CMCustomUDPData dataIn, ref CMCustomUDPData dataOut, int keyMask = int.MaxValue, bool newHistory = true) { if (filters == null) { return; } mutex.WaitOne(); CMCustomUDPData newRawData; CMCustomUDPData newFiltered; if (newHistory == true) { dataOut.Copy(dataIn); //copy to raw history newRawData = new CMCustomUDPData(); newRawData.Copy(dataIn); rawData.Add(newRawData); if (rawData.Count > maxHistorySamples) { rawData.RemoveAt(0); } //add new filtered history newFiltered = new CMCustomUDPData(); newFiltered.Copy(dataIn); filteredData.Add(newFiltered); if (filteredData.Count > maxHistorySamples) { filteredData.RemoveAt(0); } } else { newFiltered = filteredData[filteredData.Count - 1]; newRawData = rawData[rawData.Count - 1]; } //do filter lists for each key if it exists for (int i = 0; i < filters.Length; ++i) { CMCustomUDPData.DataKey key = (CMCustomUDPData.DataKey)i; List <FilterBase> filterList = filters[i]; if (!dataIn.IsValid(key)) { continue; } object value = dataIn.GetValue(key); if (filterList == null || filterList.Count == 0 || !dataIn.IsFloat(key)) { newFiltered.SetValue(key, value); newRawData.SetValue(key, value); continue; } //ignore filter if not in keyMask if (((1 << i) & keyMask) == 0) { continue; } //get float val float floatVal = (float)value; //do filters for key foreach (FilterBase filter in filterList) { floatVal = filter.Filter(floatVal); } //filtered newFiltered.SetValue(key, floatVal); dataOut.SetValue(key, floatVal); } mutex.ReleaseMutex(); }
/// <summary> /// The thread funktion to poll the telemetry data and send TelemetryUpdated events. /// </summary> private void Run() { isStopped = false; smoothInTimer = 0.0f; startWaitTimer = startWaitTime; CMCustomUDPData telemetryData = new CMCustomUDPData(); CMCustomUDPData.formatFilename = "CMCustomUDP/CMCustomUDPFormat.xml"; telemetryData.Init(); Stopwatch sw = new Stopwatch(); sw.Start(); Stopwatch processSW = new Stopwatch(); int readSize = telemetryData.GetSize(); byte[] readBuffer; UdpClient socket = null; if (config.receiveUDP) { socket = new UdpClient(); socket.ExclusiveAddressUse = false; socket.Client.Bind(new IPEndPoint(IPAddress.Any, config.udpPort)); } MemoryMappedFile mmf = null; while (!isStopped) { try { float dt = (float)sw.ElapsedMilliseconds / 1000.0f; if (config.receiveUDP) { while (true) { if (socket.Available == 0) { if (sw.ElapsedMilliseconds > 500) { Thread.Sleep(1000); } continue; } else { break; } } } else { while (true) { try { mmf = MemoryMappedFile.OpenExisting("GenericTelemetryProviderFiltered"); if (mmf != null) { break; } else { Thread.Sleep(1000); } } catch (FileNotFoundException) { Thread.Sleep(1000); } } } if (config.receiveUDP) { readBuffer = socket.Receive(ref senderIP); } else { Mutex mutex = Mutex.OpenExisting("GenericTelemetryProviderMutex"); mutex.WaitOne(); using (MemoryMappedViewStream stream = mmf.CreateViewStream()) { BinaryReader reader = new BinaryReader(stream); readBuffer = reader.ReadBytes(readSize); } mutex.ReleaseMutex(); } telemetryData.FromBytes(readBuffer); CMCustomUDPData telemetryToSend = new CMCustomUDPData(); telemetryToSend.Copy(telemetryData); //wait for start because there is a delay between when telem starts sending and platform is updated if (startWaitTimer > 0.0f) { startWaitTimer -= dt; telemetryToSend.LerpAllFromZero(0.0f); //done, so smooth in if (startWaitTimer <= 0.0f) { smoothInTimer = smoothInTime; } } else //smooth start transition if (smoothInTimer > 0.0f) { float lerp = 1.0f - (smoothInTimer / smoothInTime); smoothInTimer -= dt; telemetryToSend.LerpAllFromZero(lerp); } IsConnected = true; if (IsConnected) { IsRunning = true; sw.Restart(); TelemetryEventArgs args = new TelemetryEventArgs( new GTPTelemetryInfo(telemetryToSend)); RaiseEvent(OnTelemetryUpdate, args); bool sleep = true; if (config.receiveUDP) { if (socket.Available != 0) { sleep = false; } } if (sleep) { using (var sleeper = new ManualResetEvent(false)) { int processTime = (int)processSW.ElapsedMilliseconds; sleeper.WaitOne(Math.Max(0, 10 - processTime)); } processSW.Restart(); } } else if (sw.ElapsedMilliseconds > 500) { IsRunning = false; } } catch (Exception e) { LogError("GTPTelemetryProvider Exception while processing data", e); mmf.Dispose(); IsConnected = false; IsRunning = false; Thread.Sleep(1000); } } if (socket != null) { socket.Close(); } if (mmf != null) { mmf.Dispose(); } IsConnected = false; IsRunning = false; }
public virtual bool ProcessTransform(Matrix4x4 inTransform, float inDT) { transform = inTransform; dt = inDT; if (!ExtractFwdUpRht()) { droppedSampleCount = int.MaxValue; return(false); } if (!CheckLastFrameValid()) { return(true); } FilterDT(); if (CalcPosition()) { droppedSampleCount = 0; CalcVelocity(); FilterVelocity(); CalcAcceleration(); CalcAngles(); /* * //debug * using (MemoryMappedViewStream stream = mmf.CreateViewStream()) * { * BinaryReader reader = new BinaryReader(stream); * byte[] readBuffer = reader.ReadBytes((int)stream.Length); * * var alloc = GCHandle.Alloc(readBuffer, GCHandleType.Pinned); * GenericProviderData readTelemetry = (GenericProviderData)Marshal.PtrToStructure(alloc.AddrOfPinnedObject(), typeof(GenericProviderData)); * alloc.Free(); * } */ CalcAngularVelocityAndAccel(); SimulateSuspension(); SimulateEngine(); ProcessInputs(); //Filter everything besides position, velocity, angular velocity, suspension velocity FilterModuleCustom.Instance.Filter(rawData, ref filteredData, int.MaxValue & ~(posKeyMask | velKeyMask | angVelKeyMask | suspVelKeyMask | accelKeyMask), false); HandleTelemetryPaused(); } else { droppedSampleCount++; filteredData.Copy(lastFilteredData); } lastTransform = transform; return(true); }