public void CopyFields(WFSTAPI 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()
{
WFSTAPI packet = this;
int num = Marshal.SizeOf <WFSTAPI>(packet);
byte[] array = new byte[num];
IntPtr intPtr = Marshal.AllocHGlobal(num);
Marshal.StructureToPtr <WFSTAPI>(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()
{
isStopped = false;
WFSTAPI lastTelemetryData = new WFSTAPI();
lastTelemetryData.Reset();
Matrix4x4 lastTransform = Matrix4x4.Identity;
bool lastFrameValid = false;
Vector3 lastVelocity = Vector3.Zero;
float lastYaw = 0.0f;
Stopwatch sw = new Stopwatch();
sw.Start();
NestedSmooth accXSmooth = new NestedSmooth(3, 6, 0.5f);
NestedSmooth accYSmooth = new NestedSmooth(3, 6, 0.5f);
NestedSmooth accZSmooth = new NestedSmooth(3, 6, 0.5f);
KalmanFilter velXFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
KalmanFilter velZFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
NoiseFilter velXSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter velZSmooth = 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);
KalmanFilter posXFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.1f, 0.0f);
KalmanFilter posYFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.1f, 0.0f);
KalmanFilter posZFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.1f, 0.0f);
NestedSmooth posXSmooth = new NestedSmooth(12, 6, 0.5f);
NestedSmooth posYSmooth = new NestedSmooth(12, 6, 0.5f);
NestedSmooth posZSmooth = new NestedSmooth(12, 6, 0.5f);
NoiseFilter slipAngleSmooth = new NoiseFilter(6, 0.25f);
ProcessMemoryReader reader = new ProcessMemoryReader();
reader.ReadProcess = wfstProcess;
uint readSize = 4 * 4 * 4;
byte[] readBuffer = new byte[readSize];
reader.OpenProcess();
while (!isStopped)
{
try
{
float dt = (float)sw.ElapsedMilliseconds / 1000.0f;
int byteReadSize;
reader.ReadProcessMemory((IntPtr)memoryAddress, readSize, out byteReadSize, readBuffer);
if (byteReadSize == 0)
{
continue;
}
float[] floats = new float[4 * 4];
Buffer.BlockCopy(readBuffer, 0, floats, 0, readBuffer.Length);
Matrix4x4 transform = new Matrix4x4(floats[0], floats[1], floats[2], floats[3]
, floats[4], floats[5], floats[6], floats[7]
, floats[8], floats[9], floats[10], floats[11]
, floats[12], floats[13], floats[14], floats[15]);
Vector3 rht = new Vector3(transform.M11, transform.M12, transform.M13);
Vector3 up = new Vector3(transform.M21, transform.M22, transform.M23);
Vector3 fwd = new Vector3(transform.M31, transform.M32, transform.M33);
float rhtMag = rht.Length();
float upMag = up.Length();
float fwdMag = fwd.Length();
//reading garbage
if (rhtMag < 0.9f || upMag < 0.9f || fwdMag < 0.9f)
{
IsConnected = false;
IsRunning = false;
TelemetryLost();
break;
}
if (!lastFrameValid)
{
lastTransform = transform;
lastFrameValid = true;
lastVelocity = Vector3.Zero;
lastYaw = 0.0f;
continue;
}
WFSTAPI telemetryData = new WFSTAPI();
if (dt <= 0)
{
dt = 1.0f;
}
Vector3 worldVelocity = (transform.Translation - lastTransform.Translation) / dt;
lastTransform = transform;
Matrix4x4 rotation = new Matrix4x4();
rotation = transform;
rotation.M41 = 0.0f;
rotation.M42 = 0.0f;
rotation.M43 = 0.0f;
Matrix4x4 rotInv = new Matrix4x4();
Matrix4x4.Invert(rotation, out rotInv);
Vector3 localVelocity = Vector3.Transform(worldVelocity, rotInv);
telemetryData.velX = worldVelocity.X;
telemetryData.velZ = worldVelocity.Z;
Vector3 localAcceleration = localVelocity - lastVelocity;
lastVelocity = localVelocity;
telemetryData.accX = localAcceleration.X * 10.0f;
telemetryData.accY = localAcceleration.Y * 100.0f;
telemetryData.accZ = localAcceleration.Z * 10.0f;
float pitch = (float)Math.Asin(-fwd.Y);
float yaw = (float)Math.Atan2(fwd.X, fwd.Z);
float roll = 0.0f;
Vector3 rhtPlane = rht;
rhtPlane.Y = 0;
rhtPlane = Vector3.Normalize(rhtPlane);
if (rhtPlane.Length() <= float.Epsilon)
{
roll = -(float)(Math.Sign(rht.Y) * Math.PI * 0.5f);
}
else
{
roll = -(float)Math.Asin(Vector3.Dot(up, rhtPlane));
}
telemetryData.pitchPos = pitch;
telemetryData.yawPos = yaw;
telemetryData.rollPos = roll;
telemetryData.yawRate = CalculateAngularChange(lastYaw, yaw) * (180.0f / (float)Math.PI);
lastYaw = yaw;
// otherwise we are connected
IsConnected = true;
if (IsConnected)
{
IsRunning = true;
WFSTAPI telemetryToSend = new WFSTAPI();
telemetryToSend.Reset();
telemetryToSend.CopyFields(telemetryData);
telemetryToSend.accX = accXSmooth.Filter(telemetryData.accX);
telemetryToSend.accY = accYSmooth.Filter(telemetryData.accY);
telemetryToSend.accZ = accZSmooth.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.velZ = velZSmooth.Filter(velZFilter.Filter(telemetryData.velZ));
telemetryToSend.yawRate = yawRateSmooth.Filter(yawRateFilter.Filter(telemetryData.yawRate));
telemetryToSend.yawAcc = slipAngleSmooth.Filter(telemetryToSend.CalculateSlipAngle());
sw.Restart();
TelemetryEventArgs args = new TelemetryEventArgs(
new WFSTTelemetryInfo(telemetryToSend, lastTelemetryData));
RaiseEvent(OnTelemetryUpdate, args);
lastTelemetryData = telemetryToSend;
Thread.Sleep(1000 / 100);
}
else if (sw.ElapsedMilliseconds > 500)
{
IsRunning = false;
}
}
catch (Exception e)
{
LogError("WFSTTelemetryProvider Exception while processing data", e);
IsConnected = false;
IsRunning = false;
Thread.Sleep(1000);
}
}
IsConnected = false;
IsRunning = false;
reader.CloseHandle();
}
public WFSTTelemetryInfo(WFSTAPI telemetryData, WFSTAPI lastTelemetryData)
{
_telemetryData = telemetryData;
_lastTelemetryData = lastTelemetryData;
}