LogViewer.MainWindow.OnMavlinkMessageReceived C# (CSharp) Method

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