MissionPlanner.MAVLinkInterface.getWPsfromstream C# (CSharp) Method

        void getWPsfromstream(ref MAVLinkMessage buffer, byte sysid, byte compid)
        {
            if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_COUNT)
            {
                // clear old
                mavlink_mission_count_t wp = buffer.ToStructure<mavlink_mission_count_t>();

                if (wp.target_system == gcssysid)
                {
                    wp.target_system = sysid;
                    wp.target_component = compid;
                }

                MAVlist[wp.target_system, wp.target_component].wps.Clear();
            }

            if (buffer.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM)
            {
                mavlink_mission_item_t wp = buffer.ToStructure<mavlink_mission_item_t>();

                if (wp.target_system == gcssysid)
                {
                    wp.target_system = sysid;
                    wp.target_component = compid;
                }

                if (wp.current == 2)
                {
                    // guide mode wp
                    MAVlist[wp.target_system, wp.target_component].GuidedMode = wp;
                }
                else
                {
                    MAVlist[wp.target_system, wp.target_component].wps[wp.seq] = wp;
                }

                Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2,
                    wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command);
            }

            if (buffer.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_INT)
            {
                mavlink_mission_item_int_t wp = buffer.ToStructure<mavlink_mission_item_int_t>();

                if (wp.target_system == gcssysid)
                {
                    wp.target_system = sysid;
                    wp.target_component = compid;
                }

                if (wp.current == 2)
                {
                    // guide mode wp
                    MAVlist[wp.target_system, wp.target_component].GuidedMode = (mavlink_mission_item_t)(Locationwp)wp;
                }
                else
                {
                    MAVlist[wp.target_system, wp.target_component].wps[wp.seq] = (mavlink_mission_item_t)(Locationwp)wp;
                }

                Console.WriteLine("WP INT # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2,
                    wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command);
            }

            if (buffer.msgid == (byte) MAVLINK_MSG_ID.RALLY_POINT)
            {
                mavlink_rally_point_t rallypt = buffer.ToStructure<mavlink_rally_point_t>();

                if (rallypt.target_system == gcssysid)
                {
                    rallypt.target_system = sysid;
                    rallypt.target_component = compid;
                }

                MAVlist[rallypt.target_system, rallypt.target_component].rallypoints[rallypt.idx] = rallypt;

                Console.WriteLine("RP # {0} {1} {2} {3} {4}", rallypt.idx, rallypt.lat, rallypt.lng, rallypt.alt,
                    rallypt.break_alt);
            }

            if (buffer.msgid == (byte)MAVLINK_MSG_ID.CAMERA_FEEDBACK)
            {
                mavlink_camera_feedback_t camerapt = buffer.ToStructure<mavlink_camera_feedback_t>();

                if (MAVlist[sysid, compid].camerapoints.Count == 0 || MAVlist[sysid, compid].camerapoints.Last().time_usec != camerapt.time_usec)
                {
                    MAVlist[sysid, compid].camerapoints.Add(camerapt);
                }
            }

            if (buffer.msgid == (byte) MAVLINK_MSG_ID.FENCE_POINT)
            {
                mavlink_fence_point_t fencept = buffer.ToStructure<mavlink_fence_point_t>();

                if (fencept.target_system == gcssysid)
                {
                    fencept.target_system = sysid;
                    fencept.target_component = compid;
                }

                MAVlist[fencept.target_system, fencept.target_component].fencepoints[fencept.idx] = fencept;
            }

            if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE)
            {
                mavlink_param_value_t value = buffer.ToStructure<mavlink_param_value_t>();

                string st = System.Text.ASCIIEncoding.ASCII.GetString(value.param_id);

                int pos = st.IndexOf('\0');

                if (pos != -1)
                {
                    st = st.Substring(0, pos);
                }

                if (MAV.apname == MAV_AUTOPILOT.ARDUPILOTMEGA)
                {
                    var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value");
                    MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(Marshal.ReadInt32(value, offset.ToInt32())), MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE)value.param_type);
                }
                else
                {
                    var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value");
                    MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(Marshal.ReadInt32(value, offset.ToInt32())),
                        (MAV_PARAM_TYPE)value.param_type, (MAV_PARAM_TYPE)value.param_type);
                }

                MAVlist[sysid,compid].param.TotalReported = value.param_count;
            }
        }