Plane.FormationCreator/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs
pxzleo 125bafc8e7 [Extensions.cs]改成碰撞检测在RTK之间是小的
[FlightTask_FlyTo.cs]在飞往过程中支持跳过
[FlightTask_LoiterTime.cs]悬停任务支持暂停和跳过,之前支持不好
[FlightTask_ReturnToLand.cs]降落支持跳过
[FlightTask_TakeOff.cs]起飞支持跳过
[MapView_CopterDrawing.cs]标记为跳过的飞机变成红色文字
2017-09-21 11:35:35 +08:00

724 lines
25 KiB
C#
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTask
{
private float _LoiterTimeAttr = 0.0f;
public float LoiterTimeAttr
{
get { return _LoiterTimeAttr; }
set
{
Set(nameof(LoiterTimeAttr), ref _LoiterTimeAttr, value);
}
}
// 改变机头
private bool _ChangeYaw = false;
public bool ChangeYaw
{
get { return _ChangeYaw; }
set
{
Set(nameof(ChangeYaw), ref _ChangeYaw, value);
}
}
// 机头方向0为北顺时针360
private float _HeadYaw = 1.0f;
public float HeadYaw
{
get { return _HeadYaw; }
set
{
Set(nameof(HeadYaw), ref _HeadYaw, value);
}
}
// LED灯闪烁
private bool _flashAttr = false;
public bool flashAttr
{
get { return _flashAttr; }
set
{
Set(nameof(flashAttr), ref _flashAttr, value);
}
}
// LED闪烁次数
private float _flashPeriodAttr = 1.0f;
public float flashPeriodAttr
{
get { return _flashPeriodAttr; }
set
{
Set(nameof(flashPeriodAttr), ref _flashPeriodAttr, value);
}
}
private string Name2Index(string name) // 获取指定copter名字对应的序号
{
int index = -1;
for (int i = 0; i < SingleCopterInfos.Count; i++)
{
if (name.Equals(SingleCopterInfos[i].Copter.Name))
{
return i.ToString();
}
}
return index.ToString();
}
// 闪烁的飞机名字编号
private string _flashCopterNameArray = "";
public string flashCopterNameArray
{
get { return _flashCopterNameArray; }
set
{
Set(nameof(flashCopterNameArray), ref _flashCopterNameArray, value);
flashCopterIndexArray = "";
string[] copterArray = flashCopterNameArray.Split(',');
for (int i = 0; i < copterArray.Length; i++)
{
flashCopterIndexArray += Name2Index(copterArray[i])+',';
}
}
}
// 闪烁的飞机序号编号
private string _flashCopterIndexArray = "";
public string flashCopterIndexArray
{
get { return _flashCopterIndexArray; }
set
{
Set(nameof(flashCopterIndexArray), ref _flashCopterIndexArray, value);
}
}
// LED走马灯
private bool _oneByOneAttr = false;
public bool oneByOneAttr
{
get { return _oneByOneAttr; }
set
{
Set(nameof(oneByOneAttr), ref _oneByOneAttr, value);
}
}
// LED走马灯次数
private float _oneByOnePeriodAttr = 1.0f;
public float oneByOnePeriodAttr
{
get { return _oneByOnePeriodAttr; }
set
{
Set(nameof(oneByOnePeriodAttr), ref _oneByOnePeriodAttr, value);
}
}
private int[] columnFirst = {0, 1, 2, 3, 4 };
private int[] columnSecond = { 5, 6, 7, 8, 9 };
private int[] columnThird = { 10, 11, 12, 13, 14 };
private int[] columnFourth = { 15, 16, 17, 18, 19 };
private int[] columnFive = { 20, 21, 22, 23, 24 };
private int[] columnSix = { 25, 26, 27, 28, 29 };
private int[] rowFirst = { 0, 5, 10, 15, 20, 25 };
private int[] rowSecond = { 1, 6, 11, 16, 21, 26 };
private int[] rowThird = { 2, 7, 12, 17, 22, 27 };
private int[] rowFourth = { 3, 8, 13, 18, 23, 28 };
private int[] rowFive = { 4, 9, 14, 19, 24, 29 };
private int[] rowFirstForEight = { 0, 1, 2, 3 };
private int[] rowSecondForEight = { 4, 5, 6, 7 };
private int[] doubleColumnFirst = { 0, 1, 2, 3, 4, 25, 26, 27, 28, 29 };
private int[] doubleColumnSecond = { 5, 6, 7, 8, 9, 20, 21, 22, 23, 24 };
private int[] doubleColumnThird = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19 };
private int[] circleFirst = { 0, 1, 2, 3, 4, 9, 14, 19, 24, 29, 28, 27, 26, 25, 20, 15, 10, 5 };
private int[] circleSecond = { 6, 7, 8, 13, 18, 23, 22, 21, 16, 11 };
private int[] circleThird = { 12, 17 };
private int[] lineFirst = { 4 };
private int[] lineSecond = { 3, 9 };
private int[] lineThird = { 2, 8, 14 };
private int[] lineFourth = { 1, 7, 13, 19 };
private int[] lineFive = { 0, 6, 12, 18, 24 };
private int[] lineSix = { 5, 11, 17, 23, 29 };
private int[] lineSeven = { 10, 16, 22, 28 };
private int[] lineEight = { 15, 21, 27 };
private int[] lineNine = { 20, 26 };
private int[] lineTen = { 25 };
public async Task RunLoiterTimeTaskAsync()
{
if (ChangeYaw)
{
await Task.WhenAll(SingleCopterInfos.Select(info => SetCopterYawAsync(info,HeadYaw)));
}
// 判断flashPeriodAttr, oneByOnePeriodAttr的值执行不同的闪烁模式
// 当flashPeriodAttr小于10正常闪烁即没有预制闪烁模式
// 当flashPeriodAttr大于等于10小于20执行预制闪烁模式
// 当flashPeriodAttr大于等于20 改变飞控闪烁模式以oneByOnePeriodAttr作为周期值
//LEDFlashPlanAsync需要30架飞机
if ((flashPeriodAttr >= 10.0f) && (flashPeriodAttr < 20.0f))
{
await LEDFlashPlanAsync();
return;
}
//需要固件支持
if (flashPeriodAttr >= 20.0f)
{
await LEDFlashParaModifyPlanAsync();
return;
}
var infos = SingleCopterInfos;
if (flashAttr) // LED闪烁显示效果
{
if (flashCopterIndexArray.Equals(""))
return;
string[] copterArray = flashCopterIndexArray.Split(',');
var tasks_selected = new Task[copterArray.Length];
// LED灯全灭
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(1000).ConfigureAwait(false);
// 选中的飞机LED灯全亮
for (int i = 0; i < copterArray.Length; i++)
{
int index = int.Parse(copterArray[i]);
var info = infos[index];
tasks_selected[i] = await Task.Factory.StartNew(async () =>
{
var internalInfo = info;
await LEDFlashTaskFlySingleCopterAsync(internalInfo, true);
});
}
await Task.WhenAll(tasks_selected).ConfigureAwait(false);
await Task.Delay(3000).ConfigureAwait(false);
// LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
//延时等待
//判断是否下一步
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 if (oneByOneAttr) // LED走马灯显示效果
{
if (flashCopterNameArray.Equals(""))
return;
string[] copterArray = flashCopterIndexArray.Split(',');
//LED全灭
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(1000).ConfigureAwait(false);
// LED灯一个接一个全亮
for (int i = 0; i < copterArray.Length; i++)
{
if (copterArray[i] == "") break;
int index = int.Parse(copterArray[i]);
var info = infos[index];
await LEDFlashTaskFlySingleCopterAsync(info, true);
await Task.Delay(200).ConfigureAwait(false);
}
await Task.Delay(1000).ConfigureAwait(false);
//////////////////全部闪烁一次
// LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
await Task.Delay(200).ConfigureAwait(false);
// LED灯全灭
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(200).ConfigureAwait(false);
/////////////////////////////////////
//////////////////全部闪烁一次
// LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
await Task.Delay(200).ConfigureAwait(false);
// LED灯全灭
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(200).ConfigureAwait(false);
/////////////////////////////////////
//////////////////全部闪烁一次
// LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
await Task.Delay(200).ConfigureAwait(false);
// LED灯全灭
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(200).ConfigureAwait(false);
/////////////////////////////////////
// LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
//延时等待
//判断是否下一步
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显示效果只等待
{
//判断是否下一步
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);
}
}
}
private async Task LEDFlashParaModifyPlanAsync()
{
/*
await LEDFlashParaModifyAsync(rowFirstForEight);
await Task.Delay(500).ConfigureAwait(false);
await LEDFlashParaModifyAsync(rowSecondForEight);
await Task.Delay(500).ConfigureAwait(false);
*/
int copterCount = SingleCopterInfos.Count();
int[] intArray = new int[copterCount];
for (int ii=0; ii<copterCount; ii++)
{
intArray[ii] = ii;
}
await LEDFlashParaModifyAsync(intArray);
/*
await LEDFlashParaModifyAsync(rowFirst);
await Task.Delay(500).ConfigureAwait(false);
await LEDFlashParaModifyAsync(rowSecond);
await Task.Delay(500).ConfigureAwait(false);
await LEDFlashParaModifyAsync(rowThird);
await Task.Delay(500).ConfigureAwait(false);
await LEDFlashParaModifyAsync(rowFourth);
await Task.Delay(500).ConfigureAwait(false);
await LEDFlashParaModifyAsync(rowFive);
await Task.Delay(500).ConfigureAwait(false);
*/
}
private async Task LEDFlashParaModifyAsync(int[] LEDArray)
{
var infos = SingleCopterInfos;
var tasks_selected = new Task[LEDArray.Length];
for (int i = 0; i < LEDArray.Length; i++)
{
var info = infos[LEDArray[i]];
tasks_selected[i] = await Task.Factory.StartNew(async () =>
{
var internalInfo = info;
await LEDParaModifySingleCopterAsync(internalInfo, oneByOnePeriodAttr);
});
}
await Task.WhenAll(tasks_selected).ConfigureAwait(false);
}
private async Task SetCopterYawAsync(FlightTaskSingleCopterInfo info, float headyaw)
{
var copter = info.Copter;
// await Task.Delay(200).ConfigureAwait(false);
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 () =>
{
try
{
await copter.SetMobileControlAsync(yaw: headyaw);
while (!copter.ArrivedHeading((short)headyaw))
{
if (_flightTaskManager.IsPaused == true)
{
await info.Copter.HoverAsync();
return;
}
await Task.Delay(25).ConfigureAwait(false);
// 当该飞机被标记时,悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
}
}
catch (TimeoutException ex)
{
_logger.Log($"SetYAWtimeout, {ex.Message}, CopterId: {copter.Id}。");
}
});
await Task.WhenAll(tasks).ConfigureAwait(false);
}
private async Task LEDParaModifySingleCopterAsync(FlightTaskSingleCopterInfo info, float count)
{
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 () =>
{
try
{
if (count == 1.0)
await copter.SetShowLEDAsync(true);
else
await copter.SetShowLEDAsync(false);
//await copter.SetParamAsync("NTF_SHOWLED", count, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"SetShowLEDAsync 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
/*
var tasks = new Task[2];
tasks[0] = Task.Run(async () =>
{
try
{
await copter.SetParamAsync("NOTI_GPSLED", count, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"NOTI_GPSLED 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
tasks[1] = Task.Run(async () =>
{
try
{
await copter.SetParamAsync("NOTI_ARMLED", count, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"NOTI_ARMLED 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
*/
await Task.WhenAll(tasks).ConfigureAwait(false);
}
/**
* isOn为true打开灯isOn为false关闭灯
*/
private async Task LEDFlashTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info, bool isOn)
{
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");
/*
float ledControl = 0.0f; //关灯
if (isOn)
{
ledControl = 1.0f; //长亮
}
*/
var tasks = new Task[1];
tasks[0] = Task.Run(async () =>
{
try
{
await copter.SetShowLEDAsync(isOn);
// await copter.SetParamAsync("NTF_SHOWLED", ledControl, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"SetShowLED 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
/*
var tasks = new Task[2];
tasks[0] = Task.Run(async () =>
{
try
{
await copter.SetParamAsync("NOTI_GPSLED", ledControl, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"NOTI_GPSLED 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
tasks[1] = Task.Run(async () =>
{
try
{
await copter.SetParamAsync("NOTI_ARMLED", ledControl, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"NOTI_ARMLED 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
*/
await Task.WhenAll(tasks).ConfigureAwait(false);
}
private async Task LEDFlashPlanAsync()
{
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(500).ConfigureAwait(false);
await LEDColumnFlashAsync();
await Task.Delay(500).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(500).ConfigureAwait(false);
await LEDDoubleColumnFlashAsync();
await Task.Delay(500).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(500).ConfigureAwait(false);
await LEDLineFlashAsync();
await Task.Delay(500).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(500).ConfigureAwait(false);
await LEDCircleFlashAsync();
await Task.Delay(500).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(200).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
await Task.Delay(200).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(200).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
await Task.Delay(200).ConfigureAwait(false);
}
// 一排一排闪烁 需要30架飞机6排一排5架一排排闪灯
private async Task LEDColumnFlashAsync()
{
await LEDArrayFlashAsync(columnFirst);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(columnSecond);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(columnThird);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(columnFourth);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(columnFive);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(columnSix);
await Task.Delay(200).ConfigureAwait(false);
}
// 以圆圈收缩形式显示
private async Task LEDCircleFlashAsync()
{
await LEDArrayFlashAsync(circleFirst);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(circleSecond);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(circleThird);
await Task.Delay(200).ConfigureAwait(false);
}
// 两列同时闪烁
private async Task LEDDoubleColumnFlashAsync()
{
await LEDArrayFlashAsync(doubleColumnFirst);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(doubleColumnSecond);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(doubleColumnThird);
await Task.Delay(200).ConfigureAwait(false);
}
// 斜线方式闪烁
private async Task LEDLineFlashAsync()
{
await LEDArrayFlashAsync(lineFirst);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineSecond);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineThird);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineFourth);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineFive);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineSix);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineSeven);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineEight);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineNine);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineTen);
await Task.Delay(200).ConfigureAwait(false);
}
private async Task LEDArrayFlashAsync(int[] LEDArray)
{
var infos = SingleCopterInfos;
var tasks_selected = new Task[LEDArray.Length];
for (int i = 0; i < LEDArray.Length; i++)
{
var info = infos[LEDArray[i]];
tasks_selected[i] = await Task.Factory.StartNew(async () =>
{
var internalInfo = info;
await LEDFlashTaskFlySingleCopterAsync(internalInfo, true);
});
}
await Task.WhenAll(tasks_selected).ConfigureAwait(false);
}
}
}