电压优化、间隔选中、航点偏移优化、去掉阴影为优化速度、导入航点、优化路线(目前无法使用)

Plane.FormationCreator/Formation/FlightTaskManager.cs

@@ -878,6 +878,147 @@ namespace Plane.FormationCreator.Formation
                 i++;
             }
         }
+
+        public void OptimizeRoute2()
+        {
+            double minLat = SelectedTask.SingleCopterInfos[0].TargetLat;
+            double maxLat = SelectedTask.SingleCopterInfos[0].TargetLat;
+            double minLng = SelectedTask.SingleCopterInfos[0].TargetLng;
+            double maxLng = SelectedTask.SingleCopterInfos[0].TargetLng;
+            Dictionary<int, LatLng> recordLatLng = new Dictionary<int, LatLng>();
+            for (int i = 0; i < _copterManager.Copters.Count; i++)
+            {
+                var curinfo = SelectedTask.SingleCopterInfos[i];
+                recordLatLng.Add(i, new LatLng(curinfo.TargetLat, curinfo.TargetLng));
+                var lastInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i];
+                minLat = Math.Min(minLat, Math.Min(curinfo.TargetLat, lastInfo.TargetLat));
+                maxLat = Math.Max(maxLat, Math.Max(curinfo.TargetLat, lastInfo.TargetLat));
+
+                minLng = Math.Min(minLng, Math.Min(curinfo.TargetLng, lastInfo.TargetLng));
+                maxLng = Math.Max(maxLng, Math.Max(curinfo.TargetLng, lastInfo.TargetLng));
+            }
+            double CenterLat = (maxLat - minLat) / 2;
+            double CenterLng = (maxLng - minLng) / 2;
+
+            Dictionary<int, double> distanceDic = new Dictionary<int, double>();
+            for (int i = 0; i < _copterManager.Copters.Count; i++)
+            {
+                var curinfo = SelectedTask.SingleCopterInfos[i];
+                var lastInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i];
+               
+
+                double distance1 = GeographyUtils.CalcDistance(CenterLat, CenterLng, 1,
+                        curinfo.TargetLat, curinfo.TargetLng, 1);
+                //int[] nums = new int[] { SelectedTaskIndex, i};
+                int nums = SelectedTaskIndex << 16 ^ i;
+                distanceDic.Add(nums, distance1);
+                double distance2 = GeographyUtils.CalcDistance(CenterLat, CenterLng, 1,
+                        lastInfo.TargetLat, lastInfo.TargetLng, 1);
+                nums = (SelectedTaskIndex - 1) << 16 ^ i;
+                distanceDic.Add(nums, distance2);
+            }
+            distanceDic = distanceDic.OrderByDescending(o => o.Value).ToDictionary(p => p.Key, o => o.Value);
+            
+            while (distanceDic.Count > 0)
+            {
+                KeyValuePair<int, double> kv = distanceDic.First();
+                int taskIndex = kv.Key >> 16;
+                int copterID = kv.Key & 0xffff;
+                var curInfo = Tasks[taskIndex].SingleCopterInfos[copterID];
+                if (taskIndex == SelectedTaskIndex)
+                {
+                    double mindistance = double.MaxValue;
+                    int index = 0;
+                    for (int i = 0; i < Tasks[taskIndex - 1].SingleCopterInfos.Count; i++)
+                    {
+                        var destInfo = Tasks[taskIndex - 1].SingleCopterInfos[i];
+                        double distance = GeographyUtils.CalcDistance(curInfo.TargetLat, curInfo.TargetLng, 1,
+                        destInfo.TargetLat, destInfo.TargetLng, 1);
+                        if (distance < mindistance)
+                        {
+                            mindistance = distance;
+                            index = i;
+                        }
+                    }
+                    Tasks[taskIndex].SingleCopterInfos[index].TargetLat = recordLatLng[copterID].Lat;
+                    Tasks[taskIndex].SingleCopterInfos[index].TargetLng = recordLatLng[copterID].Lng;
+                    distanceDic.Remove(kv.Key);
+                    distanceDic.Remove(( taskIndex - 1) << 16 ^ index );
+                }                                                         
+                else
+                {
+                    double mindistance = double.MaxValue;
+                    int index = 0;
+                    for (int i = 0; i < Tasks[taskIndex + 1].SingleCopterInfos.Count; i++)
+                    {
+                        var destInfo = Tasks[taskIndex + 1].SingleCopterInfos[i];
+                        double distance = GeographyUtils.CalcDistance(curInfo.TargetLat, curInfo.TargetLng, 1,
+                        destInfo.TargetLat, destInfo.TargetLng, 1);
+                        if (distance < mindistance)
+                        {
+                            mindistance = distance;
+                            index = i;
+                        }
+                    }
+                    Tasks[taskIndex + 1].SingleCopterInfos[copterID].TargetLat = recordLatLng[index].Lat;
+                    Tasks[taskIndex + 1].SingleCopterInfos[copterID].TargetLng = recordLatLng[index].Lng;
+                    distanceDic.Remove(kv.Key);
+                    distanceDic.Remove(( taskIndex + 1) << 16 ^ index );
+                }
+                    
+            }
+            
+        }
+        public void OptimizeRoute()
+        {
+            Dictionary<int[], double> distanceDic = new Dictionary<int[], double>();
+            Dictionary<int, LatLng> recordLatLng = new Dictionary<int, LatLng>();
+            for (int i = 0; i < SelectedTask.SingleCopterInfos.Count; i++)
+            {
+                var curinfo = SelectedTask.SingleCopterInfos[i];
+                recordLatLng.Add(i, new LatLng(curinfo.TargetLat, curinfo.TargetLng));
+                for (int j = 0; j < Tasks[SelectedTaskIndex - 1].SingleCopterInfos.Count; j++)
+                {
+                    var lastInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j];
+                    
+                    double distance = GeographyUtils.CalcDistance(curinfo.TargetLat, curinfo.TargetLng, curinfo.TargetLng,
+                        lastInfo.TargetLat, lastInfo.TargetLng, lastInfo.TargetLng);
+                    int[] nums = new int[] {i,j};
+                    distanceDic.Add(nums, distance);
+                }
+            }
+            distanceDic = distanceDic.OrderBy(o=>o.Value).ToDictionary(p=>p.Key,o=>o.Value);
+            List<int> movedCopters = new List<int>();
+            List<int> usedPoints = new List<int>();
+            foreach (KeyValuePair <int[], double> kv in distanceDic)
+            {
+                int moveCopterNum = kv.Key[1];
+                int destNum = kv.Key[0];
+                if (movedCopters.Contains(moveCopterNum) || usedPoints.Contains(destNum))
+                    continue;
+                SelectedTask.SingleCopterInfos[moveCopterNum].TargetLat = recordLatLng[destNum].Lat;
+                SelectedTask.SingleCopterInfos[moveCopterNum].TargetLng = recordLatLng[destNum].Lng;
+                movedCopters.Add(moveCopterNum);
+                usedPoints.Add(destNum);
+            }
+        }
+
+        public void ImportWaypoint(string tasksText)
+        {
+            dynamic jsonfile = JsonConvert.DeserializeObject(tasksText);
+            dynamic points = jsonfile.points;
+            for (int i = 0; i < SelectedTask.SingleCopterInfos.Count; i++)
+            {
+                MapManager _mapManager = Microsoft.Practices.ServiceLocation.ServiceLocator.Current.GetInstance<MapManager>();
+               
+                var pointjson= points[i];
+                System.Windows.Point point = new System.Windows.Point((int)pointjson.x, (int)pointjson.y);
+                Microsoft.Maps.MapControl.WPF.Location loc = _mapManager.MapView.map.ViewportPointToLocation(point);
+                SelectedTask.SingleCopterInfos[i].TargetLat = loc.Latitude;
+                SelectedTask.SingleCopterInfos[i].TargetLng = loc.Longitude;
+            } 
+        }
+
         // 导入任务
         public void ImportTasks(string tasksText)
         {

Plane.FormationCreator/ViewModels/ConnectViewModel.cs

@@ -152,6 +152,32 @@ namespace Plane.FormationCreator.ViewModels
             }
         }
 
+        private ICommand _StateInquireCommand;
+        public ICommand StateInquireCommand
+        {
+            get
+            {
+                return _StateInquireCommand ?? (_StateInquireCommand = new RelayCommand(async () =>
+                {
+                    await WriteIdCommandAsync(0, 0x00);
+                }
+                ));
+            }
+        }
+
+        private ICommand _CommDataAsync;
+        public ICommand CommDataAsync
+        {
+            get
+            {
+                return _CommDataAsync ?? (_CommDataAsync = new RelayCommand(async () =>
+                {
+                    await commModule.GenerateDataAsync((short)CopterNum);
+                }
+                ));
+            }
+        }
+
         private ICommand _CommWriteMissionCommand;
         public ICommand CommWriteMissionCommand
         {
@@ -300,14 +326,37 @@ namespace Plane.FormationCreator.ViewModels
             await commModule.GeneratePacketAsync((short)num, messageType);
         }
 
-
         private async Task SendCommandAsync()
         {
             Protocols.MavComm.comm_set_mav_count mavCount = new Protocols.MavComm.comm_set_mav_count();
             mavCount.mav_count = 200;
-            await commModule.GeneratePacketAsync(0, (byte)Protocols.MavComm.COMM_SET_MAV_COUNT, mavCount);
+            await commModule.GenerateDataAsync(0, (byte)Protocols.MavComm.COMM_SET_MAV_COUNT, mavCount);
         }
 
+        public async Task<bool> DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
+        {  
+            MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();
+
+            req.target_system = 1;
+            req.target_component = 1;
+
+            if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM)
+            {
+                req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
+            }
+
+            req.command = (ushort)actionid;
+
+            req.param1 = p1;
+            req.param2 = p2;
+            req.param3 = p3;
+            req.param4 = p4;
+            req.param5 = p5;
+            req.param6 = p6;
+            req.param7 = p7;
+            await Task.Delay(10);
+            return true;
+        }
         private async Task WriteCommandAsync(ICopter copter, List<IMission> missions)
         {
             List<MAVLink.mavlink_mission_item_t> mission_list = new List<MAVLink.mavlink_mission_item_t>();

Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs

@@ -87,6 +87,8 @@ namespace Plane.FormationCreator.ViewModels
             }
         }
 
+
+        private static readonly object locker = new object();
         private ICommand _DetectionVoltage;
         public ICommand DetectionVoltage
         {
@@ -94,26 +96,38 @@ namespace Plane.FormationCreator.ViewModels
             {
                 return _DetectionVoltage ?? (_DetectionVoltage = new RelayCommand(async () =>
                 {
+                    Message.Show("--------------开始检测电压--------------");
                     Dictionary<string, float> dic_voltage = new Dictionary<string, float>();
                     await Task.WhenAll(_copterManager.Copters.Select(async c =>
                     {
                         float voltageSum = 0.0f;
+                        string name = c.Name;
+                        
                         for (int i = 0; i < 5; i++)
                         {
+                            
                             voltageSum += c.Voltage;
                             await Task.Delay(1000).ConfigureAwait(false);
                         }
                         float voltageAverage = voltageSum / 5;
-                        dic_voltage.Add(c.Name, voltageAverage);
+                        if (name != null && name != "")
-                    }));
+                        {
-                    dic_voltage = dic_voltage.OrderByDescending(o => o.Value).ToDictionary(p => p.Key, o => o.Value);
+                            lock(locker)
-                    Message.Show("--------------开始检测电压--------------");
+                            {
-                    foreach (KeyValuePair<string, float> kv in dic_voltage)
+                                dic_voltage.Add(name, voltageAverage);
-                    {
+                            }
-                        Message.Show(string.Format("{0} --> 5秒平均电压：{1}", kv.Key, kv.Value));
+                        }
-                        await Task.Delay(5).ConfigureAwait(false);
+                    })).ConfigureAwait(false);
-                    }
+                    await Task.Run(async () => {
-                    Message.Show("--------------检测电压完成--------------");
+                        Dictionary<string, float> dic_voltage_Order = dic_voltage.OrderByDescending(o => o.Value).ToDictionary(o => o.Key, o => o.Value);
+                        
+                        foreach (KeyValuePair<string, float> kv in dic_voltage_Order)
+                        {
+                            Message.Show(string.Format("{0} --> 5秒平均电压：{1}", kv.Key, kv.Value));
+                            await Task.Delay(5).ConfigureAwait(false);
+                        }
+                        Message.Show(string.Format("----检测电压完成,检测总数：{0}----", dic_voltage.Count));
+                    }).ConfigureAwait(false);
                 }));
             }
         }

Plane.FormationCreator/ViewModels/CopterListViewModel.cs

@@ -88,6 +88,13 @@ namespace Plane.FormationCreator.ViewModels
             set { Set(nameof(IntervalNum), ref _IntervalNum, value); }
         }
 
+        private int _ContinuousNum;
+        public int ContinuousNum
+        {
+            get { return _ContinuousNum; }
+            set { Set(nameof(ContinuousNum), ref _ContinuousNum, value); }
+        }
+
         private ICommand _IntervalSelectCoptersCommand;
         public ICommand IntervalSelectCoptersCommand
         {
@@ -95,16 +102,21 @@ namespace Plane.FormationCreator.ViewModels
             {
                 return _IntervalSelectCoptersCommand ?? (_IntervalSelectCoptersCommand = new RelayCommand(() =>
                 {
-                    if (_copterManager.AcceptingControlCopters.Count() == 0 || IntervalNum == 0)
+                    if (_copterManager.AcceptingControlCopters.Count() != 1 || IntervalNum == 0 || ContinuousNum == 0)
                         return;
 
-                    ICopter copter = _copterManager.AcceptingControlCopters.FirstOrDefault();
+                    ICopter copter = _copterManager.SelectedCopters.FirstOrDefault();
                     _copterManager.Select(null);
                     int index = _copterManager.Copters.IndexOf(copter);
                     _copterManager.shiftkeydown = true;
-                    for (; index < _copterManager.Copters.Count; index += IntervalNum + 1)
+                    for (; index < _copterManager.Copters.Count; index += IntervalNum)
                     {
-                        _copterManager.Select(_copterManager.Copters[index]);
+                        for (int i = 0; i < ContinuousNum; i++)
+                        {
+                            if (index >= _copterManager.Copters.Count) break;
+                            _copterManager.Select(_copterManager.Copters[index]);
+                            index++;
+                        } 
                     }
                     _copterManager.shiftkeydown = false;
 
@@ -122,9 +134,9 @@ namespace Plane.FormationCreator.ViewModels
                     var center = _mapManager.Center;
                     string id;
 
-                    int colnum = 5; //自动生成列数=4
+                    int colnum = 10; //自动生成列数=4
-                    float coldis = 3;//列相距5米
+                    float coldis = 5;//列相距5米
-                    float rowdis = 3;//行相距5米
+                    float rowdis = 2.5f;//行相距5米
                     int currcol = 0; //当前列号
                     int currrow = 0; //当前行
                     Tuple<double, double> colheadLatLng = new Tuple<double, double>(0, 0);
@@ -167,23 +179,11 @@ namespace Plane.FormationCreator.ViewModels
 
 
 
-                      /*  _lastVirtualCopterLocation =
+                        /*  _lastVirtualCopterLocation =
-                            _lastVirtualCopterLocation == null ?
+                              _lastVirtualCopterLocation == null ?
-                            new LatLng(center.Lat, center.Lng) :
+                              new LatLng(center.Lat, center.Lng) :
-                            new LatLng(_lastVirtualCopterLocation.Value.Lat, _lastVirtualCopterLocation.Value.Lng + 0.0001);
+                              new LatLng(_lastVirtualCopterLocation.Value.Lat, _lastVirtualCopterLocation.Value.Lng + 0.0001);
-                            */
+                              */
-
-
-
-
-
-
-
-
-
-
-
-
 
 
 

Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs

@@ -505,7 +505,39 @@ namespace Plane.FormationCreator.ViewModels
                 }));
             }
         }
+        private ICommand _ImportWayPointCommand;
+        public ICommand ImportWayPointCommand
+        {
+            get
+            {
+                return _ImportWayPointCommand ?? (_ImportWayPointCommand = new RelayCommand<int>(async =>
+                {
+                    var dialog = new OpenFileDialog
+                    {
+                        DefaultExt = "json",
+                        Filter = "json文件|*.json"
+                    };
+                    if (dialog.ShowDialog() == true)
+                    {
+                        var tasksText = File.ReadAllText(dialog.FileName);
+                        _flightTaskManager.ImportWaypoint(tasksText);
                        
+                    }
+                }));
+            }
+        }
+
+        private ICommand _OptimizeRouteCommand;
+        public ICommand OptimizeRouteCommand
+        {
+            get
+            {
+                return _OptimizeRouteCommand ?? (_OptimizeRouteCommand = new RelayCommand<int>(async =>
+                {
+                    _flightTaskManager.OptimizeRoute2();
+                }));
+            }
+        }
 
         private ICommand _LevelAlignmentCommand;
         public ICommand LevelAlignmentCommand
@@ -1255,7 +1287,6 @@ public ICommand VerticlAlignmentCommand
                     }
                     if ((lng1 != 0) && (lng2 != 0))
                     {
-
                         distance = GeographyUtils.CalcDistance(
                         lat1, lng1, alt1,
                         lat2, lng2, alt2);

Plane.FormationCreator/Views/ConnectWindow.xaml

@@ -66,6 +66,7 @@
             <RowDefinition />
             <RowDefinition Height="Auto" />
             <RowDefinition Height="Auto" />
+            <RowDefinition Height="Auto" />
         </Grid.RowDefinitions>
         <Grid.ColumnDefinitions>
             <ColumnDefinition Width="Auto" />
@@ -108,7 +109,7 @@
                                IsProcessing="{Binding IsProcessing}"
                                Command="{Binding Path=ConnectCommand}"
                                CommandParameter="SerialPort" />
-            <Button Content="切换写航点"  Margin="5,5,0,5" Command="{Binding Path=ChangeWriteMissionCommand}" />
+            
             <!--<ec:ProgressButton Name="btnConnectUdp"
                                Grid.Row="1"
                                Grid.ColumnSpan="3"
@@ -120,17 +121,27 @@
                                IsProcessing="{Binding IsProcessing}"
                                Command="{Binding Path=ConnectCommand}"
                                CommandParameter="UDP" />-->
+            <Button Content="关闭TCP" Margin="5" Command="{Binding CloseCommand}"/>
         </StackPanel>
         <StackPanel Orientation="Horizontal"
                     HorizontalAlignment="Center"
                     Grid.Row="2"
                     Grid.ColumnSpan="3">
-            <Button Content="关闭TCP" Margin="5" Command="{Binding CloseCommand}"/>
+            <Button Content="状态查询" Margin="5" Command="{Binding Path=StateInquireCommand}"/>
             <Button Content="设置20台" Margin="5" Command="{Binding Path=SendCommand}" />
             <Button Content="写入航点" Margin="5" Command="{Binding Path=CommWriteMissionCommand}" />
-            <Button Content="广播编号"  Margin="5,5,0,5" Command="{Binding Path=WriteIdCommand}" />
+            <Button Content="切换写航点"  Margin="5" Command="{Binding Path=ChangeWriteMissionCommand}" />
             
+            
+        </StackPanel>
+        <StackPanel Orientation="Horizontal"
+                    HorizontalAlignment="Center"
+                    Grid.Row="3"
+                    Grid.ColumnSpan="3">
             <TextBox Margin="2,5,5,5" Width="30" Text="{Binding CopterNum}"></TextBox>
+            <Button Content="广播编号"  Margin="5,5,0,5" Command="{Binding Path=WriteIdCommand}" />
+            <Button Content="闪灯" Margin="5" Command="{Binding CommDataAsync}"/>
+            
         </StackPanel>
 
     </Grid>

Plane.FormationCreator/Views/CopterInfoView.xaml

@@ -33,7 +33,7 @@
 
         <Grid.ColumnDefinitions>
             <ColumnDefinition Width="180" />
-            <ColumnDefinition Width="120" />
+            <ColumnDefinition Width="Auto" />
         </Grid.ColumnDefinitions>
 
         <StackPanel Orientation="Vertical">

Plane.FormationCreator/Views/CopterListView.xaml

@@ -56,8 +56,11 @@
         <StackPanel Grid.Row="1" Margin="0,0,0,5"
                     Orientation="Horizontal">
             <Button Content="间隔选中" Command="{Binding IntervalSelectCoptersCommand}"/>
+            <TextBlock Text="连选数量" Margin="5,5,0,0"/>
+            <TextBox Width="30" Margin="5,0,0,0" Text="{Binding ContinuousNum}"/>
             <TextBlock Text="间隔数量" Margin="5,5,0,0"/>
             <TextBox Width="30" Margin="5,0,0,0" Text="{Binding IntervalNum}"/>
+            
         </StackPanel>
 
         <StackPanel Grid.Row="2"

Plane.FormationCreator/Views/MapView.xaml

@@ -10,6 +10,7 @@
              d:DesignHeight="300"
              d:DesignWidth="300">
     <Grid>
+        
         <bingMaps:Map Name="map"
                       CredentialsProvider="8IGVSMWVqW8lDaMuGr2c~XaqB2qlBDLvSvXFzrQ8c-A~AiPIQttopdwAl4kXs8xm6_r59NEGdyqXejcaMDum6qB1BUJ6e25uViKL7fEdEROP"
                       ZoomLevel="20">
@@ -62,5 +63,6 @@
 
 
 
+
     </Grid>
 </UserControl>

Plane.FormationCreator/Views/MapView.xaml.cs

@@ -109,7 +109,6 @@ namespace Plane.FormationCreator.Views
 
             _copterManager.Copters.ForEach(copter => copter.LocationChanged += Copter_LocationChanged);
             _copterManager.Copters.CollectionChanged += Copters_CollectionChanged;
-
             //_copterManager.Copters.ForEach(copter => AddOrMoveCopterForModifyingTask(copter));
             //_copterManager.Copters.CollectionChanged += CoptersForModifyingTask_CollectionChanged;
 
@@ -149,6 +148,14 @@ namespace Plane.FormationCreator.Views
                   if ( map.Mode.GetType().ToString() == "Microsoft.Maps.MapControl.WPF.AerialMode")
                     if (map.ZoomLevel >19)
                       map.ZoomLevel = 19;
+                  if (original!= null && map.Children.Contains(original))
+                  {
+                      Location location = new Location(_flightTaskManager.OriginLat, _flightTaskManager.OriginLng);
+                      Point wpPos = map.LocationToViewportPoint(location);
+                      wpPos.X -= ORIGIN_RADIUS;
+                      wpPos.Y -= ORIGIN_RADIUS;
+                      MapLayer.SetPosition(original, map.ViewportPointToLocation(wpPos));
+                  }
               };
 
             Rectangle rectangle = new Rectangle();
@@ -261,7 +268,8 @@ namespace Plane.FormationCreator.Views
         public void GoHome()
         {
             map.ZoomLevel = 19;
-            map.Center = new Location(40.0559055, 116.322233);
+            //map.Center = new Location(40.0559055, 116.322233);
+            map.Center = new Location(40.6801557090282, 114.670060030638);
         }
 
 
@@ -318,6 +326,10 @@ namespace Plane.FormationCreator.Views
                     map.Children.RemoveAt(i);
                 }
             }
+            foreach (var item in _copterDrawings)
+            {
+                item.Value.RemoveMap_ViewChanged();
+            }
             _copterDrawings.Clear();
         }
 
@@ -372,6 +384,8 @@ namespace Plane.FormationCreator.Views
                 PointCount = 5,
             };
             
+         
+
             map.Children.Add(original);
             MapLayer.SetPosition(original, mapLocation);
             MapLayer.SetZIndex(original, 200);

Plane.FormationCreator/Views/MapView_CopterDrawing.cs

@@ -48,8 +48,19 @@ namespace Plane.FormationCreator.Views
                     }
                     this.Waypoints.Clear();
                 };
+
+                //_map.ViewChangeOnFrame += new EventHandler<MapEventArgs>(Map_ViewChanged);
+                
+
+                selectMarkup.Width = 7;
+                selectMarkup.Height = 7;
+                selectMarkup.HorizontalAlignment = HorizontalAlignment.Left;
+                selectMarkup.VerticalAlignment = VerticalAlignment.Top;
+                selectMarkup.Fill = new SolidColorBrush(Colors.Black);
             }
 
+            Rectangle selectMarkup = new Rectangle();
+
             static DropShadowEffect _selectedEffect = new DropShadowEffect();
 
             static SolidColorBrush _selectedTaskStroke = new SolidColorBrush(Colors.White);
@@ -81,7 +92,7 @@ namespace Plane.FormationCreator.Views
             public Location LastLocation { get; set; }
 
             public MapPolyline Route { get; set; }
-            public List<Ellipse> Waypoints { get; set; } = new List<Ellipse>();
+            public List<ShapesContainer> Waypoints { get; set; } = new List<ShapesContainer>();
 
             Map _map;
             Color _color;
@@ -91,6 +102,22 @@ namespace Plane.FormationCreator.Views
             CopterManager _copterManager = ServiceLocator.Current.GetInstance<CopterManager>();
             FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
 
+
+            private void Map_ViewChanged<MapEventArgs>(object sender, MapEventArgs e)
+            {
+                for (int index = 1; index < _flightTaskManager.Tasks.Count; index++)
+                {
+                    var info = _flightTaskManager.Tasks[index].SingleCopterInfos.Find(i => i.Copter == this.Copter); ;
+                    ShapesContainer wpContainer = Waypoints[index - 1];  //起飞任务没有Waypoint;
+                    Location location = new Location(info.TargetLat, info.TargetLng);
+                    Point wpPos = _map.LocationToViewportPoint(location);
+                    wpPos.X -= WAYPOINT_RADIUS;
+                    wpPos.Y -= WAYPOINT_RADIUS;
+                    MapLayer.SetPosition(wpContainer, _map.ViewportPointToLocation(wpPos));
+                }
+
+            }
+
             public void AddOrMoveCopter()
             {
                 try
@@ -198,8 +225,8 @@ namespace Plane.FormationCreator.Views
                     if (Copter.GetShowLEDAsync())
                         blackBrush.Color = Colors.White;
                     else
-                        blackBrush.Color = Colors.Transparent;
+                         blackBrush.Color = Colors.Transparent;
-                    Dot.Stroke = blackBrush;
+                   Dot.Stroke = blackBrush;
 
                     var trans = Dot.RenderTransform as RotateTransform;
                     if (trans == null)
@@ -231,25 +258,18 @@ namespace Plane.FormationCreator.Views
             public void AddWaypoint(Location location, FlightTaskType type)
             {
                 // Add waypoint.
-                /*
+
-                if (!Copter.IsGpsAccurate && !(Copter is FakeCopter))
+               
-                    location = _map.Center;
+                ShapesContainer shapesContainer = new ShapesContainer(_brush);
-                    */
+                shapesContainer.Tag = WAYPOINT_TAG;
-                var wp = new Ellipse
+
-                {
+                Waypoints.Add(shapesContainer);
-                    Tag = WAYPOINT_TAG,
+                _map.Children.Add(shapesContainer);
-                    Fill = _brush,
+                MapLayer.SetZIndex(shapesContainer, 100);
-                    StrokeThickness = 2,
-                    Width = WAYPOINT_RADIUS * 2,
-                    Height = WAYPOINT_RADIUS * 2
-                }; ;
-                Waypoints.Add(wp);
-                _map.Children.Add(wp);
-                MapLayer.SetZIndex(wp, 100);
                 var wpPos = _map.LocationToViewportPoint(location);
-                wpPos.X -= WAYPOINT_RADIUS;
+                //wpPos.X -= WAYPOINT_RADIUS;
-                wpPos.Y -= WAYPOINT_RADIUS;
+                //wpPos.Y -= WAYPOINT_RADIUS;
-                MapLayer.SetPosition(wp, _map.ViewportPointToLocation(wpPos));
+                MapLayer.SetPosition(shapesContainer, _map.ViewportPointToLocation(wpPos));
 
                 SetEffect(_copterManager.SelectedCopters.Contains(Copter));
 
@@ -257,10 +277,10 @@ namespace Plane.FormationCreator.Views
                 Route.Locations.Add(location);
 
                 // Register event handlers.
-                RegisterEventHandlersForDraggingWaypoint(wp, taskIndex: Waypoints.Count);
+                RegisterEventHandlersForDraggingWaypoint(shapesContainer, taskIndex: Waypoints.Count);
 
                 // Register event handlers for task info.
-                RegisterEventHandlersForTaskInfo(wp, taskIndex: Waypoints.Count);
+                RegisterEventHandlersForTaskInfo(shapesContainer, taskIndex: Waypoints.Count);
             }
 
             private void RegisterEventHandlersForDraggingCopter(Grid copterElement)
@@ -310,7 +330,7 @@ namespace Plane.FormationCreator.Views
 
             private Dictionary<ICopter, Point> selectWayOriginPoint = new Dictionary<ICopter, Point>();
             private Dictionary<object, bool> _dictDraggingWp = new Dictionary<object, bool>();
-            private void RegisterEventHandlersForDraggingWaypoint(Ellipse wp, int taskIndex)
+            private void RegisterEventHandlersForDraggingWaypoint(Grid wp, int taskIndex)
             {
                 _dictDraggingWp[wp] = false;
                 double originX = 0;
@@ -376,9 +396,9 @@ namespace Plane.FormationCreator.Views
 
                         var routePoint = this.Route.Locations[taskIndex];
                         var leftTopPos = new Point(eventPos.X - wpOffsetX, eventPos.Y - wpOffsetY);
-                        var newRoutePoint = new Point(leftTopPos.X + WAYPOINT_RADIUS, leftTopPos.Y + WAYPOINT_RADIUS);
+                        //var newRoutePoint = new Point(leftTopPos.X + WAYPOINT_RADIUS, leftTopPos.Y + WAYPOINT_RADIUS);
-                        routePoint.Latitude = _map.ViewportPointToLocation(newRoutePoint).Latitude;
+                        routePoint.Latitude = _map.ViewportPointToLocation(leftTopPos).Latitude;
-                        routePoint.Longitude = _map.ViewportPointToLocation(newRoutePoint).Longitude;
+                        routePoint.Longitude = _map.ViewportPointToLocation(leftTopPos).Longitude;
                         MapLayer.SetPosition(wp, _map.ViewportPointToLocation(leftTopPos));
                     }
                 };
@@ -388,7 +408,7 @@ namespace Plane.FormationCreator.Views
                 };
             }
 
-            private void RegisterEventHandlersForTaskInfo(Ellipse wp, int taskIndex)
+            private void RegisterEventHandlersForTaskInfo(Grid wp, int taskIndex)
             {
                 var info = _flightTaskManager.Tasks[taskIndex].SingleCopterInfos.FirstOrDefault(i => i.Copter == this.Copter);
                 if (info == null) return;
@@ -403,7 +423,7 @@ namespace Plane.FormationCreator.Views
                             {
                                 var centerLocation = new Location(info.TargetLat, info.TargetLng, info.TargetAlt);
                                 var centerPos = _map.LocationToViewportPoint(centerLocation);
-                                var leftTopPos = new Point(centerPos.X - WAYPOINT_RADIUS, centerPos.Y - WAYPOINT_RADIUS);
+                                var leftTopPos = new Point(centerPos.X, centerPos.Y);
                                 MapLayer.SetPosition(wp, _map.ViewportPointToLocation(leftTopPos));
                                 var routePoint = Route.Locations[taskIndex];
                                 routePoint.Latitude = info.TargetLat;
@@ -426,6 +446,11 @@ namespace Plane.FormationCreator.Views
                 }
             }
 
+            public void RemoveMap_ViewChanged()
+            {
+                _map.ViewChangeOnFrame -= Map_ViewChanged;
+            }
+
             public void SetShowroute(bool? showroute)
             {
              
@@ -448,8 +473,29 @@ namespace Plane.FormationCreator.Views
 
             public void SetEffect(bool selected)
             {
-                Dot.Effect = selected ? _selectedEffect : null;
+            
+                if (selected)
+                {
+                    if (!DotContainer.Children.Contains(selectMarkup))
+                        DotContainer.Children.Add(selectMarkup);
+                }
+                    
+                else
+                {
+                    if (DotContainer.Children.Contains(selectMarkup))
+                        DotContainer.Children.Remove(selectMarkup);
+                }
+
+
+                Waypoints.ForEach(wp => wp.Ismark = selected);
+
+
+                /*
                 Waypoints.ForEach(wp => wp.Effect = Dot.Effect);
+
+                //Dot.Effect = selected ? _selectedEffect : null;
+                Waypoints.ForEach(wp => wp.Effect = Dot.Effect);
+                */
             }
 
             public void SetTaskEffect(int taskIndex)
@@ -457,8 +503,8 @@ namespace Plane.FormationCreator.Views
                 var wpIndex = taskIndex - 1;    // Waypoints 中没有起飞点。
                 if (wpIndex >= 0 && wpIndex < Waypoints.Count)
                 {
-                    Waypoints.ForEach(p => p.Stroke = null);
+                    Waypoints.ForEach(p => p.wp.Stroke = null);
-                    var wp = Waypoints[wpIndex];
+                    var wp = Waypoints[wpIndex].wp;
                     wp.Stroke = _selectedTaskStroke;
                 }
             }
@@ -490,5 +536,51 @@ namespace Plane.FormationCreator.Views
                 }
             }
         }
+
+        public class ShapesContainer : Grid
+        {
+            const double WAYPOINT_RADIUS = 6;
+            public ShapesContainer(Brush brush)
+            {
+                wp = new Ellipse
+                {
+                    //Tag = WAYPOINT_TAG,
+                    Fill = brush,
+                    StrokeThickness = 2,
+                    Width = WAYPOINT_RADIUS * 2,
+                    Height = WAYPOINT_RADIUS * 2,
+                    Margin = new Thickness(-WAYPOINT_RADIUS, -WAYPOINT_RADIUS, 0, 0)
+                };
+                this.Children.Add(wp);
+                mark = new Rectangle();
+                mark.Width = 6;
+                mark.Height = 6;
+                mark.HorizontalAlignment = HorizontalAlignment.Left;
+                mark.VerticalAlignment = VerticalAlignment.Top;
+                mark.Fill = new SolidColorBrush(Colors.Black);
+                this.Children.Add(mark);
+
+                mark.Visibility = Visibility.Hidden;
+            }
+
+            //public MapPolygon
+            private bool isMarked;
+            public Ellipse wp;
+            public Rectangle mark;
+
+            public bool Ismark
+            {
+                get
+                {
+                    return isMarked;
+                }
+
+                set
+                {
+                    isMarked = value;
+                    mark.Visibility = value ? Visibility.Visible : Visibility.Hidden;
+                }
+            }
+        }
     }
 }

Plane.FormationCreator/Views/ModifyTaskView.xaml

@@ -65,7 +65,12 @@
                                 <Setter Property="Orientation" Value="Horizontal" />
                             </Style>
                         </StackPanel.Resources>
-                        <TextBlock Margin="5" Text="航点任务"/>
+                        <StackPanel>
+                            <TextBlock Margin="5" Text="航点任务"/>
+                            <Button Content="导入航点" Command="{Binding ImportWayPointCommand}" />
+                            <Button Content="优化路线" Command="{Binding OptimizeRouteCommand}" />
+                        </StackPanel>
+                        
                         <Separator/>
                         <StackPanel>
                             <Button Content="上边对齐"