/// <summary> /// Calculates the fourier transforms and updates the axis scales. /// </summary> /// <param name="sender"></param> /// <param name="e"></param> private void BackgroundWorker_CalculateFourierTransforms(object sender, DoWorkEventArgs e) { BackgroundWorker worker = (BackgroundWorker)sender; float[] pidFFTW = FourierTransform.CalculateFFTW(PIDOutput.ToArray()); float[] adrcFFTW = FourierTransform.CalculateFFTW(ADRCOutput.ToArray()); float[] pidAngleFFTW = FourierTransform.CalculateFFTW(PIDAngle.ToArray()); float[] adrcAngleFFTW = FourierTransform.CalculateFFTW(ADRCAngle.ToArray()); this.BeginInvoke((Action)(() => { double pidMax, adrcMax; pidMax = pidMaxValue.Filter((pidFFTW.Max() + Math.Abs(pidFFTW.Min())) / 2); adrcMax = adrcMaxValue.Filter((adrcFFTW.Max() + Math.Abs(adrcFFTW.Min())) / 2); pidScale.Text = "PID Scale: " + pidMax; adrcScale.Text = "ADRC Scale: " + adrcMax; chart3.ChartAreas[0].AxisY.Maximum = pidMax; chart3.ChartAreas[0].AxisY.Minimum = -pidMax; chart4.ChartAreas[0].AxisY.Maximum = adrcMax; chart4.ChartAreas[0].AxisY.Minimum = -adrcMax; })); e.Result = new float[4][] { pidFFTW, adrcFFTW, pidAngleFFTW, adrcAngleFFTW }; }
public GyroQuaternion Filter(GyroQuaternion gyroQuaternion) { _quaternionQueue.Enqueue(gyroQuaternion); GyroQuaternion smaValue = SimpleMovingAverageQuaternion.GetLastValue(_quaternionQueue.GetAllItems(), _period); float qX = _kalmanFilterX.Filter(smaValue.qX); float qY = _kalmanFilterY.Filter(smaValue.qY); float qZ = _kalmanFilterZ.Filter(smaValue.qZ); float qW = _kalmanFilterW.Filter(smaValue.qW); GyroQuaternion result = new GyroQuaternion(gyroQuaternion.sensorName, qX, qY, qZ, qW); return(result); }
public static IObservable <AngularVelocity3> Filter( this IObservable <AngularVelocity3> source, double estimatedMeasurementNoiseCovarianceX, double estimatedProcessNoiseCovarianceX, double controlWeightX, double initialEstimateX, double initialErrorCovarianceX, double estimatedMeasurementNoiseCovarianceY, double estimatedProcessNoiseCovarianceY, double controlWeightY, double initialEstimateY, double initialErrorCovarianceY, double estimatedMeasurementNoiseCovarianceZ, double estimatedProcessNoiseCovarianceZ, double controlWeightZ, double initialEstimateZ, double initialErrorCovarianceZ) { var filterX = new KalmanFilter(estimatedMeasurementNoiseCovarianceX, estimatedProcessNoiseCovarianceX, controlWeightX, initialEstimateX, initialErrorCovarianceX); var filterY = new KalmanFilter(estimatedMeasurementNoiseCovarianceY, estimatedProcessNoiseCovarianceY, controlWeightY, initialEstimateY, initialErrorCovarianceY); var filterZ = new KalmanFilter(estimatedMeasurementNoiseCovarianceZ, estimatedProcessNoiseCovarianceZ, controlWeightZ, initialEstimateZ, initialErrorCovarianceZ); return(source .Select(angularVelocity => { return new AngularVelocity3( filterX.Filter(angularVelocity.X), filterY.Filter(angularVelocity.Y), filterZ.Filter(angularVelocity.Z)); })); }
/// <summary> /// Calculates the fourier transforms and updates the axis scales. /// </summary> /// <param name="sender"></param> /// <param name="e"></param> private void CalculateFourierTransforms(object sender, DoWorkEventArgs e) { BackgroundWorker worker = (BackgroundWorker)sender; float[] FFTW = FourierTransform.CalculateFFTW(PreviousOutputs.ToArray()); float[] AngleFFTW = FourierTransform.CalculateFFTW(PreviousAngles.ToArray()); this.BeginInvoke((Action)(() => { double fourierMaxOutput = feedbackMaxKalman.Filter((FFTW.Max() + Math.Abs(FFTW.Min())) / 2); pidScale.Text = "Scale: " + fourierMaxOutput; chart3.ChartAreas[0].AxisY.Maximum = fourierMaxOutput; chart3.ChartAreas[0].AxisY.Minimum = -fourierMaxOutput; })); e.Result = new float[2][] { FFTW, AngleFFTW }; }
/// <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(); }
/// <summary> /// The thread funktion to poll the telemetry data and send TelemetryUpdated events. /// </summary> private void Run() { isStopped = false; DIRT5API lastTelemetryData = new DIRT5API(); 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); int readSize = 4 * 4 * 4; byte[] readBuffer; MemoryMappedFile mmf = null; while (!isStopped) { try { float dt = (float)sw.ElapsedMilliseconds / 1000.0f; while (true) { try { mmf = MemoryMappedFile.OpenExisting("Dirt5MatrixProvider"); if (mmf != null) { break; } else { Thread.Sleep(1000); } } catch (FileNotFoundException) { Thread.Sleep(1000); } } Mutex mutex = Mutex.OpenExisting("Dirt5MatrixProviderMutex"); mutex.WaitOne(); using (MemoryMappedViewStream stream = mmf.CreateViewStream()) { BinaryReader reader = new BinaryReader(stream); readBuffer = reader.ReadBytes(readSize); } mutex.ReleaseMutex(); 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; break; } if (!lastFrameValid) { lastTransform = transform; lastFrameValid = true; lastVelocity = Vector3.Zero; lastYaw = 0.0f; continue; } DIRT5API telemetryData = new DIRT5API(); 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; rotation.M44 = 1.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); // Debug.WriteLine( "---Roll = " + roll + " " + Math.Sign( rht.Y ) ); } else { roll = -(float)Math.Asin(Vector3.Dot(up, rhtPlane)); // Debug.WriteLine( "Roll = " + roll + " " + Math.Sign(rht.Y) ); } // Debug.WriteLine( "" ); 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; DIRT5API telemetryToSend = new DIRT5API(); 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 DIRT5TelemetryInfo(telemetryToSend, lastTelemetryData)); RaiseEvent(OnTelemetryUpdate, args); lastTelemetryData = telemetryToSend; Thread.Sleep(1000 / 100); } else if (sw.ElapsedMilliseconds > 500) { IsRunning = false; } } catch (Exception e) { LogError("DIRT5TelemetryProvider Exception while processing data", e); mmf.Dispose(); IsConnected = false; IsRunning = false; Thread.Sleep(1000); } } mmf.Dispose(); IsConnected = false; IsRunning = false; }
/// <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 override float Filter(float value) { return(filter.Filter(value)); }