MissionPlanner.MAVLinkInterface.setGuidedModeWP C# (CSharp) Method

        public void setGuidedModeWP(Locationwp gotohere, bool setguidedmode = true)
        {
            if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0)
                return;

            giveComport = true;

            try
            {
                gotohere.id = (ushort)MAV_CMD.WAYPOINT;

                if (setguidedmode)
                {
                    // fix for followme change
                    if (MAV.cs.mode.ToUpper() != "GUIDED")
                        setMode("GUIDED");
                }

                MAV_MISSION_RESULT ans = setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte) 2);

                if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
                    throw new Exception("Guided Mode Failed");
            }
            catch (Exception ex)
            {
                log.Error(ex);
            }

            giveComport = false;
        }