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