Exemplo n.º 1
0
        /// <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 };
        }
Exemplo n.º 2
0
    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);
    }
Exemplo n.º 3
0
        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));
            }));
        }
Exemplo n.º 4
0
        /// <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 };
        }
Exemplo n.º 5
0
        /// <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;
        }
Exemplo n.º 7
0
        /// <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;
        }
Exemplo n.º 8
0
 public override float Filter(float value)
 {
     return(filter.Filter(value));
 }