导入导出高度计算上摆放高度

Plane.FormationCreator/Formation/Extensions.cs

@@ -79,16 +79,17 @@ namespace Plane.FormationCreator.Formation
             return Math.Abs(copter.Heading - targetHeading) < 2;
         }
 
-        public static bool IsTooCloseTo(this ICopter copter, ICopter copter2)
+        public static bool IsTooCloseTo(this ICopter copter, ICopter copter2, out double distance)
         {
+            distance = Math.Round(copter.DistanceTo(copter2), 2) ;
             //return false;  //效果图用  不检测碰撞
             if (copter.Altitude < 3 || copter2.Altitude < 3)
                 return false;
 
             if ((copter.GpsFixType == GpsFixType.RTKFIXED) && (copter2.GpsFixType == GpsFixType.RTKFIXED))
-                return copter.DistanceTo(copter2) < RTKClosedDis;  //最近距离0.5米
+                return distance < RTKClosedDis;  //最近距离0.5米
             else
-                return copter.DistanceTo(copter2) < GpsCloseDis;  //最近距离2米
+                return distance < GpsCloseDis;  //最近距离2米
         }
     }
 }

Plane.FormationCreator/Formation/FlightTaskManager.cs

@@ -421,20 +421,30 @@ namespace Plane.FormationCreator.Formation
             string[] lines = txt.Replace("\r\n", "\n").Split('\n');
             if (lines.Length != _copterManager.Copters.Count)
                 return;
-
+            Dictionary<int, string[]> PointDic = new Dictionary<int, string[]>();
-
-            var lastTask = Tasks.LastOrDefault();
-            var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = true, FlytoTime = 10, LoiterTime = 10 };
-            int i = 0;
             foreach (string line in lines)
             {
-                
                 string[] parameters = line.Split(' ');
                 string id = parameters[0];
                 string frame = parameters[1];
-                string x = parameters[2];   //左右  经度
+                string[] point = new string[3];
-                string y = parameters[3];   //上下  高度  
+                point[0] = parameters[2];   //左右  经度
-                string z = parameters[4];   //前后  纬度
+                point[1] = parameters[3];   //上下  高度  
+                point[2] = parameters[4];   //前后  纬度
+                PointDic.Add(Convert.ToInt32(id), point);
+            }
+           
+
+            var lastTask = Tasks.LastOrDefault();
+            var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = true, FlytoTime = 10, LoiterTime = 10 };
+            for (int i = 0; i < PointDic.Count; i++)
+            {
+                string[] point = PointDic[i + 1];
+
+                //string frame = parameters[1];
+                string x = point[0];   //左右  经度
+                string y = point[1];   //上下  高度  
+                string z = point[2];   //前后  纬度
                 Tuple<double, double> observationLatLng = null;
                 observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
                    OriginLat,
@@ -449,10 +459,10 @@ namespace Plane.FormationCreator.Formation
                   Convert.ToSingle(z) / 100);
 
                 var thisSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
-                        _copterManager.Copters[Convert.ToInt32(id)], new LatLng(observationLatLng.Item1 - OriginLat, observationLatLng.Item2 - OriginLng),
+                        _copterManager.Copters[i], new LatLng(observationLatLng.Item1 - OriginLat, observationLatLng.Item2 - OriginLng),
                         Convert.ToSingle(y)/100, false);
                 newTask.SingleCopterInfos.Add(thisSingleCopterInfo);
-                i++;
+
             }
             Tasks.Add(newTask);
             TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask });
@@ -528,7 +538,7 @@ namespace Plane.FormationCreator.Formation
                     tagalt = (float)singleCopterInfoObj.targetAlt;
                     newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
                         copter, new LatLng((double)singleCopterInfoObj.latOffset,
-                        (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (bool)singleCopterInfoObj.isLandWaypoint);
+                        (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt - copter.GroundAlt, (bool)singleCopterInfoObj.isLandWaypoint);
                     
                     var jsonArray = singleCopterInfoObj.ledInfos as Newtonsoft.Json.Linq.JArray;
                     ObservableCollection<LEDInfo> ledList = jsonArray.ToObject<ObservableCollection<LEDInfo>>();
@@ -660,6 +670,16 @@ namespace Plane.FormationCreator.Formation
             {
                 var singleCopterInfoObj = singleCopterInfos[i];
                 takeOffTask.SingleCopterInfos[i].TakeOffWaitTime = (ushort)singleCopterInfoObj.waitTime;
+                if (singleCopterInfoObj.ledInfos!= null)
+                {
+                    var jsonArray = singleCopterInfoObj.ledInfos as Newtonsoft.Json.Linq.JArray;
+                    ObservableCollection<LEDInfo> ledList = jsonArray.ToObject<ObservableCollection<LEDInfo>>();
+                    foreach (LEDInfo info in ledList)
+                    {
+                        takeOffTask.SingleCopterInfos[i].AddLEDInfo(info);
+                    }
+                }
+                    
                 //Message.Show(((ushort)singleCopterInfoObj.waitTime).ToString());
             }
         }
@@ -832,7 +852,8 @@ namespace Plane.FormationCreator.Formation
                             {
                                 return new
                                 {
-                                    waitTime = info.TakeOffWaitTime
+                                    waitTime = info.TakeOffWaitTime,
+                                    ledInfos = info.LEDInfos
                                 };
                             })
                         };
@@ -850,7 +871,7 @@ namespace Plane.FormationCreator.Formation
                                 {
                                     latOffset = offset.Lat,
                                     lngOffset = offset.Lng,
-                                    targetAlt = info.TargetAlt,
+                                    targetAlt = info.TargetAlt + info.Copter.GroundAlt,  //导出时加上地面摆放高度，导入后若没有摆放高度，模拟就会清楚的看到高度错误
                                     //showLED = info.FlytoShowLED,
                                     ledInfos = info.LEDInfos,
                                     isLandWaypoint = info.IsLandWaypoint,
@@ -1210,10 +1231,10 @@ namespace Plane.FormationCreator.Formation
             }
         }
 
-        private void ImportCoptersLocate(dynamic coptersLocate)
+//         private void ImportCoptersLocate(dynamic coptersLocate)
-        {
+//         {
-
+// 
-        }
+//         }
 
         private void ImportGroupsInfo(dynamic coptersLocate)
         {
@@ -1417,15 +1438,15 @@ namespace Plane.FormationCreator.Formation
                     // bool RTLFlag = false;
                     //if ( (CurrentRunningTaskIndex != 0) && !RTLFlag)     
                     {
-                        
+                        double distance;
                         foreach (var copter in _copterManager.Copters)
                         {
                             foreach (var anotherCopter in _copterManager.Copters)
                             {
-                                if (copter != anotherCopter && copter.IsTooCloseTo(anotherCopter))
+                                if (copter != anotherCopter && copter.IsTooCloseTo(anotherCopter, out distance))
                                 {
                                     //Pause();
-                                    Message.Show($"{copter.Name} 与 {anotherCopter.Name} 距离过近，已中止任务。");
+                                    Message.Show($"{copter.Name} 与 {anotherCopter.Name} 距离过近，间距{distance}米。");
                                     //  Alert.Show($"{copter.Name} 与 {anotherCopter.Name} 距离过近，已中止任务。");
                                     //return;
                                 }

Plane.FormationCreator/MainWindow.xaml

@@ -103,8 +103,8 @@
                             <TextBox VerticalAlignment="Bottom" Width="40"
                                      Text="{Binding txtendindex, UpdateSourceTrigger=PropertyChanged}"/>
                         </StackPanel>
-                        <Button BorderThickness="1" BorderBrush="#FFFFFF" Content="导入摆放位置" Margin="25,8,0,8" Grid.Row="2"
+                        <Button BorderThickness="1" BorderBrush="#FFFFFF" Content="导入分组信息" Margin="25,8,0,8" Grid.Row="2"
-                                Command="{Binding ImportTasksCommand}"/>
+                                Command="{Binding ImportGroupCommand}"/>
                     </Grid>
                 </MenuItem>
             </Menu>

Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs

@@ -441,6 +441,37 @@ namespace Plane.FormationCreator.ViewModels
             }
         }
 
+        private float _LatOffset = 18.4023833f;
+        public float LatOffset
+        {
+            get { return _LatOffset; }
+            set { Set(nameof(LatOffset), ref _LatOffset, value); }
+        }
+
+        private float _LngOffset = 109.799761f;
+        public float LngOffset
+        {
+            get { return _LngOffset; }
+            set { Set(nameof(LngOffset), ref _LngOffset, value); }
+        }
+
+
+        private ICommand _RLTOffsetCommand;
+        public ICommand RLTOffsetCommand
+        {
+            get
+            {
+                return _RLTOffsetCommand ?? (_RLTOffsetCommand = new RelayCommand(async () =>
+                {
+                    Message.Show($"偏移Lat = {LatOffset}，Lng = {LngOffset}");
+                    if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
+                        await _commModuleManager.RLTOffsetAsync(LatOffset, LngOffset, _copterManager.AcceptingControlCopters);
+
+
+                }));
+            }
+        }
+
         private ICommand _LockCommand;
         public ICommand LockCommand
         {

Plane.FormationCreator/ViewModels/MainViewModel.cs

@@ -175,7 +175,7 @@ namespace Plane.FormationCreator.ViewModels
         }
 
 
-        private bool _OnlyImpotWaypointS = true;
+        private bool _OnlyImpotWaypointS = false;
         public bool OnlyImpotWaypointS
         {
             get { return _OnlyImpotWaypointS; }
@@ -289,7 +289,7 @@ namespace Plane.FormationCreator.ViewModels
                             taskinfo = importInfo.tasks;
                             if (importInfo.locate != null)
                             {
-                                //ImportCoptersLocate(importInfo.locate);
+                                ImportCoptersLocate(importInfo.locate);
                             }
                             if (importInfo.groups != null)
                             {
@@ -322,6 +322,53 @@ namespace Plane.FormationCreator.ViewModels
             }
         }
 
+        private ICommand _ImportGroupCommand;
+        public ICommand ImportGroupCommand
+        {
+            get
+            {
+                return _ImportGroupCommand ?? (_ImportGroupCommand = new RelayCommand(() =>
+                {
+                    var dialog = new OpenFileDialog
+                    {
+                        DefaultExt = "fcg",
+                        Filter = "编队飞行任务 (*.fcg)|*.fcg"
+                    };
+                    if (dialog.ShowDialog() == true)
+                    {
+
+                        var importText = File.ReadAllText(dialog.FileName);
+
+                        dynamic importInfo = JsonConvert.DeserializeObject(importText);
+                        dynamic taskinfo = null;
+                        if (importInfo is Newtonsoft.Json.Linq.JObject)
+                        {
+                            taskinfo = importInfo.tasks;
+                            if (importInfo.groups != null)
+                            {
+                                _groupManager.ImportGroupsInfo(importInfo.groups);
+                            }
+                        }
+                        else if (importInfo is Newtonsoft.Json.Linq.JArray)
+                        {
+                            taskinfo = importInfo;
+                        }
+                    }
+                }));
+            }
+        }
+
+        private void ImportCoptersLocate(dynamic coptersLocate)
+        {
+            int index = 0;
+            foreach (var locate in coptersLocate)
+            {
+                _copterManager.Copters[index].GroundAlt = Convert.ToSingle(locate[2]);
+                index++;
+
+            }
+        }
+
         private ICommand _ClearLogs;
         public ICommand ClearLogs
         {

Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs

@@ -1219,7 +1219,7 @@ public ICommand VerticlAlignmentCommand
                             (_flightTaskManager.Tasks[i].TaskType == FlightTaskType.ReturnToLand))
                             continue;
 
-
+                     
                         for (int j = 0; j < _flightTaskManager.Tasks[i].SingleCopterInfos.Count; j++)
                         {
                             _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetAlt += Modialtvalue;
@@ -1229,7 +1229,6 @@ public ICommand VerticlAlignmentCommand
                                 lowtask = i;
                                 lowCopter = j;
                             }
-
                         }
                     }
                     Alert.Show("指定步骤任务高度已改变"+ Modialtvalue + "米，步骤["+ lowtask + "],ID["+

Plane.FormationCreator/Views/ControlPanelView.xaml

@@ -84,6 +84,12 @@
                      Command="{Binding WriteMissionSingleCommand}" />
             <Button Content="统计模块" 
                      Command="{Binding DetectionCommModuleVersion}" />
+            <Label>Lat</Label>
+            <TextBox Text="{Binding LatOffset}" Width="50"/>
+            <Label>Lng</Label>
+            <TextBox Text="{Binding LngOffset}" Width="50"/>
+            <Button Content="设置返航点" 
+                     Command="{Binding RLTOffsetCommand}" />
         </WrapPanel>
         <Separator/>
         <WrapPanel>

Plane.FormationCreator/Views/MapView.xaml.cs

@@ -230,7 +230,15 @@ namespace Plane.FormationCreator.Views
                                 seekPoint.Y > leftTopPoint.Y && seekPoint.Y < leftTopPoint.Y + rectangle.Height)
                             {
                                 selectedCount++;
-                                _copterManager.Select(taskCopterInfo.Copter);
+                                if (_copterManager.ShowCopter.Count != 0 )
+                                {
+                                    if (_copterManager.ShowCopter.Contains(taskCopterInfo.Copter))
+                                    {
+                                        _copterManager.Select(taskCopterInfo.Copter);
+                                    }
+                                }
+                                else
+                                    _copterManager.Select(taskCopterInfo.Copter);
                             }
                         }
                         _copterManager.shiftkeydown = false;