WPOverlay: fix loiter circle rendering

magicrub · Nov 17, 2023 · 3f21cc1 · 3f21cc1
1 parent 977c741
commit 3f21cc1
Showing 1 changed file with 18 additions and 3 deletions.
diff --git a/ExtLibs/Maps/WPOverlay.cs b/ExtLibs/Maps/WPOverlay.cs
@@ -139,6 +139,7 @@ public void CreateOverlay(PointLatLngAlt home, List<Locationwp> 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<Locationwp> 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<Locationwp> 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<Locationwp> 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)