group/Plane.FormationCreator
1
0
Fork 0
Code Issues 3 Pull Requests Actions Packages Projects Releases Wiki Activity

[Extensions.cs]改成碰撞检测在RTK之间是小的

Browse Source 
[FlightTask_FlyTo.cs]在飞往过程中支持跳过
[FlightTask_LoiterTime.cs]悬停任务支持暂停和跳过，之前支持不好
[FlightTask_ReturnToLand.cs]降落支持跳过
[FlightTask_TakeOff.cs]起飞支持跳过
[MapView_CopterDrawing.cs]标记为跳过的飞机变成红色文字
This commit is contained in:
pxzleo 2017-09-21 11:35:35 +08:00
parent 5f762aa876
commit 125bafc8e7
6 changed files with 170 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Plane.FormationCreator/Formation/Extensions.cs
View File

@ -83,7 +83,7 @@ namespace Plane.FormationCreator.Formation
public static bool IsTooCloseTo(this ICopter copter, ICopter copter2) 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米 return copter.DistanceTo(copter2) < RTKClosedDis; //最近距离0.5米
else else
return copter.DistanceTo(copter2) < GpsCloseDis; //最近距离2米 return copter.DistanceTo(copter2) < GpsCloseDis; //最近距离2米

12
Plane.FormationCreator/Formation/FlightTask_FlyTo.cs
View File

@ -643,9 +643,12 @@ namespace Plane.FormationCreator.Formation
int taskIndex = _flightTaskManager.CurrentRunningTaskIndex; int taskIndex = _flightTaskManager.CurrentRunningTaskIndex;
int copterIndex = SingleCopterInfos.IndexOf(info); int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，跳过飞行任务 // 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex]) if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return; return;
}
await info.Copter.SetShowLEDAsync(info.FlytoShowLED); await info.Copter.SetShowLEDAsync(info.FlytoShowLED);
@ -721,7 +724,12 @@ namespace Plane.FormationCreator.Formation
} }
dtLastTime = dtNow; dtLastTime = dtNow;
} }
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
} }
// await info.Copter.HoverAsync(); // await info.Copter.HoverAsync();
if (taskIndex == _flightTaskManager.Tasks.Count() - 1) if (taskIndex == _flightTaskManager.Tasks.Count() - 1)

87
Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs
View File

@ -232,7 +232,26 @@ namespace Plane.FormationCreator.Formation
// LED灯全亮 // LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true))); 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灯全亮 // LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true))); 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显示效果只等待 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); // await Task.Delay(200).ConfigureAwait(false);
int copterIndex = SingleCopterInfos.IndexOf(info); int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，跳过飞行任务 // 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex]) if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return; return;
}
@ -381,6 +440,12 @@ namespace Plane.FormationCreator.Formation
return; return;
} }
await Task.Delay(25).ConfigureAwait(false); await Task.Delay(25).ConfigureAwait(false);
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
} }
} }
catch (TimeoutException ex) catch (TimeoutException ex)
@ -395,10 +460,12 @@ namespace Plane.FormationCreator.Formation
var copter = info.Copter; var copter = info.Copter;
int copterIndex = SingleCopterInfos.IndexOf(info); int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，跳过飞行任务 // 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex]) if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return; return;
}
var tasks = new Task[1]; var tasks = new Task[1];
tasks[0] = Task.Run(async () => tasks[0] = Task.Run(async () =>
{ {
@ -452,10 +519,12 @@ namespace Plane.FormationCreator.Formation
var copter = info.Copter; var copter = info.Copter;
int copterIndex = SingleCopterInfos.IndexOf(info); int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，跳过飞行任务 // 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex]) if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return; return;
}
// float gpsLed = await c.GetParamAsync("NOTI_GPSLED"); // float gpsLed = await c.GetParamAsync("NOTI_GPSLED");
/* /*

39
Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs
View File

@ -310,11 +310,13 @@ namespace Plane.FormationCreator.Formation
int copterIndex = SingleCopterInfos.IndexOf(info); int copterIndex = SingleCopterInfos.IndexOf(info);
var copter = info.Copter; var copter = info.Copter;
var copterPreviousTask = _flightTaskManager.Tasks[TaskCount-2].SingleCopterInfos[copterIndex]; // 倒数第二步的目标位置 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 dtNow = DateTime.Now;
DateTime dtLastTime = DateTime.Now; DateTime dtLastTime = DateTime.Now;
TimeSpan ts = dtNow - dtLastTime; TimeSpan ts = dtNow - dtLastTime;
@ -345,6 +347,16 @@ namespace Plane.FormationCreator.Formation
} }
dtLastTime = dtNow; dtLastTime = dtNow;
} }
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
} }
info.RTLStage++; info.RTLStage++;
@ -382,6 +394,12 @@ namespace Plane.FormationCreator.Formation
} }
dtLastTime = dtNow; dtLastTime = dtNow;
} }
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
} }
info.RTLStage++; info.RTLStage++;
@ -395,10 +413,13 @@ namespace Plane.FormationCreator.Formation
// await Task.Run(async () => // await Task.Run(async () =>
// { // {
int copterIndex = SingleCopterInfos.IndexOf(info); int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，跳过飞行任务 // 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex]) if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return; return;
}
DateTime dtNow = DateTime.Now; DateTime dtNow = DateTime.Now;
DateTime dtLastTime = DateTime.Now; DateTime dtLastTime = DateTime.Now;
@ -434,6 +455,12 @@ namespace Plane.FormationCreator.Formation
} }
dtLastTime = dtNow; dtLastTime = dtNow;
} }
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
} }
info.RTLStage++; info.RTLStage++;

24
Plane.FormationCreator/Formation/FlightTask_TakeOff.cs
View File

@ -231,10 +231,12 @@ namespace Plane.FormationCreator.Formation
int copterIndex = SingleCopterInfos.IndexOf(info); int copterIndex = SingleCopterInfos.IndexOf(info);
var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex]; var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex];
// 当该飞机被标记时，跳过飞行任务 // 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex]) if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return; return;
// await Task.Run(async () => } // await Task.Run(async () =>
// { // {
// 小于9米时等待----按起飞高度算此时可能已到达15米起飞高度 // 小于9米时等待----按起飞高度算此时可能已到达15米起飞高度
if (info.takeOffStage == 0) if (info.takeOffStage == 0)
@ -247,6 +249,12 @@ namespace Plane.FormationCreator.Formation
return; return;
} }
await Task.Delay(100).ConfigureAwait(false); await Task.Delay(100).ConfigureAwait(false);
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
} }
if (_flightTaskManager.IsPaused == false) if (_flightTaskManager.IsPaused == false)
{ {
@ -293,6 +301,12 @@ namespace Plane.FormationCreator.Formation
} }
dtLastTime = dtNow; dtLastTime = dtNow;
} }
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
} }
if (_flightTaskManager.IsPaused == false) if (_flightTaskManager.IsPaused == false)
{ {
@ -334,6 +348,12 @@ namespace Plane.FormationCreator.Formation
} }
dtLastTime = dtNow; dtLastTime = dtNow;
} }
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
} }
} }

28
Plane.FormationCreator/Views/MapView_CopterDrawing.cs
View File

@ -73,6 +73,9 @@ namespace Plane.FormationCreator.Views
public ICopter Copter { get; set; } public ICopter Copter { get; set; }
public Grid DotContainer { get; set; } public Grid DotContainer { get; set; }
public Polygon Dot { get; set; } public Polygon Dot { get; set; }
public TextBlock CopterText { get; set; }
public MapPolyline Track { get; set; } public MapPolyline Track { get; set; }
public Location LastLocation { get; set; } public Location LastLocation { get; set; }
@ -121,14 +124,22 @@ namespace Plane.FormationCreator.Views
DotContainer = new Grid { Tag = COPTER_TAG }; DotContainer = new Grid { Tag = COPTER_TAG };
DotContainer.Children.Add(Dot); DotContainer.Children.Add(Dot);
DotContainer.Children.Add(new TextBlock
//飞机里面的编号
CopterText = new TextBlock
{ {
Text = Copter.Name, Text = Copter.Name,
Foreground = new SolidColorBrush(Colors.White), Foreground = new SolidColorBrush(Colors.White),
Margin = new Thickness(-COPTER_RADIUS * 2, -COPTER_RADIUS * 1.5, 0, 0), Margin = new Thickness(-COPTER_RADIUS * 2, -COPTER_RADIUS * 1.5, 0, 0),
HorizontalAlignment = HorizontalAlignment.Center, HorizontalAlignment = HorizontalAlignment.Center,
VerticalAlignment = VerticalAlignment.Center VerticalAlignment = VerticalAlignment.Center
}); };
DotContainer.Children.Add(CopterText);
_map.Children.Add(DotContainer); _map.Children.Add(DotContainer);
MapLayer.SetZIndex(DotContainer, 100); MapLayer.SetZIndex(DotContainer, 100);
@ -147,6 +158,19 @@ namespace Plane.FormationCreator.Views
} }
else 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) if (LastLocation != null && location.CalcDistance(LastLocation) < 0.1)
{ {
locationUpdated = false; locationUpdated = false;