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