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