Пример #1
0
        private void OnTelemetryDataReceived(object sender, OnTelemetryDataReceivedEventArgs e)
        {
#pragma warning disable 4014
            switch (e.CommandType)
            {
                case CommandInformation.Information:
                    InformationOutputTextBox.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => InformationOutputTextBox.Text += (string) e.CommandData + "\n");
                    break;
                case CommandInformation.Servo:
                    var servoTelemetry = (ServoControllerData) e.CommandData;
                    switch (servoTelemetry.ServoId)
                    {
                        case 0:
                            ServoVelocityValue_Zero.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => ServoVelocityValue_Zero.Text = servoTelemetry.VelocityValue.ToString());
                            ServoVelocityBar_Zero.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => ServoVelocityBar_Zero.Value = Convert.ToInt32(servoTelemetry.VelocityValue/byte.MaxValue)*100);
                            break;
                        case 1:
                            ServoVelocityValue_One.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => ServoVelocityValue_One.Text = servoTelemetry.VelocityValue.ToString());
                            ServoVelocityBar_One.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => ServoVelocityBar_One.Value = Convert.ToInt32(servoTelemetry.VelocityValue / byte.MaxValue)*100);
                            break;
                        case 2:
                            ServoVelocityValue_Two.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => ServoVelocityValue_Two.Text = servoTelemetry.VelocityValue.ToString());
                            ServoVelocityBar_Two.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => ServoVelocityBar_Two.Value = Convert.ToInt32(servoTelemetry.VelocityValue / byte.MaxValue)*100);
                            break;
                        case 3:
                            ServoVelocityValue_Three.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => ServoVelocityValue_Three.Text = servoTelemetry.VelocityValue.ToString());
                            ServoVelocityBar_Three.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => ServoVelocityBar_Three.Value = Convert.ToInt32(servoTelemetry.VelocityValue / byte.MaxValue)*100);
                            break;
                    }
                    break;
                case CommandInformation.Accelerometer:
                    var accTelemetry = (AccelerometerData) e.CommandData;
                    AccelerationX_TextBlock.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => AccelerationX_TextBlock.Text = accTelemetry.Acceleration.X.ToString(CultureInfo.InvariantCulture));
                    AccelerationY_TextBlock.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => AccelerationY_TextBlock.Text = accTelemetry.Acceleration.Y.ToString(CultureInfo.InvariantCulture));
                    AccelerationZ_TextBlock.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => AccelerationZ_TextBlock.Text = accTelemetry.Acceleration.Z.ToString(CultureInfo.InvariantCulture));
                    break;
                case CommandInformation.Gyroscope:
                    var gyroTelemetry = (GyroscopeData) e.CommandData;
                    AngleX_TextBlock.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => AngleX_TextBlock.Text = gyroTelemetry.Angle.X.ToString(CultureInfo.InvariantCulture));
                    AngleY_TextBlock.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => AngleY_TextBlock.Text = gyroTelemetry.Angle.Y.ToString(CultureInfo.InvariantCulture));
                    AngleZ_TextBlock.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => AngleZ_TextBlock.Text = gyroTelemetry.Angle.Z.ToString(CultureInfo.InvariantCulture));
                    break;
            }
#pragma warning restore 4014
        }
Пример #2
0
        /// <summary>
        /// Method used for the "Collection Thread"
        /// </summary>
        /// <param name="operation"></param>
        private async void CollectTelemetryData(IAsyncAction operation)
        {
            DataReader reader;
            uint unconsumedBufferLength = 0;
            lock (m_pConnectedSocket)
            {
                reader = new DataReader(m_pConnectedSocket.InputStream);
            }

            while (IsClientConnected | (unconsumedBufferLength > 0))
            {
                await m_pTelemetryReaderSemaphoreSlim.WaitAsync();
                try
                {
                    await reader.LoadAsync(sizeof (byte));
                    unconsumedBufferLength = reader.UnconsumedBufferLength;
                }
                finally
                {
                    m_pTelemetryReaderSemaphoreSlim.Release();
                }
                if (reader.UnconsumedBufferLength <= 0) continue;
                await m_pTelemetryReaderSemaphoreSlim.WaitAsync();
                try
                {
                    byte b;
                    var e = new OnTelemetryDataReceivedEventArgs();
                    b = reader.ReadByte();
                    var commandInformation = (CommandInformation) b;
                    switch (commandInformation)
                    {
                        case CommandInformation.Information:
                            await reader.LoadAsync(sizeof (uint));
                            var n = reader.ReadUInt32();
                            await reader.LoadAsync(n*sizeof (char));
                            e.CommandData = reader.ReadString(n);
                            e.CommandType = CommandInformation.Information;
                            break;
                        case CommandInformation.Gyroscope:
                            float angleX, angleY, angleZ;
                            long gyroTime;
                            await reader.LoadAsync(3*sizeof (double));
                            await reader.LoadAsync(sizeof (long));
                            angleX = Convert.ToSingle(reader.ReadDouble());
                            angleY = Convert.ToSingle(reader.ReadDouble());
                            angleZ = Convert.ToSingle(reader.ReadDouble());
                            gyroTime = reader.ReadInt64();
                            e.CommandData = new GyroscopeData()
                            {
                                Angle = new Vector3(angleX, angleY, angleZ),
                                TimeStamp = new DateTime(gyroTime)
                            };
                            e.CommandType = CommandInformation.Gyroscope;
                            break;
                        case CommandInformation.Accelerometer:
                            float accX, accY, accZ;
                            long accTime;
                            await reader.LoadAsync(3*sizeof (double));
                            await reader.LoadAsync(sizeof (long));
                            accX = Convert.ToSingle(reader.ReadDouble());
                            accY = Convert.ToSingle(reader.ReadDouble());
                            accZ = Convert.ToSingle(reader.ReadDouble());
                            accTime = reader.ReadInt64();
                            e.CommandData = new AccelerometerData()
                            {
                                Acceleration = new Vector3(accX, accY, accZ),
                                TimeStamp = new DateTime(accTime)
                            };
                            e.CommandType = CommandInformation.Accelerometer;
                            break;
                        case CommandInformation.Servo:
                            byte id, velocity;
                            long servoTime;
                            await reader.LoadAsync(2*sizeof (byte));
                            await reader.LoadAsync(sizeof (long));
                            id = reader.ReadByte();
                            velocity = reader.ReadByte();
                            servoTime = reader.ReadInt64();
                            e.CommandData = new ServoControllerData() {ServoId = id, VelocityValue = velocity, TimeStamp = new DateTime(servoTime)};
                            e.CommandType = CommandInformation.Servo;
                            break;
                    }
                    var handler = OnTelemetryDataReceived;
                    handler?.Invoke(this, e);
                }
                finally
                {
                    m_pTelemetryReaderSemaphoreSlim.Release();
                }
            }
        }