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