5454using System . Drawing . Imaging ;
5555using SharpKml . Engine ;
5656using MissionPlanner . Controls . Waypoints ;
57+ using MissionPlanner . Utilities . Mission ;
5758
5859namespace MissionPlanner . GCSViews
5960{
@@ -127,6 +128,7 @@ private void but_mincommands_Click(object sender, System.EventArgs e)
127128 private PointLatLng MouseDownStart ;
128129 private PointLatLngAlt mouseposdisplay = new PointLatLngAlt ( 0 , 0 ) ;
129130 private WPOverlay wpOverlay ;
131+ private WPOverlay2 wpOverlay2 ;
130132 private bool polygongridmode ;
131133 private MissionPlanner . Controls . Icon . Polygon polyicon = new MissionPlanner . Controls . Icon . Polygon ( ) ;
132134 private MissionPlanner . Controls . Icon . Zoom zoomicon = new MissionPlanner . Controls . Icon . Zoom ( ) ;
@@ -1411,49 +1413,70 @@ public void writeKML()
14111413
14121414 if ( ( MAVLink . MAV_MISSION_TYPE ) cmb_missiontype . SelectedValue == MAVLink . MAV_MISSION_TYPE . MISSION )
14131415 {
1414- wpOverlay = new WPOverlay ( ) ;
1415- wpOverlay . overlay . Id = "WPOverlay" ;
1416+ var useV2 = Settings . Instance . GetBoolean ( "UseWPOverlay2" , true ) ;
1417+ GMapOverlay activeOverlay ;
1418+ List < PointLatLngAlt > activePointlist ;
14161419
1420+ if ( TXT_WPRad . Text == "" ) TXT_WPRad . Text = startupWPradius ;
1421+ if ( TXT_loiterrad . Text == "" ) TXT_loiterrad . Text = "30" ;
1422+
1423+ double wprad = 0 , loiterrad = 0 ;
14171424 try
14181425 {
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 . multiplierdist ,
1425- double . Parse ( TXT_loiterrad . Text ) / CurrentState . multiplierdist , CurrentState . multiplieralt ) ;
1426+ wprad = double . Parse ( TXT_WPRad . Text ) / CurrentState . multiplierdist ;
1427+ loiterrad = double . Parse ( TXT_loiterrad . Text ) / CurrentState . multiplierdist ;
14261428 }
14271429 catch ( FormatException )
14281430 {
14291431 CustomMessageBox . Show ( Strings . InvalidNumberEntered + "\n " + "WP Radius or Loiter Radius" ,
14301432 Strings . ERROR ) ;
14311433 }
14321434
1435+ if ( useV2 )
1436+ {
1437+ wpOverlay2 = new WPOverlay2 ( )
1438+ {
1439+ VehicleClass = MainV2 . comPort . MAV . cs . vehicleClass ,
1440+ ShowPlusMarkers = true ,
1441+ } ;
1442+ wpOverlay2 . overlay . Id = "WPOverlay" ;
1443+ wpOverlay2 . CreateOverlay ( home , commandlist , wprad , loiterrad , CurrentState . multiplieralt ) ;
1444+ activeOverlay = wpOverlay2 . overlay ;
1445+ activePointlist = wpOverlay2 . pointlist ;
1446+ }
1447+ else
1448+ {
1449+ wpOverlay = new WPOverlay ( ) ;
1450+ wpOverlay . overlay . Id = "WPOverlay" ;
1451+ wpOverlay . CreateOverlay ( home , commandlist , wprad , loiterrad , CurrentState . multiplieralt ) ;
1452+ activeOverlay = wpOverlay . overlay ;
1453+ activePointlist = wpOverlay . pointlist ;
1454+ }
1455+
14331456 MainMap . HoldInvalidation = true ;
14341457
1435- var existing = MainMap . Overlays . Where ( a => a . Id == wpOverlay . overlay . Id ) . ToList ( ) ;
1458+ var existing = MainMap . Overlays . Where ( a => a . Id == activeOverlay . Id ) . ToList ( ) ;
14361459 foreach ( var b in existing )
14371460 {
14381461 MainMap . Overlays . Remove ( b ) ;
14391462 }
14401463
1441- MainMap . Overlays . Insert ( 1 , wpOverlay . overlay ) ;
1464+ MainMap . Overlays . Insert ( 1 , activeOverlay ) ;
14421465
1443- wpOverlay . overlay . ForceUpdate ( ) ;
1466+ activeOverlay . ForceUpdate ( ) ;
14441467
14451468 lbl_distance . Text = rm . GetString ( "lbl_distance.Text" ) + ": " +
14461469 FormatDistance ( (
1447- wpOverlay . overlay . Routes . SelectMany ( a => a . Points )
1470+ activeOverlay . Routes . SelectMany ( a => a . Points )
14481471 . Select ( a => ( PointLatLngAlt ) a )
14491472 . Aggregate ( 0.0 , ( d , p1 , p2 ) => d + p1 . GetDistance ( p2 ) )
14501473 ) / 1000.0 , false ) ;
14511474
1452- setgradanddistandaz ( wpOverlay . pointlist , home ) ;
1475+ setgradanddistandaz ( activePointlist , home ) ;
14531476
1454- if ( wpOverlay . pointlist . Count <= 1 )
1477+ if ( activePointlist . Count <= 1 )
14551478 {
1456- RectLatLng ? rect = MainMap . GetRectOfAllMarkers ( wpOverlay . overlay . Id ) ;
1479+ RectLatLng ? rect = MainMap . GetRectOfAllMarkers ( activeOverlay . Id ) ;
14571480 if ( rect . HasValue )
14581481 {
14591482 MainMap . Position = rect . Value . LocationMiddle ;
@@ -1462,8 +1485,9 @@ public void writeKML()
14621485 MainMap_OnMapZoomChanged ( ) ;
14631486 }
14641487
1465- pointlist = wpOverlay . pointlist ;
1488+ pointlist = activePointlist ;
14661489
1490+ if ( ! useV2 )
14671491 {
14681492 foreach ( var pointLatLngAlt in pointlist . PrevNowNext ( ) )
14691493 {
@@ -1479,10 +1503,17 @@ public void writeKML()
14791503
14801504 var pnt = new GMapMarkerPlus ( mid ) ;
14811505 pnt . Tag = new midline ( ) { now = now , next = next } ;
1482- wpOverlay . overlay . Markers . Add ( pnt ) ;
1506+ activeOverlay . Markers . Add ( pnt ) ;
14831507 }
14841508 }
14851509
1510+ editStyleToolStripMenuItem . Visible = useV2 ;
1511+
1512+ // Sync checkbox without re-triggering CheckedChanged -> writeKML
1513+ useLegacyOverlayToolStripMenuItem . CheckedChanged -= useLegacyOverlayToolStripMenuItem_CheckedChanged ;
1514+ useLegacyOverlayToolStripMenuItem . Checked = ! useV2 ;
1515+ useLegacyOverlayToolStripMenuItem . CheckedChanged += useLegacyOverlayToolStripMenuItem_CheckedChanged ;
1516+
14861517 // draw fence
14871518 {
14881519 var fenceoverlay = new WPOverlay ( ) ;
@@ -3472,6 +3503,13 @@ public void FlightPlanner_Load(object sender, EventArgs e)
34723503
34733504 writeKML ( ) ;
34743505
3506+ BeginInvoke ( ( Action ) ( ( ) =>
3507+ Common . MessageShowAgain ( "New Mission Overlay" ,
3508+ "Mission Planner now uses an improved mission overlay with styled lines and markers.\n \n " +
3509+ "You can customize colors, line styles, and markers via Map Tool > Edit Style.\n \n " +
3510+ "If you experience any issues, you can switch back via Map Tool > Use Legacy Overlay." )
3511+ ) ) ;
3512+
34753513 // switch the action and wp table
34763514 if ( Settings . Instance [ "FP_docking" ] == "Bottom" )
34773515 {
@@ -7634,7 +7672,10 @@ private void MainMap_MouseUp(object sender, MouseEventArgs e)
76347672 else if ( ( MAVLink . MAV_MISSION_TYPE ) cmb_missiontype . SelectedValue ==
76357673 MAVLink . MAV_MISSION_TYPE . MISSION )
76367674 {
7637-
7675+ while ( pnt2 > 1 && CommandUtils . IsBookmark ( GetCommandList ( ) [ pnt2 - 2 ] . id ) )
7676+ {
7677+ pnt2 -= 1 ;
7678+ }
76387679 InsertCommand ( pnt2 - 1 , MAVLink . MAV_CMD . WAYPOINT , 0 , 0 , 0 , 0 ,
76397680 CurrentMidLine . Position . Lng ,
76407681 CurrentMidLine . Position . Lat , float . Parse ( TXT_DefaultAlt . Text ) ) ;
@@ -8526,5 +8567,30 @@ private bool checkZeroAlts(int wpno)
85268567
85278568 return true ;
85288569 }
8570+
8571+ MissionStyleEditor _styleEditor ;
8572+
8573+ private void editStyleToolStripMenuItem_Click ( object sender , EventArgs e )
8574+ {
8575+ if ( _styleEditor != null && ! _styleEditor . IsDisposed )
8576+ {
8577+ _styleEditor . BringToFront ( ) ;
8578+ return ;
8579+ }
8580+
8581+ _styleEditor = new MissionStyleEditor ( WPOverlay2 . missionStyle . Config , ( style ) =>
8582+ {
8583+ WPOverlay2 . missionStyle = style ;
8584+ writeKML ( ) ;
8585+ } ) ;
8586+ _styleEditor . FormClosed += ( s , _ ) => _styleEditor = null ;
8587+ _styleEditor . Show ( FindForm ( ) ) ;
8588+ }
8589+
8590+ private void useLegacyOverlayToolStripMenuItem_CheckedChanged ( object sender , EventArgs e )
8591+ {
8592+ Settings . Instance [ "UseWPOverlay2" ] = ( ! useLegacyOverlayToolStripMenuItem . Checked ) . ToString ( ) ;
8593+ writeKML ( ) ;
8594+ }
85298595 }
85308596}
0 commit comments