From 84659afdabc2f50965accff7945289d00c476fa3 Mon Sep 17 00:00:00 2001 From: pxzleo Date: Wed, 15 Mar 2017 23:17:14 +0800 Subject: [PATCH] =?UTF-8?q?=E8=87=AA=E5=8A=A8=E7=94=9F=E6=88=90=E8=88=AA?= =?UTF-8?q?=E7=82=B9=E5=92=8C=E9=A3=9E=E6=9C=BA=E6=8C=89=E6=96=B9=E9=98=B5?= =?UTF-8?q?=E7=94=9F=E6=88=90=20=E9=99=90=E5=88=B6=E5=8D=AB=E6=98=9F?= =?UTF-8?q?=E5=9C=B0=E5=9B=BE=E6=94=BE=E5=A4=A7=E6=9C=80=E5=A4=A7=E4=B8=BA?= =?UTF-8?q?19=E7=BA=A7=20=E9=A3=9E=E8=A1=8C=E8=88=AA=E7=BA=BF=E5=92=8C?= =?UTF-8?q?=E5=AE=9E=E9=99=85=E8=88=AA=E7=BA=BF=E9=BB=98=E8=AE=A4=E4=B8=8D?= =?UTF-8?q?=E6=98=BE=E7=A4=BA=EF=BC=8C=E5=A2=9E=E5=8A=A0=E6=98=BE=E7=A4=BA?= =?UTF-8?q?=E9=80=89=E9=A1=B9=20=E5=8F=AF=E5=AF=BC=E5=85=A5=E6=9F=90?= =?UTF-8?q?=E4=BA=9B=E6=AD=A5=E9=AA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Formation/FlightTaskManager.cs | 82 ++++++++++++++++++- .../Formation/FlightTask_FlyTo.cs | 4 +- Plane.FormationCreator/MainWindow.xaml | 4 +- Plane.FormationCreator/MainWindow.xaml.cs | 13 ++- .../ViewModels/CopterListViewModel.cs | 61 +++++++++++++- .../ViewModels/ModifyTaskViewModel.cs | 38 ++++++++- .../Views/CopterListView.xaml | 2 +- Plane.FormationCreator/Views/MapView.xaml | 38 +++++++-- Plane.FormationCreator/Views/MapView.xaml.cs | 40 ++++++++- .../Views/MapView_CopterDrawing.cs | 31 ++++++- .../Views/ModifyTaskView.xaml | 54 ++++++++---- Plane.FormationCreator/Views/TaskBarView.xaml | 4 + 12 files changed, 331 insertions(+), 40 deletions(-) diff --git a/Plane.FormationCreator/Formation/FlightTaskManager.cs b/Plane.FormationCreator/Formation/FlightTaskManager.cs index 81196cd..cfcef97 100644 --- a/Plane.FormationCreator/Formation/FlightTaskManager.cs +++ b/Plane.FormationCreator/Formation/FlightTaskManager.cs @@ -201,15 +201,46 @@ namespace Plane.FormationCreator.Formation if (nullableCenter == null) return; var center = nullableCenter.Value; var newTask = new FlightTask(FlightTaskType.FlyTo); + int coptindex = 0; + + int colnum = 5; //自动生成列数=4 + float coldis = 5;//列相距5米 + float rowdis = 5;//行相距5米 + float matrixdis = 20; //生成方阵距离30米 + + + int currcol = 0; //当前列号 + int currrow = 0; //当前行 + Tuple colLatLng = new Tuple(0, 0); + Tuple targetLatLng = new Tuple(0, 0); + FlightTaskSingleCopterInfo lastSingleCopterInfo =null; foreach (var copter in copters) { - var lastSingleCopterInfo = lastTask.SingleCopterInfos.Find(info => info.Copter == copter); - var direction = GeographyUtils.CalcDirection2D(center.Lat, center.Lng, lastSingleCopterInfo.TargetLat, lastSingleCopterInfo.TargetLng); - var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(lastSingleCopterInfo.TargetLat, lastSingleCopterInfo.TargetLng, (float)GeographyUtils.RadToDeg(direction), 5); - var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(copter, targetLatLng.Item1, targetLatLng.Item2, lastSingleCopterInfo.TargetAlt,true); + if (coptindex == 0) + { + lastSingleCopterInfo = lastTask.SingleCopterInfos.Find(info => info.Copter == copter); + targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(lastSingleCopterInfo.TargetLat, lastSingleCopterInfo.TargetLng, 0, matrixdis); + colLatLng = targetLatLng; + } + else + { + if (currcol < colnum) + targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Item1, colLatLng.Item2, 90, currcol* coldis); + else + { + currrow++; + currcol = 0; + targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Item1, colLatLng.Item2, 180, rowdis); + colLatLng = targetLatLng; + } + } + currcol++; + coptindex++; + var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(copter, targetLatLng.Item1, targetLatLng.Item2, lastSingleCopterInfo.TargetAlt, true); newSingleCopterInfo.TargetHeading = lastSingleCopterInfo.TargetHeading; newSingleCopterInfo.CenterDirectionDeg = lastSingleCopterInfo.TargetHeading; newTask.SingleCopterInfos.Add(newSingleCopterInfo); + } Tasks.Add(newTask); try @@ -633,6 +664,49 @@ namespace Plane.FormationCreator.Formation return JsonConvert.SerializeObject(tasks); } + + public void ImportTasksindex(string tasksText,int startindex,int endindex) + { + dynamic tasks = JsonConvert.DeserializeObject(tasksText); + var copters = _copterManager.Copters; + int i=1; + foreach (var task in tasks) + { + if ((i >= startindex)&& (i <= endindex)) + { + switch ((FlightTaskType)task.type) + { + case FlightTaskType.TakeOff: + // AddTakeOffTask(copters); // added by ZJF + // TakeOffNumAttr = task.takeoffnumber; + break; + case FlightTaskType.FlyTo: + RestoreFlyToTask((bool)task.staggerRoutes, task.singleCopterInfos); + break; + case FlightTaskType.Turn: + RestoreTurnTask(task.singleCopterInfos); + break; + case FlightTaskType.Circle: + RestoreCircleTask(task.singleCopterInfos); + break; + case FlightTaskType.Loiter: + RestoreLoiterTimeTask((float)task.loiterTimeAttr, (bool)task.flashCheck, (float)task.flashCheckPeriod, + (bool)task.oneByOneCheck, (float)task.oneByOneCheckPeriod, (string)task.flashNameArray, + (string)task.flashIndexArray, (bool)task.ChangeYaw, (float)task.HeadYaw, task.singleCopterInfos); + + break; + case FlightTaskType.SimpleCircle: + RestoreSimpleCircleTask(task.singleCopterInfos); + break; + case FlightTaskType.ReturnToLand: + // RestoreReturnToLandTask(copters, (int)task.retnumber, (int)task.rtlalt); + break; + } + } + i++; + } + } + public void ImportTasks(string tasksText) { dynamic tasks = JsonConvert.DeserializeObject(tasksText); diff --git a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs index bb63084..e339bab 100644 --- a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs +++ b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs @@ -525,8 +525,7 @@ namespace Plane.FormationCreator.Formation tasks[i] = await Task.Factory.StartNew(async () => { var internalInfo = info; - await info.Copter.SetShowLEDAsync(info.FlytoShowLED); - //if (i1 > 0) + //if (i1 > 0) //{ // var prevCopter = infos[i1 - 1].Copter; // while (CheckCrossing(infos, i1) && @@ -645,6 +644,7 @@ namespace Plane.FormationCreator.Formation if ((bool)_copterManager.CopterStatus[copterIndex]) return; + await info.Copter.SetShowLEDAsync(info.FlytoShowLED); DateTime dtNow = DateTime.Now; DateTime dtLastTime = DateTime.Now; TimeSpan ts = dtNow - dtLastTime; diff --git a/Plane.FormationCreator/MainWindow.xaml b/Plane.FormationCreator/MainWindow.xaml index f62652a..5a9cd6e 100644 --- a/Plane.FormationCreator/MainWindow.xaml +++ b/Plane.FormationCreator/MainWindow.xaml @@ -70,8 +70,8 @@ - - + + colheadLatLng = new Tuple(0, 0); + Tuple targetLatLng = new Tuple(0, 0); + + LatLng? colLatLng = new LatLng(center.Lat, center.Lng); ; + for (int i = 0; i < addcount; ++i, ++_virtualCopterId) { id = _virtualCopterId.ToString(); - _lastVirtualCopterLocation = + + + + if (i == 0) + { + _lastVirtualCopterLocation = new LatLng(center.Lat, center.Lng); + + colLatLng = _lastVirtualCopterLocation; + + + + } + else + { + if (currcol < colnum) + targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Value.Lat, colLatLng.Value.Lng , 90, currcol * coldis); + else + { + currrow++; + currcol = 0; + targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Value.Lat, colLatLng.Value.Lng, 180, rowdis); + colLatLng = new LatLng(targetLatLng.Item1, targetLatLng.Item2); + + colheadLatLng = targetLatLng; + } + _lastVirtualCopterLocation= new LatLng(targetLatLng.Item1, targetLatLng.Item2); + } + currcol++; + + + + + /* _lastVirtualCopterLocation = _lastVirtualCopterLocation == null ? new LatLng(center.Lat, center.Lng) : new LatLng(_lastVirtualCopterLocation.Value.Lat, _lastVirtualCopterLocation.Value.Lng + 0.0001); + */ + + + + + + + + + + + + + + var copter = new FakeCopter(SynchronizationContext.Current); copter.SetProperties( diff --git a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs index 5b11ca7..0966dde 100644 --- a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs @@ -149,6 +149,24 @@ namespace Plane.FormationCreator.ViewModels get { return _Distancevalue; } set { Set(nameof(Distancevalue), ref _Distancevalue, value); } } + + private int _txtStarindex=0; + public int txtStarindex + { + get { return _txtStarindex; } + set { Set(nameof(txtStarindex), ref _txtStarindex, value); } + } + + private int _txtendindex=0; + public int txtendindex + { + get { return _txtendindex; } + set { Set(nameof(txtendindex), ref _txtendindex, value); } + } + + + + private double _FlyToLat; public double FlyToLat { @@ -212,8 +230,23 @@ namespace Plane.FormationCreator.ViewModels }; if (dialog.ShowDialog() == true) { + int _startindex = txtStarindex; + int _endindex= txtendindex; + var tasksText = File.ReadAllText(dialog.FileName); - _flightTaskManager.ImportTasks(tasksText); + if ((txtStarindex == 0) && (txtendindex == 0)) + _flightTaskManager.ImportTasks(tasksText); + else + { + _endindex = txtendindex; + if (_startindex == 0) + _startindex = 1; + if (_endindex == 0) + _endindex = _startindex; + _flightTaskManager.ImportTasksindex(tasksText, _startindex, _endindex); + } + + } })); } @@ -221,6 +254,7 @@ namespace Plane.FormationCreator.ViewModels + private ICommand _LevelAverageCommand; public ICommand LevelAverageCommand { @@ -355,7 +389,7 @@ namespace Plane.FormationCreator.ViewModels if (copterisselect) { - _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = minlat + avgl * coptnum; + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = maxlat - avgl * coptnum; coptnum++; } diff --git a/Plane.FormationCreator/Views/CopterListView.xaml b/Plane.FormationCreator/Views/CopterListView.xaml index d2c3b8e..0faa079 100644 --- a/Plane.FormationCreator/Views/CopterListView.xaml +++ b/Plane.FormationCreator/Views/CopterListView.xaml @@ -57,7 +57,7 @@