From 125bafc8e76f7703a903924c9463663fe888b69d Mon Sep 17 00:00:00 2001 From: pxzleo Date: Thu, 21 Sep 2017 11:35:35 +0800 Subject: [PATCH] =?UTF-8?q?[Extensions.cs]=E6=94=B9=E6=88=90=E7=A2=B0?= =?UTF-8?q?=E6=92=9E=E6=A3=80=E6=B5=8B=E5=9C=A8RTK=E4=B9=8B=E9=97=B4?= =?UTF-8?q?=E6=98=AF=E5=B0=8F=E7=9A=84=20[FlightTask=5FFlyTo.cs]=E5=9C=A8?= =?UTF-8?q?=E9=A3=9E=E5=BE=80=E8=BF=87=E7=A8=8B=E4=B8=AD=E6=94=AF=E6=8C=81?= =?UTF-8?q?=E8=B7=B3=E8=BF=87=20[FlightTask=5FLoiterTime.cs]=E6=82=AC?= =?UTF-8?q?=E5=81=9C=E4=BB=BB=E5=8A=A1=E6=94=AF=E6=8C=81=E6=9A=82=E5=81=9C?= =?UTF-8?q?=E5=92=8C=E8=B7=B3=E8=BF=87=EF=BC=8C=E4=B9=8B=E5=89=8D=E6=94=AF?= =?UTF-8?q?=E6=8C=81=E4=B8=8D=E5=A5=BD=20[FlightTask=5FReturnToLand.cs]?= =?UTF-8?q?=E9=99=8D=E8=90=BD=E6=94=AF=E6=8C=81=E8=B7=B3=E8=BF=87=20[Fligh?= =?UTF-8?q?tTask=5FTakeOff.cs]=E8=B5=B7=E9=A3=9E=E6=94=AF=E6=8C=81?= =?UTF-8?q?=E8=B7=B3=E8=BF=87=20[MapView=5FCopterDrawing.cs]=E6=A0=87?= =?UTF-8?q?=E8=AE=B0=E4=B8=BA=E8=B7=B3=E8=BF=87=E7=9A=84=E9=A3=9E=E6=9C=BA?= =?UTF-8?q?=E5=8F=98=E6=88=90=E7=BA=A2=E8=89=B2=E6=96=87=E5=AD=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Formation/Extensions.cs | 2 +- .../Formation/FlightTask_FlyTo.cs | 12 ++- .../Formation/FlightTask_LoiterTime.cs | 87 +++++++++++++++++-- .../Formation/FlightTask_ReturnToLand.cs | 39 +++++++-- .../Formation/FlightTask_TakeOff.cs | 24 ++++- .../Views/MapView_CopterDrawing.cs | 28 +++++- 6 files changed, 170 insertions(+), 22 deletions(-) diff --git a/Plane.FormationCreator/Formation/Extensions.cs b/Plane.FormationCreator/Formation/Extensions.cs index c64b304..ca820b6 100644 --- a/Plane.FormationCreator/Formation/Extensions.cs +++ b/Plane.FormationCreator/Formation/Extensions.cs @@ -83,7 +83,7 @@ namespace Plane.FormationCreator.Formation public static bool IsTooCloseTo(this ICopter copter, ICopter copter2) { - if (copter.GpsFixType == GpsFixType.RTKFIXED) + if ((copter.GpsFixType == GpsFixType.RTKFIXED)&& (copter2.GpsFixType == GpsFixType.RTKFIXED)) return copter.DistanceTo(copter2) < RTKClosedDis; //最近距离0.5米 else return copter.DistanceTo(copter2) < GpsCloseDis; //最近距离2米 diff --git a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs index d1a3485..f9eafe9 100644 --- a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs +++ b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs @@ -643,9 +643,12 @@ namespace Plane.FormationCreator.Formation int taskIndex = _flightTaskManager.CurrentRunningTaskIndex; int copterIndex = SingleCopterInfos.IndexOf(info); - // 当该飞机被标记时，跳过飞行任务 + // 当该飞机被标记时，悬停并跳过飞行任务 if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); return; + } await info.Copter.SetShowLEDAsync(info.FlytoShowLED); @@ -721,7 +724,12 @@ namespace Plane.FormationCreator.Formation } dtLastTime = dtNow; } - + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } } // await info.Copter.HoverAsync(); if (taskIndex == _flightTaskManager.Tasks.Count() - 1) diff --git a/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs b/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs index f6e5218..5558303 100644 --- a/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs +++ b/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs @@ -232,7 +232,26 @@ namespace Plane.FormationCreator.Formation // LED灯全亮 await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true))); //延时等待 - await Task.Delay((int)(LoiterTimeAttr * 1000)).ConfigureAwait(false); + //判断是否下一步 + DateTime start = DateTime.Now; + + while (true) + { + if (_flightTaskManager.IsPaused == true) + { + await Task.WhenAll(SingleCopterInfos.Select(info => (info.Copter.HoverAsync()))); + return; + } + + DateTime end = DateTime.Now; + TimeSpan ts = end - start; + //等待指定时间 + if (ts.TotalMilliseconds > LoiterTimeAttr * 1000) + break; + await Task.Delay(20).ConfigureAwait(false); + + } + } @@ -286,12 +305,50 @@ namespace Plane.FormationCreator.Formation // LED灯全亮 await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true))); //延时等待 - await Task.Delay((int)(LoiterTimeAttr * 1000)).ConfigureAwait(false); + + //判断是否下一步 + DateTime start = DateTime.Now; + while (true) + { + if (_flightTaskManager.IsPaused == true) + { + await Task.WhenAll(SingleCopterInfos.Select(info => (info.Copter.HoverAsync()))); + return; + } + + DateTime end = DateTime.Now; + TimeSpan ts = end - start; + //等待指定时间 + if (ts.TotalMilliseconds > LoiterTimeAttr * 1000) + break; + await Task.Delay(20).ConfigureAwait(false); + + } + } else // 没有LED显示效果只等待 { - await Task.Delay((int)(LoiterTimeAttr * 1000)).ConfigureAwait(false); + + //判断是否下一步 + DateTime start = DateTime.Now; + + while (true) + { + if (_flightTaskManager.IsPaused == true) + { + await Task.WhenAll(SingleCopterInfos.Select(info => (info.Copter.HoverAsync()))); + return; + } + + DateTime end = DateTime.Now; + TimeSpan ts = end - start; + //等待指定时间 + if (ts.TotalMilliseconds > LoiterTimeAttr * 1000) + break; + await Task.Delay(20).ConfigureAwait(false); + + } } } @@ -356,10 +413,12 @@ namespace Plane.FormationCreator.Formation // await Task.Delay(200).ConfigureAwait(false); int copterIndex = SingleCopterInfos.IndexOf(info); - // 当该飞机被标记时，跳过飞行任务 + // 当该飞机被标记时，悬停并跳过飞行任务 if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); return; - + } @@ -381,6 +440,12 @@ namespace Plane.FormationCreator.Formation return; } await Task.Delay(25).ConfigureAwait(false); + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } } } catch (TimeoutException ex) @@ -395,10 +460,12 @@ namespace Plane.FormationCreator.Formation var copter = info.Copter; int copterIndex = SingleCopterInfos.IndexOf(info); - // 当该飞机被标记时，跳过飞行任务 + // 当该飞机被标记时，悬停并跳过飞行任务 if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); return; - + } var tasks = new Task[1]; tasks[0] = Task.Run(async () => { @@ -452,10 +519,12 @@ namespace Plane.FormationCreator.Formation var copter = info.Copter; int copterIndex = SingleCopterInfos.IndexOf(info); - // 当该飞机被标记时，跳过飞行任务 + // 当该飞机被标记时，悬停并跳过飞行任务 if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); return; - + } // float gpsLed = await c.GetParamAsync("NOTI_GPSLED"); /* diff --git a/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs b/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs index 5b52156..f4c12c3 100644 --- a/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs +++ b/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs @@ -310,11 +310,13 @@ namespace Plane.FormationCreator.Formation int copterIndex = SingleCopterInfos.IndexOf(info); var copter = info.Copter; var copterPreviousTask = _flightTaskManager.Tasks[TaskCount-2].SingleCopterInfos[copterIndex]; // 倒数第二步的目标位置 - - // 当该飞机被标记时，跳过飞行任务 - if ((bool)_copterManager.CopterStatus[copterIndex]) - return; + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } DateTime dtNow = DateTime.Now; DateTime dtLastTime = DateTime.Now; TimeSpan ts = dtNow - dtLastTime; @@ -345,6 +347,16 @@ namespace Plane.FormationCreator.Formation } dtLastTime = dtNow; } + + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } + + + } info.RTLStage++; @@ -382,6 +394,12 @@ namespace Plane.FormationCreator.Formation } dtLastTime = dtNow; } + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } } info.RTLStage++; @@ -395,10 +413,13 @@ namespace Plane.FormationCreator.Formation // await Task.Run(async () => // { int copterIndex = SingleCopterInfos.IndexOf(info); - - // 当该飞机被标记时，跳过飞行任务 + + // 当该飞机被标记时，悬停并跳过飞行任务 if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); return; + } DateTime dtNow = DateTime.Now; DateTime dtLastTime = DateTime.Now; @@ -434,6 +455,12 @@ namespace Plane.FormationCreator.Formation } dtLastTime = dtNow; } + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } } info.RTLStage++; diff --git a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs index 08c1218..4dc19ab 100644 --- a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs +++ b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs @@ -231,10 +231,12 @@ namespace Plane.FormationCreator.Formation int copterIndex = SingleCopterInfos.IndexOf(info); var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex]; - // 当该飞机被标记时，跳过飞行任务 + // 当该飞机被标记时，悬停并跳过飞行任务 if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); return; - // await Task.Run(async () => + } // await Task.Run(async () => // { // 小于9米时等待----按起飞高度算此时可能已到达15米起飞高度 if (info.takeOffStage == 0) @@ -247,6 +249,12 @@ namespace Plane.FormationCreator.Formation return; } await Task.Delay(100).ConfigureAwait(false); + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } } if (_flightTaskManager.IsPaused == false) { @@ -293,6 +301,12 @@ namespace Plane.FormationCreator.Formation } dtLastTime = dtNow; } + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } } if (_flightTaskManager.IsPaused == false) { @@ -334,6 +348,12 @@ namespace Plane.FormationCreator.Formation } dtLastTime = dtNow; } + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } } } diff --git a/Plane.FormationCreator/Views/MapView_CopterDrawing.cs b/Plane.FormationCreator/Views/MapView_CopterDrawing.cs index f14be08..a3875d6 100644 --- a/Plane.FormationCreator/Views/MapView_CopterDrawing.cs +++ b/Plane.FormationCreator/Views/MapView_CopterDrawing.cs @@ -73,6 +73,9 @@ namespace Plane.FormationCreator.Views public ICopter Copter { get; set; } public Grid DotContainer { get; set; } public Polygon Dot { get; set; } + + public TextBlock CopterText { get; set; } + public MapPolyline Track { get; set; } public Location LastLocation { get; set; } @@ -121,14 +124,22 @@ namespace Plane.FormationCreator.Views DotContainer = new Grid { Tag = COPTER_TAG }; DotContainer.Children.Add(Dot); - DotContainer.Children.Add(new TextBlock + + //飞机里面的编号 + + + CopterText = new TextBlock { Text = Copter.Name, Foreground = new SolidColorBrush(Colors.White), Margin = new Thickness(-COPTER_RADIUS * 2, -COPTER_RADIUS * 1.5, 0, 0), HorizontalAlignment = HorizontalAlignment.Center, VerticalAlignment = VerticalAlignment.Center - }); + }; + + + DotContainer.Children.Add(CopterText); + _map.Children.Add(DotContainer); MapLayer.SetZIndex(DotContainer, 100); @@ -147,6 +158,19 @@ namespace Plane.FormationCreator.Views } else { + + + if ((bool)_copterManager.CopterStatus[_copterManager.Copters.IndexOf(Copter)]) + { + CopterText.Foreground = new SolidColorBrush(Colors.Red); + } + else { + + CopterText.Foreground = new SolidColorBrush(Colors.White); + } + + + if (LastLocation != null && location.CalcDistance(LastLocation) < 0.1) { locationUpdated = false;