private void OnMavlinkMessageReceived(object sender, MavLinkMessage e)
{
if (currentFlightLog != null && !pauseRecording)
{
currentFlightLog.AddMessage(e);
}
switch (e.MsgId)
{
case MAVLink.MAVLINK_MSG_ID.ATTITUDE_QUATERNION:
{
var payload = (MAVLink.mavlink_attitude_quaternion_t)e.TypedPayload;
var q = new System.Windows.Media.Media3D.Quaternion(payload.q1, payload.q2, payload.q3, payload.q4);
UiDispatcher.RunOnUIThread(() =>
{
//ModelViewer.ModelAttitude = initialAttitude * q;
});
break;
}
case MAVLink.MAVLINK_MSG_ID.ATTITUDE:
{
var payload = (MAVLink.mavlink_attitude_t)e.TypedPayload;
Quaternion y = new Quaternion(new Vector3D(0, 0, 1), -payload.yaw * 180 / Math.PI);
Quaternion x = new Quaternion(new Vector3D(1, 0, 0), payload.pitch * 180 / Math.PI);
Quaternion z = new Quaternion(new Vector3D(0, 1, 0), payload.roll * 180 / Math.PI);
UiDispatcher.RunOnUIThread(() =>
{
ModelViewer.ModelAttitude = initialAttitude * (y * x * z);
});
break;
}
case MAVLink.MAVLINK_MSG_ID.HIL_STATE_QUATERNION:
{
var payload = (MAVLink.mavlink_hil_state_quaternion_t)e.TypedPayload;
Quaternion q = new Quaternion(payload.attitude_quaternion[0],
payload.attitude_quaternion[1],
payload.attitude_quaternion[2],
payload.attitude_quaternion[3]);
UiDispatcher.RunOnUIThread(() =>
{
ModelViewer.ModelAttitude = initialAttitude * q;
});
break;
}
case MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT:
{
var payload = (MAVLink.mavlink_global_position_int_t)e.TypedPayload;
UiDispatcher.RunOnUIThread(() =>
{
MapLocation((double)payload.lat / 1e7, (double)payload.lon / 1e7);
});
break;
}
case MAVLink.MAVLINK_MSG_ID.SERIAL_CONTROL:
{
var ctrl = (MAVLink.mavlink_serial_control_t)e.TypedPayload;
if (ctrl.count > 0)
{
string text = System.Text.Encoding.ASCII.GetString(ctrl.data, 0, ctrl.count);
UiDispatcher.RunOnUIThread(() =>
{
SystemConsole.Write(text);
});
}
break;
}
case MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE:
if (showImageStream)
{
var p = (MAVLink.mavlink_data_transmission_handshake_t)e.TypedPayload;
incoming_image.size = p.size;
incoming_image.packets = p.packets;
incoming_image.payload = p.payload;
incoming_image.quality = p.jpg_quality;
incoming_image.type = p.type;
incoming_image.width = p.width;
incoming_image.height = p.height;
incoming_image.start = Environment.TickCount;
incoming_image.packetsArrived = 0;
incoming_image.data = new byte[incoming_image.size];
}
break;
case MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA:
if (showImageStream)
{
var img = (MAVLink.mavlink_encapsulated_data_t)e.TypedPayload;
int seq = img.seqnr;
uint pos = (uint)seq * (uint)incoming_image.payload;
// Check if we have a valid transaction
if (incoming_image.packets == 0 || incoming_image.size == 0)
{
// not expecting an image?
incoming_image.packetsArrived = 0;
break;
}
uint available = (uint)incoming_image.payload;
if (pos + available > incoming_image.size)
{
available = incoming_image.size - pos;
}
Array.Copy(img.data, 0, incoming_image.data, pos, available);
progress.ShowProgress(0, incoming_image.size, pos + available);
++incoming_image.packetsArrived;
//Debug.WriteLine("packet {0} of {1}, position {2} of {3}", incoming_image.packetsArrived, incoming_image.packets,
// pos + available, incoming_image.size);
// emit signal if all packets arrived
if (pos + available >= incoming_image.size)
{
// Restart state machine
incoming_image.packets = 0;
incoming_image.packetsArrived = 0;
byte[] saved = incoming_image.data;
incoming_image.data = null;
UiDispatcher.RunOnUIThread(() =>
{
progress.ShowProgress(0, 0, 0);
ShowImage(saved);
});
}
}
break;
}
}
private void GraphItem(LogItemSchema schema)
{
if (schema.IsNumeric)
{
ChartStack.Visibility = Visibility.Visible;
ChartStack.UpdateLayout();
SimpleLineChart chart = new SimpleLineChart();
chart.Margin = defaultChartMargin;
chart.Focusable = false;
chart.Closed += OnChartClosed;
chart.LineColor = GetRandomColor();
chart.StrokeThickness = 1;
chart.Tag = schema;
if (currentFlightLog != null && schema.Root == currentFlightLog.Schema)
{
List <DataValue> values = new List <DataValue>(currentFlightLog.GetDataValues(schema, DateTime.MinValue, TimeSpan.MaxValue));
InitializeChartData(schema, chart, values);
// now turn on live scrolling...
chart.LiveScrolling = true;
// the X values are in microseconds (s0 the numerator is the speed of scrolling).
chart.LiveScrollingXScale = 50.0 / 1000000.0;
liveScrolling.Add(chart);
// now start watching the live update for new values that need to be added to this chart.
Task.Run(() =>
{
LiveUpdate(chart, currentFlightLog, schema);
});
}
else
{
List <DataValue> values = new List <DataValue>(GetSelectedDataValues(schema));
if (values.Count > 0)
{
InitializeChartData(schema, chart, values);
}
else
{
chart = null;
}
ShowStatus(string.Format("Found {0} data values", values.Count));
}
if (chart != null)
{
if (chartGroup != null)
{
chartGroup.AddChart(chart);
if (chartGroup.Parent == null)
{
ChartStack.AddChartGroup(chartGroup);
}
}
else
{
ChartStack.AddChart(chart);
}
LayoutCharts();
}
ConsoleButton.IsChecked = false;
SystemConsole.Hide();
}
else
{
StringBuilder sb = new StringBuilder();
foreach (var value in GetSelectedDataValues(schema))
{
sb.AppendLine(value.Label);
}
SystemConsole.Write(sb.ToString());
ConsoleButton.IsChecked = true;
SystemConsole.Show();
}
}
public void Write_String_WritesToConsoleOut()
{
_console.Write("foo");
StringAssert.Contains("foo", ConsoleOut.ToString());
}