Skip to content

Commit 88b3995

Browse files
tatsuytatsuy
committed
FlightPlanner: Simultaneous Display of Mission, Geofence, and Rally Points in PLAN Screen
1 parent a25c8a3 commit 88b3995

File tree

1 file changed

+106
-178
lines changed

1 file changed

+106
-178
lines changed

‎GCSViews/FlightPlanner.cs

Lines changed: 106 additions & 178 deletions
Original file line numberDiff line numberDiff line change
@@ -1374,228 +1374,156 @@ public class midline
13741374
public PointLatLngAlt next { get; set; }
13751375
}
13761376

1377-
/// <summary>
1378-
/// used to write a KML, update the Map view polygon, and update the row headers
1379-
/// </summary>
1380-
public void writeKML()
1377+
private void CreateAndDisplayOverlay(string overlayId, List<MAVLink.mavlink_mission_item_int_t> points, bool isEditable)
13811378
{
1382-
// quickadd is for when loading wps from eeprom or file, to prevent slow, loading times
1383-
if (quickadd)
1384-
return;
1379+
var overlay = new WPOverlay();
1380+
overlay.overlay.Id = overlayId;
13851381

1386-
if (Disposing)
1387-
return;
1388-
1389-
updateRowNumbers();
1390-
1391-
PointLatLngAlt home = new PointLatLngAlt();
1392-
if (TXT_homealt.Text != "" && TXT_homelat.Text != "" && TXT_homelng.Text != "")
1382+
PointLatLngAlt home = PointLatLngAlt.Zero;
1383+
var wpRadius = 0.0;
1384+
var loiterRadius = 0.0;
1385+
if (overlayId == "wp")
13931386
{
1387+
if (TXT_homealt.Text != "" && TXT_homelat.Text != "" && TXT_homelng.Text != "")
1388+
{
1389+
try
1390+
{
1391+
home = new PointLatLngAlt(
1392+
double.Parse(TXT_homelat.Text), double.Parse(TXT_homelng.Text),
1393+
double.Parse(TXT_homealt.Text) / CurrentState.multiplieralt, "H")
1394+
{ Tag2 = CMB_altmode.SelectedValue.ToString() };
1395+
}
1396+
catch (Exception ex)
1397+
{
1398+
CustomMessageBox.Show(Strings.Invalid_home_location, Strings.ERROR);
1399+
log.Error(ex);
1400+
}
1401+
}
13941402
try
13951403
{
1396-
home = new PointLatLngAlt(
1397-
double.Parse(TXT_homelat.Text), double.Parse(TXT_homelng.Text),
1398-
double.Parse(TXT_homealt.Text) / CurrentState.multiplieralt, "H")
1399-
{Tag2 = CMB_altmode.SelectedValue.ToString()};
1404+
if (TXT_WPRad.Text == "") TXT_WPRad.Text = startupWPradius;
1405+
wpRadius = double.Parse(TXT_WPRad.Text) / CurrentState.multiplieralt;
1406+
if (TXT_loiterrad.Text == "") TXT_loiterrad.Text = "30";
1407+
loiterRadius = double.Parse(TXT_loiterrad.Text) / CurrentState.multiplieralt;
14001408
}
1401-
catch (Exception ex)
1409+
catch (FormatException)
14021410
{
1403-
CustomMessageBox.Show(Strings.Invalid_home_location, Strings.ERROR);
1404-
log.Error(ex);
1411+
CustomMessageBox.Show(Strings.InvalidNumberEntered + "\n" + "WP Radius or Loiter Radius",
1412+
Strings.ERROR);
14051413
}
14061414
}
1407-
14081415
try
14091416
{
1410-
var commandlist = GetCommandList();
1411-
1412-
if ((MAVLink.MAV_MISSION_TYPE) cmb_missiontype.SelectedValue == MAVLink.MAV_MISSION_TYPE.MISSION)
1417+
if (isEditable)
14131418
{
1414-
wpOverlay = new WPOverlay();
1415-
wpOverlay.overlay.Id = "WPOverlay";
1416-
1417-
try
1418-
{
1419-
if (TXT_WPRad.Text == "") TXT_WPRad.Text = startupWPradius;
1420-
if (TXT_loiterrad.Text == "") TXT_loiterrad.Text = "30";
1421-
1422-
wpOverlay.CreateOverlay(home,
1423-
commandlist,
1424-
double.Parse(TXT_WPRad.Text) / CurrentState.multiplieralt,
1425-
double.Parse(TXT_loiterrad.Text) / CurrentState.multiplieralt, CurrentState.multiplieralt);
1426-
}
1427-
catch (FormatException)
1428-
{
1429-
CustomMessageBox.Show(Strings.InvalidNumberEntered + "\n" + "WP Radius or Loiter Radius",
1430-
Strings.ERROR);
1431-
}
1419+
var commandlist = GetCommandList();
1420+
overlay.CreateOverlay(home, commandlist, wpRadius, loiterRadius, CurrentState.multiplieralt);
1421+
}
1422+
else
1423+
{
1424+
overlay.CreateOverlay(home, points.Select(a => (Locationwp)a).ToList(), wpRadius, loiterRadius, CurrentState.multiplieralt);
1425+
}
1426+
}
1427+
catch (FormatException)
1428+
{
1429+
CustomMessageBox.Show(Strings.InvalidNumberEntered, Strings.ERROR);
1430+
}
14321431

1433-
MainMap.HoldInvalidation = true;
1432+
if (!isEditable)
1433+
overlay.overlay.Markers.Select(a => a.IsHitTestVisible = false).ToArray();
14341434

1435-
var existing = MainMap.Overlays.Where(a => a.Id == wpOverlay.overlay.Id).ToList();
1436-
foreach (var b in existing)
1437-
{
1438-
MainMap.Overlays.Remove(b);
1439-
}
1435+
// Remove any existing overlay with the same Id before adding the new one
1436+
var existingOverlay = MainMap.Overlays.FirstOrDefault(a => a.Id == overlayId);
1437+
if (existingOverlay != null)
1438+
MainMap.Overlays.Remove(existingOverlay);
14401439

1441-
MainMap.Overlays.Insert(1, wpOverlay.overlay);
1440+
if (isEditable || overlayId == "wp")
1441+
MainMap.Overlays.Add(overlay.overlay);
1442+
else
1443+
MainMap.Overlays.Insert(MainMap.Overlays.Count - 1, overlay.overlay);
14421444

1443-
wpOverlay.overlay.ForceUpdate();
1445+
// Update the overlay to reflect any changes
1446+
overlay.overlay.ForceUpdate();
14441447

1448+
if (overlayId == "wp" && isEditable)
1449+
{
1450+
if (isEditable)
1451+
{
14451452
lbl_distance.Text = rm.GetString("lbl_distance.Text") + ": " +
1446-
FormatDistance((
1447-
wpOverlay.overlay.Routes.SelectMany(a => a.Points)
1448-
.Select(a => (PointLatLngAlt) a)
1449-
.Aggregate(0.0, (d, p1, p2) => d + p1.GetDistance(p2))
1450-
) / 1000.0, false);
1453+
FormatDistance((
1454+
overlay.overlay.Routes.SelectMany(a => a.Points)
1455+
.Select(a => (PointLatLngAlt)a)
1456+
.Aggregate(0.0, (d, p1, p2) => d + p1.GetDistance(p2))
1457+
) / 1000.0, false);
14511458

1452-
setgradanddistandaz(wpOverlay.pointlist, home);
1459+
setgradanddistandaz(overlay.pointlist, home);
14531460

1454-
if (wpOverlay.pointlist.Count <= 1)
1461+
if (overlay.pointlist.Count <= 1)
14551462
{
1456-
RectLatLng? rect = MainMap.GetRectOfAllMarkers(wpOverlay.overlay.Id);
1463+
RectLatLng? rect = MainMap.GetRectOfAllMarkers(overlay.overlay.Id);
14571464
if (rect.HasValue)
14581465
{
14591466
MainMap.Position = rect.Value.LocationMiddle;
14601467
}
14611468

14621469
MainMap_OnMapZoomChanged();
14631470
}
1464-
1465-
pointlist = wpOverlay.pointlist;
1466-
1467-
{
1468-
foreach (var pointLatLngAlt in pointlist.PrevNowNext())
1469-
{
1470-
var prev = pointLatLngAlt.Item1;
1471-
var now = pointLatLngAlt.Item2;
1472-
var next = pointLatLngAlt.Item3;
1473-
1474-
if (now == null || next == null)
1475-
continue;
1476-
1477-
var mid = new PointLatLngAlt((now.Lat + next.Lat) / 2, (now.Lng + next.Lng) / 2,
1478-
(now.Alt + next.Alt) / 2);
1479-
1480-
var pnt = new GMapMarkerPlus(mid);
1481-
pnt.Tag = new midline() {now = now, next = next};
1482-
wpOverlay.overlay.Markers.Add(pnt);
1483-
}
1484-
}
1485-
1486-
// draw fence
1487-
{
1488-
var fenceoverlay = new WPOverlay();
1489-
fenceoverlay.overlay.Id = "fence";
1490-
try
1491-
{
1492-
fenceoverlay.CreateOverlay(PointLatLngAlt.Zero,
1493-
MainV2.comPort.MAV.fencepoints.Values.Select(a => (Locationwp) a).ToList(), 0, 0,
1494-
CurrentState.multiplieralt);
1495-
}
1496-
catch
1497-
{
1498-
1499-
}
1500-
1501-
fenceoverlay.overlay.Markers.Select(a => a.IsHitTestVisible = false).ToArray();
1502-
var fence = MainMap.Overlays.Where(a => a.Id == "fence");
1503-
if (fence.Count() > 0)
1504-
MainMap.Overlays.Remove(fence.First());
1505-
MainMap.Overlays.Add(fenceoverlay.overlay);
1506-
1507-
fenceoverlay.overlay.ForceUpdate();
1508-
}
1509-
1510-
MainMap.Refresh();
15111471
}
15121472

1513-
if ((MAVLink.MAV_MISSION_TYPE) cmb_missiontype.SelectedValue == MAVLink.MAV_MISSION_TYPE.FENCE)
1514-
{
1515-
var fenceoverlay = new WPOverlay();
1516-
fenceoverlay.overlay.Id = "fence";
1473+
pointlist = overlay.pointlist;
15171474

1518-
try
1519-
{
1520-
fenceoverlay.CreateOverlay(PointLatLngAlt.Zero,
1521-
commandlist, 0, 0, CurrentState.multiplieralt);
1522-
}
1523-
catch (FormatException)
1524-
{
1525-
CustomMessageBox.Show(Strings.InvalidNumberEntered, Strings.ERROR);
1526-
}
1527-
1528-
MainMap.HoldInvalidation = true;
1529-
1530-
var existing = MainMap.Overlays.Where(a => a.Id == fenceoverlay.overlay.Id).ToList();
1531-
foreach (var b in existing)
1532-
{
1533-
MainMap.Overlays.Remove(b);
1534-
}
1535-
1536-
MainMap.Overlays.Insert(1, fenceoverlay.overlay);
1537-
1538-
fenceoverlay.overlay.ForceUpdate();
1539-
1540-
if (true)
1475+
{
1476+
foreach (var pointLatLngAlt in pointlist.PrevNowNext())
15411477
{
1542-
foreach (var poly in fenceoverlay.overlay.Polygons)
1543-
{
1544-
var startwp = int.Parse(poly.Name);
1545-
var a = 1;
1546-
foreach (var pointLatLngAlt in poly.Points.CloseLoop().PrevNowNext())
1547-
{
1548-
var now = pointLatLngAlt.Item2;
1549-
var next = pointLatLngAlt.Item3;
1550-
1551-
if (now == null || next == null)
1552-
continue;
1478+
var prev = pointLatLngAlt.Item1;
1479+
var now = pointLatLngAlt.Item2;
1480+
var next = pointLatLngAlt.Item3;
15531481

1554-
var mid = new PointLatLngAlt((now.Lat + next.Lat) / 2, (now.Lng + next.Lng) / 2, 0);
1482+
if (now == null || next == null)
1483+
continue;
15551484

1556-
var pnt = new GMapMarkerPlus(mid);
1557-
pnt.Tag = new midline() {now = now, next = next};
1558-
((midline) pnt.Tag).now.Tag = (startwp + a).ToString();
1559-
((midline) pnt.Tag).next.Tag = (startwp + a + 1).ToString();
1560-
fenceoverlay.overlay.Markers.Add(pnt);
1485+
var mid = new PointLatLngAlt((now.Lat + next.Lat) / 2, (now.Lng + next.Lng) / 2,
1486+
(now.Alt + next.Alt) / 2);
15611487

1562-
a++;
1563-
}
1564-
}
1488+
var pnt = new GMapMarkerPlus(mid);
1489+
pnt.Tag = new midline() { now = now, next = next };
1490+
overlay.overlay.Markers.Add(pnt);
15651491
}
1566-
1567-
MainMap.Refresh();
15681492
}
1493+
}
1494+
}
15691495

1570-
if ((MAVLink.MAV_MISSION_TYPE) cmb_missiontype.SelectedValue == MAVLink.MAV_MISSION_TYPE.RALLY)
1571-
{
1572-
var rallyoverlay = new WPOverlay();
1573-
rallyoverlay.overlay.Id = "rally";
1496+
/// <summary>
1497+
/// used to write a KML, update the Map view polygon, and update the row headers
1498+
/// </summary>
1499+
public void writeKML()
1500+
{
1501+
// quickadd is for when loading wps from eeprom or file, to prevent slow, loading times
1502+
if (quickadd)
1503+
return;
15741504

1575-
try
1576-
{
1577-
rallyoverlay.CreateOverlay(PointLatLngAlt.Zero,
1578-
commandlist, 0, 0, CurrentState.multiplieralt);
1579-
}
1580-
catch (FormatException)
1581-
{
1582-
CustomMessageBox.Show(Strings.InvalidNumberEntered, Strings.ERROR);
1583-
}
1505+
if (Disposing)
1506+
return;
15841507

1585-
MainMap.HoldInvalidation = true;
1508+
updateRowNumbers();
15861509

1587-
var existing = MainMap.Overlays.Where(a => a.Id == rallyoverlay.overlay.Id).ToList();
1588-
foreach (var b in existing)
1589-
{
1590-
MainMap.Overlays.Remove(b);
1591-
}
1510+
try
1511+
{
1512+
var missionType = (MAVLink.MAV_MISSION_TYPE)cmb_missiontype.SelectedValue;
15921513

1593-
MainMap.Overlays.Insert(1, rallyoverlay.overlay);
1514+
// Determine editability based on selected mission type
1515+
bool isMissionEditable = missionType == MAVLink.MAV_MISSION_TYPE.MISSION;
1516+
bool isFenceEditable = missionType == MAVLink.MAV_MISSION_TYPE.FENCE;
1517+
bool isRallyEditable = missionType == MAVLink.MAV_MISSION_TYPE.RALLY;
15941518

1595-
rallyoverlay.overlay.ForceUpdate();
1519+
MainMap.HoldInvalidation = true;
15961520

1597-
MainMap.Refresh();
1598-
}
1521+
// Create overlays for each type, setting editability accordingly
1522+
CreateAndDisplayOverlay("wp", MainV2.comPort.MAV.wps.Values.Skip(1).ToList(), isMissionEditable);
1523+
CreateAndDisplayOverlay("fence", MainV2.comPort.MAV.fencepoints.Values.ToList(), isFenceEditable);
1524+
CreateAndDisplayOverlay("rally", MainV2.comPort.MAV.rallypoints.Values.ToList(), isRallyEditable);
1525+
1526+
MainMap.Refresh();
15991527
}
16001528
catch (FormatException ex)
16011529
{

0 commit comments

Comments
 (0)