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