MissionPlanner.MAVLinkInterface.setWP C# (CSharp) Method

        public MAV_MISSION_RESULT setWP(mavlink_mission_item_t req)
        {
            giveComport = true;

            ushort index = req.seq;

            log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1,
                req.x, req.y, req.z, index);

            // request
            generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM, req);


            DateTime start = DateTime.Now;
            int retrys = 10;

            while (true)
            {
                if (!(start.AddMilliseconds(400) > DateTime.Now))
                {
                    if (retrys > 0)
                    {
                        log.Info("setWP Retry " + retrys);
                        generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM, req);

                        start = DateTime.Now;
                        retrys--;
                        continue;
                    }
                    giveComport = false;
                    throw new TimeoutException("Timeout on read - setWP");
                }
                MAVLinkMessage buffer = readPacket();
                if (buffer.Length > 5)
                {
                    if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ACK)
                    {
                        var ans = buffer.ToStructure<mavlink_mission_ack_t>();
                        log.Info("set wp " + index + " ACK 47 : " + buffer.msgid + " ans " +
                                 Enum.Parse(typeof (MAV_MISSION_RESULT), ans.type.ToString()));
                        giveComport = false;

                        if (req.current == 2)
                        {
                            MAV.GuidedMode = req;
                        }
                        else if (req.current == 3)
                        {
                        }
                        else
                        {
                            MAV.wps[req.seq] = req;
                        }

                        return (MAV_MISSION_RESULT) ans.type;
                    }
                    else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST)
                    {
                        var ans = buffer.ToStructure<mavlink_mission_request_t>();
                        if (ans.seq == (index + 1))
                        {
                            log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer.msgid);
                            giveComport = false;

                            if (req.current == 2)
                            {
                                MAV.GuidedMode = req;
                            }
                            else if (req.current == 3)
                            {
                            }
                            else
                            {
                                MAV.wps[req.seq] = req;
                            }

                            return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED;
                        }
                        else
                        {
                            log.InfoFormat(
                                "set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + buffer.msgid +
                                " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component);
                            // resend point now
                            start = DateTime.MinValue;
                        }
                    }
                    else
                    {
                        //Console.WriteLine(DateTime.Now + " PC setwp " + buffer.msgid);
                    }
                }
            }

            // return MAV_MISSION_RESULT.MAV_MISSION_INVALID;
        }