From 3f21cc103eae7154c37eaa85b4991e9256456321 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 15 Nov 2023 09:50:41 +1100 Subject: [PATCH] WPOverlay: fix loiter circle rendering --- ExtLibs/Maps/WPOverlay.cs | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/ExtLibs/Maps/WPOverlay.cs b/ExtLibs/Maps/WPOverlay.cs index fb4fc36daf..93c8de95fe 100644 --- a/ExtLibs/Maps/WPOverlay.cs +++ b/ExtLibs/Maps/WPOverlay.cs @@ -139,6 +139,7 @@ public void CreateOverlay(PointLatLngAlt home, List missionitems, do } else if (command == (ushort) MAVLink.MAV_CMD.LOITER_TIME || command == (ushort) MAVLink.MAV_CMD.LOITER_TURNS || + command == (ushort) MAVLink.MAV_CMD.LOITER_TO_ALT || command == (ushort) MAVLink.MAV_CMD.LOITER_UNLIM) { if (item.lat == 0 && item.lng == 0) @@ -160,6 +161,20 @@ public void CreateOverlay(PointLatLngAlt home, List missionitems, do color = Color.LightBlue }); + // Calculate the loiter radius and direction for this command, if one is specified + var this_loiterradius = loiterradius; + if (command == (ushort)MAVLink.MAV_CMD.LOITER_TURNS || + command == (ushort)MAVLink.MAV_CMD.LOITER_UNLIM) + { + this_loiterradius = item.p3 != 0 ? item.p3 : this_loiterradius; + } + else if (command == (ushort)MAVLink.MAV_CMD.LOITER_TO_ALT) + { + this_loiterradius = item.p2 != 0 ? item.p2 : this_loiterradius; + } + int loiterdirection = Math.Sign(this_loiterradius); + this_loiterradius = Math.Abs(this_loiterradius); + // exit at tangent if (item.p4 == 1) { @@ -175,10 +190,10 @@ public void CreateOverlay(PointLatLngAlt home, List missionitems, do var bearing = from.GetBearing(to); var dist = from.GetDistance(to); - if (dist > loiterradius) + if (dist > this_loiterradius) { route.Add(pointlist[pointlist.Count - 1]); - var offset = from.newpos(bearing + 90, loiterradius); + var offset = from.newpos(bearing - loiterdirection*90, this_loiterradius); route.Add(offset); } else @@ -190,7 +205,7 @@ public void CreateOverlay(PointLatLngAlt home, List missionitems, do route.Add(pointlist[pointlist.Count - 1]); addpolygonmarker((a + 1).ToString(), item.lng, item.lat, - item.alt * altunitmultiplier, Color.LightBlue, loiterradius); + item.alt * altunitmultiplier, Color.LightBlue, this_loiterradius); } } else if (command == (ushort) MAVLink.MAV_CMD.SPLINE_WAYPOINT)