diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs index 28acf12fbe..baf1b18fe4 100644 --- a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs +++ b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs @@ -4422,8 +4422,6 @@ public void setGuidedModeWP(byte sysid, byte compid, Locationwp gotohere, bool s if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0) return; - giveComport = true; - try { gotohere.id = (ushort) MAV_CMD.WAYPOINT; @@ -4444,7 +4442,6 @@ public void setGuidedModeWP(byte sysid, byte compid, Locationwp gotohere, bool s if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) { - giveComport = false; throw new Exception("Guided Mode Failed"); } } @@ -4459,8 +4456,6 @@ public void setGuidedModeWP(byte sysid, byte compid, Locationwp gotohere, bool s { log.Error(ex); } - - giveComport = false; } [Obsolete] @@ -4482,7 +4477,6 @@ public void setNewWPAlt(byte sysid, byte compid, Locationwp gotohere) if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) { - giveComport = false; throw new Exception("Alt Change Failed"); } @@ -4495,12 +4489,9 @@ public void setNewWPAlt(byte sysid, byte compid, Locationwp gotohere) } catch (Exception ex) { - giveComport = false; log.Error(ex); throw; } - - giveComport = false; } public void setPositionTargetGlobalInt(byte sysid, byte compid, bool pos, bool vel, bool acc, bool yaw,