diff --git a/GCSViews/FlightPlanner.cs b/GCSViews/FlightPlanner.cs index 8fa04b5247..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; @@ -1376,86 +1379,107 @@ 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; - - if (Disposing) - return; - - updateRowNumbers(); + var overlay = new WPOverlay(); + overlay.overlay.Id = overlayId; - 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.Visible) + { + 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) + // Store command list for waypoints, fences, and rally points + if (overlayId == "wp") wpCommandList = commandlist; + else if (overlayId == "fence") fenceCommandList = commandlist; + else if (overlayId == "rally") rallyCommandList = commandlist; + } + else + { + if (MainV2.comPort.BaseStream.IsOpen) commandlist = points.Select(a => (Locationwp)a).ToList(); + else { - CustomMessageBox.Show(Strings.InvalidNumberEntered + "\n" + "WP Radius or Loiter Radius", - Strings.ERROR); + // 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) + { + 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 +1487,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"; - - 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(); + pointlist = overlay.pointlist; - 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; - MainMap.HoldInvalidation = true; + updateRowNumbers(); - var existing = MainMap.Overlays.Where(a => a.Id == rallyoverlay.overlay.Id).ToList(); - foreach (var b in existing) - { - MainMap.Overlays.Remove(b); - } + try + { + var missionType = (MAVLink.MAV_MISSION_TYPE)cmb_missiontype.SelectedValue; - MainMap.Overlays.Insert(1, rallyoverlay.overlay); + // 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; - rallyoverlay.overlay.ForceUpdate(); + MainMap.HoldInvalidation = true; - MainMap.Refresh(); - } + // 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(); } catch (FormatException ex) { @@ -2172,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)," + @@ -2191,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(); @@ -6411,18 +6371,21 @@ private void setWPParams() try { - TXT_loiterrad.Enabled = false; + TXT_loiterrad.Visible = false; + label5.Visible = false; if (param.ContainsKey("LOITER_RADIUS")) { TXT_loiterrad.Text = (((double) param["LOITER_RADIUS"] * CurrentState.multiplierdist)).ToString(); - TXT_loiterrad.Enabled = true; + TXT_loiterrad.Visible = true; + label5.Visible = true; } else if (param.ContainsKey("WP_LOITER_RAD")) { TXT_loiterrad.Text = (((double) param["WP_LOITER_RAD"] * CurrentState.multiplierdist)).ToString(); - TXT_loiterrad.Enabled = true; + TXT_loiterrad.Visible = true; + label5.Visible = true; } log.Info("param LOITER_RADIUS " + TXT_loiterrad.Text);