From 88103492cce6bc71ec59e038103a8a134250592a Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Sat, 9 Mar 2024 22:45:43 +0900 Subject: [PATCH 1/3] FlightPlanner: Simultaneous display of mission, geofence, and rally points in PLAN screen --- GCSViews/FlightPlanner.cs | 284 ++++++++++++++------------------------ 1 file changed, 106 insertions(+), 178 deletions(-) diff --git a/GCSViews/FlightPlanner.cs b/GCSViews/FlightPlanner.cs index 8fa04b5247..2e16278860 100644 --- a/GCSViews/FlightPlanner.cs +++ b/GCSViews/FlightPlanner.cs @@ -1376,86 +1376,93 @@ public class midline public PointLatLngAlt next { get; set; } } - /// - /// used to write a KML, update the Map view polygon, and update the row headers - /// - public void writeKML() + private void CreateAndDisplayOverlay(string overlayId, List points, bool isEditable) { - // quickadd is for when loading wps from eeprom or file, to prevent slow, loading times - if (quickadd) - return; + var overlay = new WPOverlay(); + overlay.overlay.Id = overlayId; - if (Disposing) - return; - - updateRowNumbers(); - - PointLatLngAlt home = new PointLatLngAlt(); - if (TXT_homealt.Text != "" && TXT_homelat.Text != "" && TXT_homelng.Text != "") + PointLatLngAlt home = PointLatLngAlt.Zero; + var wpRadius = 0.0; + var loiterRadius = 0.0; + if (overlayId == "wp") { + if (TXT_homealt.Text != "" && TXT_homelat.Text != "" && TXT_homelng.Text != "") + { + try + { + home = new PointLatLngAlt( + double.Parse(TXT_homelat.Text), double.Parse(TXT_homelng.Text), + double.Parse(TXT_homealt.Text) / CurrentState.multiplieralt, "H") + { Tag2 = CMB_altmode.SelectedValue.ToString() }; + } + catch (Exception ex) + { + CustomMessageBox.Show(Strings.Invalid_home_location, Strings.ERROR); + log.Error(ex); + } + } try { - home = new PointLatLngAlt( - double.Parse(TXT_homelat.Text), double.Parse(TXT_homelng.Text), - double.Parse(TXT_homealt.Text) / CurrentState.multiplieralt, "H") - {Tag2 = CMB_altmode.SelectedValue.ToString()}; + if (TXT_WPRad.Text == "") TXT_WPRad.Text = startupWPradius; + wpRadius = double.Parse(TXT_WPRad.Text) / CurrentState.multiplieralt; + if (TXT_loiterrad.Text == "") TXT_loiterrad.Text = "30"; + loiterRadius = double.Parse(TXT_loiterrad.Text) / CurrentState.multiplieralt; } - catch (Exception ex) + catch (FormatException) { - CustomMessageBox.Show(Strings.Invalid_home_location, Strings.ERROR); - log.Error(ex); + CustomMessageBox.Show(Strings.InvalidNumberEntered + "\n" + "WP Radius or Loiter Radius", + Strings.ERROR); } } - try { - var commandlist = GetCommandList(); - - if ((MAVLink.MAV_MISSION_TYPE) cmb_missiontype.SelectedValue == MAVLink.MAV_MISSION_TYPE.MISSION) + if (isEditable) { - wpOverlay = new WPOverlay(); - wpOverlay.overlay.Id = "WPOverlay"; - - try - { - if (TXT_WPRad.Text == "") TXT_WPRad.Text = startupWPradius; - if (TXT_loiterrad.Text == "") TXT_loiterrad.Text = "30"; - - wpOverlay.CreateOverlay(home, - commandlist, - double.Parse(TXT_WPRad.Text) / CurrentState.multiplieralt, - double.Parse(TXT_loiterrad.Text) / CurrentState.multiplieralt, CurrentState.multiplieralt); - } - catch (FormatException) - { - CustomMessageBox.Show(Strings.InvalidNumberEntered + "\n" + "WP Radius or Loiter Radius", - Strings.ERROR); - } + var commandlist = GetCommandList(); + overlay.CreateOverlay(home, commandlist, wpRadius, loiterRadius, CurrentState.multiplieralt); + } + else + { + overlay.CreateOverlay(home, points.Select(a => (Locationwp)a).ToList(), wpRadius, loiterRadius, CurrentState.multiplieralt); + } + } + catch (FormatException) + { + CustomMessageBox.Show(Strings.InvalidNumberEntered, Strings.ERROR); + } - MainMap.HoldInvalidation = true; + if (!isEditable) + overlay.overlay.Markers.Select(a => a.IsHitTestVisible = false).ToArray(); - var existing = MainMap.Overlays.Where(a => a.Id == wpOverlay.overlay.Id).ToList(); - foreach (var b in existing) - { - MainMap.Overlays.Remove(b); - } + // Remove any existing overlay with the same Id before adding the new one + var existingOverlay = MainMap.Overlays.FirstOrDefault(a => a.Id == overlayId); + if (existingOverlay != null) + MainMap.Overlays.Remove(existingOverlay); - MainMap.Overlays.Insert(1, wpOverlay.overlay); + if (isEditable || overlayId == "wp") + MainMap.Overlays.Add(overlay.overlay); + else + MainMap.Overlays.Insert(MainMap.Overlays.Count - 1, overlay.overlay); - wpOverlay.overlay.ForceUpdate(); + // Update the overlay to reflect any changes + overlay.overlay.ForceUpdate(); + if (overlayId == "wp" && isEditable) + { + if (isEditable) + { lbl_distance.Text = rm.GetString("lbl_distance.Text") + ": " + - FormatDistance(( - wpOverlay.overlay.Routes.SelectMany(a => a.Points) - .Select(a => (PointLatLngAlt) a) - .Aggregate(0.0, (d, p1, p2) => d + p1.GetDistance(p2)) - ) / 1000.0, false); + FormatDistance(( + overlay.overlay.Routes.SelectMany(a => a.Points) + .Select(a => (PointLatLngAlt)a) + .Aggregate(0.0, (d, p1, p2) => d + p1.GetDistance(p2)) + ) / 1000.0, false); - setgradanddistandaz(wpOverlay.pointlist, home); + setgradanddistandaz(overlay.pointlist, home); - if (wpOverlay.pointlist.Count <= 1) + if (overlay.pointlist.Count <= 1) { - RectLatLng? rect = MainMap.GetRectOfAllMarkers(wpOverlay.overlay.Id); + RectLatLng? rect = MainMap.GetRectOfAllMarkers(overlay.overlay.Id); if (rect.HasValue) { MainMap.Position = rect.Value.LocationMiddle; @@ -1463,141 +1470,62 @@ public void writeKML() MainMap_OnMapZoomChanged(); } - - pointlist = wpOverlay.pointlist; - - { - foreach (var pointLatLngAlt in pointlist.PrevNowNext()) - { - var prev = pointLatLngAlt.Item1; - var now = pointLatLngAlt.Item2; - var next = pointLatLngAlt.Item3; - - if (now == null || next == null) - continue; - - var mid = new PointLatLngAlt((now.Lat + next.Lat) / 2, (now.Lng + next.Lng) / 2, - (now.Alt + next.Alt) / 2); - - var pnt = new GMapMarkerPlus(mid); - pnt.Tag = new midline() {now = now, next = next}; - wpOverlay.overlay.Markers.Add(pnt); - } - } - - // draw fence - { - var fenceoverlay = new WPOverlay(); - fenceoverlay.overlay.Id = "fence"; - try - { - fenceoverlay.CreateOverlay(PointLatLngAlt.Zero, - MainV2.comPort.MAV.fencepoints.Values.Select(a => (Locationwp) a).ToList(), 0, 0, - CurrentState.multiplieralt); - } - catch - { - - } - - fenceoverlay.overlay.Markers.Select(a => a.IsHitTestVisible = false).ToArray(); - var fence = MainMap.Overlays.Where(a => a.Id == "fence"); - if (fence.Count() > 0) - MainMap.Overlays.Remove(fence.First()); - MainMap.Overlays.Add(fenceoverlay.overlay); - - fenceoverlay.overlay.ForceUpdate(); - } - - MainMap.Refresh(); } - if ((MAVLink.MAV_MISSION_TYPE) cmb_missiontype.SelectedValue == MAVLink.MAV_MISSION_TYPE.FENCE) - { - var fenceoverlay = new WPOverlay(); - fenceoverlay.overlay.Id = "fence"; + pointlist = overlay.pointlist; - try - { - fenceoverlay.CreateOverlay(PointLatLngAlt.Zero, - commandlist, 0, 0, CurrentState.multiplieralt); - } - catch (FormatException) - { - CustomMessageBox.Show(Strings.InvalidNumberEntered, Strings.ERROR); - } - - MainMap.HoldInvalidation = true; - - var existing = MainMap.Overlays.Where(a => a.Id == fenceoverlay.overlay.Id).ToList(); - foreach (var b in existing) - { - MainMap.Overlays.Remove(b); - } - - MainMap.Overlays.Insert(1, fenceoverlay.overlay); - - fenceoverlay.overlay.ForceUpdate(); - - if (true) + { + foreach (var pointLatLngAlt in pointlist.PrevNowNext()) { - foreach (var poly in fenceoverlay.overlay.Polygons) - { - var startwp = int.Parse(poly.Name); - var a = 1; - foreach (var pointLatLngAlt in poly.Points.CloseLoop().PrevNowNext()) - { - var now = pointLatLngAlt.Item2; - var next = pointLatLngAlt.Item3; - - if (now == null || next == null) - continue; + var prev = pointLatLngAlt.Item1; + var now = pointLatLngAlt.Item2; + var next = pointLatLngAlt.Item3; - var mid = new PointLatLngAlt((now.Lat + next.Lat) / 2, (now.Lng + next.Lng) / 2, 0); + if (now == null || next == null) + continue; - var pnt = new GMapMarkerPlus(mid); - pnt.Tag = new midline() {now = now, next = next}; - ((midline) pnt.Tag).now.Tag = (startwp + a).ToString(); - ((midline) pnt.Tag).next.Tag = (startwp + a + 1).ToString(); - fenceoverlay.overlay.Markers.Add(pnt); + var mid = new PointLatLngAlt((now.Lat + next.Lat) / 2, (now.Lng + next.Lng) / 2, + (now.Alt + next.Alt) / 2); - a++; - } - } + var pnt = new GMapMarkerPlus(mid); + pnt.Tag = new midline() { now = now, next = next }; + overlay.overlay.Markers.Add(pnt); } - - MainMap.Refresh(); } + } + } - if ((MAVLink.MAV_MISSION_TYPE) cmb_missiontype.SelectedValue == MAVLink.MAV_MISSION_TYPE.RALLY) - { - var rallyoverlay = new WPOverlay(); - rallyoverlay.overlay.Id = "rally"; + /// + /// used to write a KML, update the Map view polygon, and update the row headers + /// + public void writeKML() + { + // quickadd is for when loading wps from eeprom or file, to prevent slow, loading times + if (quickadd) + return; - try - { - rallyoverlay.CreateOverlay(PointLatLngAlt.Zero, - commandlist, 0, 0, CurrentState.multiplieralt); - } - catch (FormatException) - { - CustomMessageBox.Show(Strings.InvalidNumberEntered, Strings.ERROR); - } + if (Disposing) + return; + + updateRowNumbers(); - MainMap.HoldInvalidation = true; + try + { + var missionType = (MAVLink.MAV_MISSION_TYPE)cmb_missiontype.SelectedValue; - var existing = MainMap.Overlays.Where(a => a.Id == rallyoverlay.overlay.Id).ToList(); - foreach (var b in existing) - { - MainMap.Overlays.Remove(b); - } + // Determine editability based on selected mission type + bool isMissionEditable = missionType == MAVLink.MAV_MISSION_TYPE.MISSION; + bool isFenceEditable = missionType == MAVLink.MAV_MISSION_TYPE.FENCE; + bool isRallyEditable = missionType == MAVLink.MAV_MISSION_TYPE.RALLY; - MainMap.Overlays.Insert(1, rallyoverlay.overlay); + MainMap.HoldInvalidation = true; - rallyoverlay.overlay.ForceUpdate(); + // Create overlays for each type, setting editability accordingly + CreateAndDisplayOverlay("wp", MainV2.comPort.MAV.wps.Values.Skip(1).ToList(), isMissionEditable); + CreateAndDisplayOverlay("fence", MainV2.comPort.MAV.fencepoints.Values.ToList(), isFenceEditable); + CreateAndDisplayOverlay("rally", MainV2.comPort.MAV.rallypoints.Values.ToList(), isRallyEditable); - MainMap.Refresh(); - } + MainMap.Refresh(); } catch (FormatException ex) { From f821bea35293ce8a895226fc839be8f7a368d794 Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Tue, 12 Mar 2024 17:21:52 +0900 Subject: [PATCH 2/3] FlightPlanner: Hide LOITER RADIUS field when not applicable --- GCSViews/FlightPlanner.cs | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/GCSViews/FlightPlanner.cs b/GCSViews/FlightPlanner.cs index 2e16278860..d47ae977ec 100644 --- a/GCSViews/FlightPlanner.cs +++ b/GCSViews/FlightPlanner.cs @@ -1405,8 +1405,11 @@ private void CreateAndDisplayOverlay(string overlayId, List Date: Thu, 14 Mar 2024 13:13:44 +0900 Subject: [PATCH 3/3] FlightPlanner: Simultaneous display when not connected to vehicle --- GCSViews/FlightPlanner.cs | 43 ++++++++++++++++++++++++++++++++------- 1 file changed, 36 insertions(+), 7 deletions(-) diff --git a/GCSViews/FlightPlanner.cs b/GCSViews/FlightPlanner.cs index d47ae977ec..b1c5724bd0 100644 --- a/GCSViews/FlightPlanner.cs +++ b/GCSViews/FlightPlanner.cs @@ -109,6 +109,9 @@ private void but_mincommands_Click(object sender, System.EventArgs e) private bool grid; private List groupmarkers = new List(); private List> history = new List>(); + private List wpCommandList = new List(); + private List fenceCommandList = new List(); + private List rallyCommandList = new List(); private bool isMouseClickOffMenu; private bool isMouseDown; private bool isMouseDraging; @@ -1419,15 +1422,26 @@ private void CreateAndDisplayOverlay(string overlayId, List (Locationwp)a).ToList(), wpRadius, loiterRadius, CurrentState.multiplieralt); + if (MainV2.comPort.BaseStream.IsOpen) commandlist = points.Select(a => (Locationwp)a).ToList(); + else + { + // Use the stored command list if not connected to vehicle + if (overlayId == "wp") commandlist = wpCommandList; + else if (overlayId == "fence") commandlist = fenceCommandList; + else if (overlayId == "rally") commandlist = rallyCommandList; + } } + overlay.CreateOverlay(home, commandlist, wpRadius, loiterRadius, CurrentState.multiplieralt); } catch (FormatException) { @@ -2103,16 +2117,22 @@ public void Cmb_missiontype_SelectedIndexChanged(object sender, EventArgs e) if (rally.Count() > 0) MainMap.Overlays.Remove(rally.First()); // update the displayed items + // if connected to a vehicle, display the vehicle's items; otherwise, display local items if ((MAVLink.MAV_MISSION_TYPE) cmb_missiontype.SelectedValue == MAVLink.MAV_MISSION_TYPE.RALLY) { BUT_Add.Visible = false; - processToScreen(MainV2.comPort.MAV.rallypoints.Select(a => (Locationwp) a.Value).ToList()); - + if (MainV2.comPort.BaseStream.IsOpen) + processToScreen(MainV2.comPort.MAV.rallypoints.Select(a => (Locationwp) a.Value).ToList()); + else + processToScreen(rallyCommandList); } else if ((MAVLink.MAV_MISSION_TYPE) cmb_missiontype.SelectedValue == MAVLink.MAV_MISSION_TYPE.FENCE) { BUT_Add.Visible = false; - processToScreen(MainV2.comPort.MAV.fencepoints.Select(a => (Locationwp) a.Value).ToList()); + if (MainV2.comPort.BaseStream.IsOpen) + processToScreen(MainV2.comPort.MAV.fencepoints.Select(a => (Locationwp) a.Value).ToList()); + else + processToScreen(fenceCommandList); Common.MessageShowAgain("FlightPlan Fence", "Please use the Polygon drawing tool to draw " + "Inclusion and Exclusion areas (round circle to the left)," + @@ -2122,7 +2142,16 @@ public void Cmb_missiontype_SelectedIndexChanged(object sender, EventArgs e) else { BUT_Add.Visible = true; - processToScreen(MainV2.comPort.MAV.wps.Select(a => (Locationwp) a.Value).ToList()); + if (MainV2.comPort.BaseStream.IsOpen) + processToScreen(MainV2.comPort.MAV.wps.Select(a => (Locationwp) a.Value).ToList()); + else + { + if (wpCommandList.Count > 0) + // insert a dummy home point + wpCommandList.Insert(0, new Locationwp().Set(0, 0, 0, 0)); + + processToScreen(wpCommandList); + } } writeKML();