pxzleo
cd3f894449
2.加入缩放控制，也是基于平面的，不能用于3D情况 3.加入同时起飞和同时降落数量的设置，并保存入飞行任务中 4.跑马灯飞机编号允许人工输入 5.计算距离改为两个选择的飞机航点之间，不用输入飞机序号
314 lines
12 KiB
C#
314 lines
12 KiB
C#
using Plane.Copters;
|
||
using Microsoft.Practices.ServiceLocation;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
using Plane.Geography;
|
||
|
||
namespace Plane.FormationCreator.Formation
|
||
{
|
||
public partial class FlightTask
|
||
{
|
||
|
||
|
||
private int _TakeOffNumAttr = 1;
|
||
public int TakeOffNumAttr
|
||
{
|
||
get { return _TakeOffNumAttr; }
|
||
set
|
||
{
|
||
Set(nameof(TakeOffNumAttr), ref _TakeOffNumAttr, value);
|
||
}
|
||
}
|
||
|
||
public async Task RunTakeOffTaskAsync()
|
||
{
|
||
// float takeOffAlt = 15;
|
||
int TaskCount = _flightTaskManager.Tasks.Count();
|
||
if (TaskCount > 1)
|
||
{
|
||
var infos = SingleCopterInfos;
|
||
// var tasks = new Task[infos.Count];
|
||
var tasksTmp = new Task[infos.Count];
|
||
for (int i = 0; i < infos.Count; i++)
|
||
{
|
||
var info = infos[i];
|
||
|
||
if (info.takeOffStage == 0) // 第一阶段：起飞到10m
|
||
{
|
||
await TakeOffTaskFlySingleCopterAsync(info);
|
||
//if (_flightTaskManager.IsPaused == false)
|
||
//{
|
||
// info.takeOffStage++;
|
||
//}
|
||
}
|
||
|
||
tasksTmp[i] = TakeOffSecondTaskAsync(info); // 第二和第三阶段
|
||
}
|
||
await Task.WhenAll(tasksTmp).ConfigureAwait(false);
|
||
if (_flightTaskManager.IsPaused == false)
|
||
{
|
||
for (int i = 0; i < infos.Count; i++)
|
||
{
|
||
var info = infos[i];
|
||
info.takeOffStage = 0;
|
||
}
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
// 几架飞机同时起飞，参数为takeOffCount
|
||
public async Task MutilRunTakeOffTaskAsync()
|
||
{
|
||
int TaskCount = _flightTaskManager.Tasks.Count();
|
||
if (TaskCount > 1)
|
||
{
|
||
var infos = SingleCopterInfos;
|
||
|
||
int takeOffCount = TakeOffNumAttr;
|
||
int copterCount = infos.Count;
|
||
int integerPart = copterCount / takeOffCount;
|
||
int residualPart = copterCount % takeOffCount;
|
||
|
||
// var tasks = new Task[infos.Count];
|
||
var tasksTmp = new Task[infos.Count];
|
||
for (int i = 0; i < integerPart; i++)
|
||
{
|
||
var tasksTakeOff = new Task[takeOffCount];
|
||
for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++)
|
||
{
|
||
var info = infos[j];
|
||
|
||
int indexTmp = j - takeOffCount * i;
|
||
if (info.takeOffStage == 0) // 第一阶段：起飞到10m
|
||
{
|
||
tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info);
|
||
}
|
||
else
|
||
{
|
||
tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
||
}
|
||
}
|
||
await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
|
||
|
||
for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++)
|
||
{
|
||
var info = infos[j];
|
||
tasksTmp[j] = TakeOffSecondTaskAsync(info); // 第二和第三阶段
|
||
}
|
||
}
|
||
|
||
// 余数架飞机同时起飞
|
||
if (residualPart > 0)
|
||
{
|
||
var tasksTakeOff = new Task[residualPart];
|
||
for (int j = integerPart * takeOffCount; j < takeOffCount * integerPart + residualPart; j++)
|
||
{
|
||
var info = infos[j];
|
||
|
||
int indexTmp = j - takeOffCount * integerPart;
|
||
if (info.takeOffStage == 0) // 第一阶段：起飞到10m
|
||
{
|
||
tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info);
|
||
}
|
||
else
|
||
{
|
||
tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
||
}
|
||
}
|
||
await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
|
||
|
||
for (int j = integerPart * takeOffCount; j < takeOffCount * integerPart + residualPart; j++)
|
||
{
|
||
var info = infos[j];
|
||
tasksTmp[j] = TakeOffSecondTaskAsync(info); // 第二和第三阶段
|
||
}
|
||
}
|
||
await Task.WhenAll(tasksTmp).ConfigureAwait(false);
|
||
if (_flightTaskManager.IsPaused == false)
|
||
{
|
||
for (int i = 0; i < infos.Count; i++)
|
||
{
|
||
var info = infos[i];
|
||
info.takeOffStage = 0;
|
||
}
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
private CopterManager _copterManager = ServiceLocator.Current.GetInstance<CopterManager>();
|
||
|
||
private async Task TakeOffTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
|
||
{
|
||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
||
var copter = info.Copter;
|
||
var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex];
|
||
|
||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
||
return;
|
||
|
||
if (_flightTaskManager.IsPaused == true)
|
||
{
|
||
return;
|
||
}
|
||
await copter.SetShowLEDAsync(true);
|
||
await copter.UnlockAsync();
|
||
for (int i = 0; !copter.IsUnlocked; i++)
|
||
{
|
||
//if (_flightTaskManager.IsPaused == true)
|
||
//{
|
||
// return;
|
||
//}
|
||
if (i % (1000 / 25) == 1000 / 25 - 1)
|
||
{
|
||
await copter.UnlockAsync(); // 每 1000 毫秒重试一次。
|
||
}
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
}
|
||
|
||
// 为了返航，记录家的位置, 应该放在起飞命令
|
||
info.TargetLat = info.Copter.Latitude;
|
||
info.TargetLng = info.Copter.Longitude;
|
||
|
||
for (int i = 0; i < 5; i++) // added by ZJF
|
||
{
|
||
await copter.TakeOffAsync();
|
||
await Task.Delay(50).ConfigureAwait(false);
|
||
}
|
||
|
||
// while (copter.Altitude < 4 || copter.State == Copters.CopterState.TakingOff)
|
||
while (copter.Altitude < 5) // 修改起飞逻辑，当高度达到5米时，下一架开始起飞
|
||
{
|
||
if (_flightTaskManager.IsPaused == true)
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
await Task.Delay(100).ConfigureAwait(false);
|
||
}
|
||
|
||
/* //先不要这个控制看能否正常工作
|
||
if (copter.Altitude > 8)
|
||
{
|
||
await info.Copter.GuidAsync();
|
||
return;
|
||
}
|
||
*/
|
||
}
|
||
|
||
private async Task TakeOffSecondTaskAsync(FlightTaskSingleCopterInfo info)
|
||
{
|
||
float takeOffAlt = 15;
|
||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
||
var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex];
|
||
|
||
// 当该飞机被标记时，跳过飞行任务
|
||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
||
return;
|
||
// await Task.Run(async () =>
|
||
// {
|
||
// 修改起飞逻辑，当高度达到5米时，下一架开始起飞
|
||
if (info.takeOffStage == 0)
|
||
{
|
||
while (info.Copter.Altitude < 9)
|
||
{
|
||
if (_flightTaskManager.IsPaused == true)
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
await Task.Delay(100).ConfigureAwait(false);
|
||
}
|
||
if (_flightTaskManager.IsPaused == false)
|
||
{
|
||
info.takeOffStage++;
|
||
}
|
||
}
|
||
|
||
DateTime dtNow = DateTime.Now;
|
||
DateTime dtLastTime = DateTime.Now;
|
||
TimeSpan ts = dtNow - dtLastTime;
|
||
|
||
// 第二阶段：水平飞行
|
||
if (info.takeOffStage == 1)
|
||
{
|
||
await info.Copter.GuidAsync();
|
||
await info.Copter.SetShowLEDAsync(copterNextTask.FlytoShowLED );
|
||
for (int j = 0; j < 3; j++)
|
||
{
|
||
await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt);
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
|
||
while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt))
|
||
{
|
||
if (_flightTaskManager.IsPaused == true)
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
|
||
dtNow = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
if (ts.TotalMilliseconds > 2000)
|
||
{
|
||
for (int j = 0; j < 2; j++)
|
||
{
|
||
await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt);
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
dtLastTime = dtNow;
|
||
}
|
||
}
|
||
if (_flightTaskManager.IsPaused == false)
|
||
{
|
||
info.takeOffStage++;
|
||
}
|
||
}
|
||
|
||
dtNow = DateTime.Now;
|
||
dtLastTime = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
// 第三阶段：垂直飞行
|
||
if (info.takeOffStage == 2)
|
||
{
|
||
for (int j = 0; j < 3; j++)
|
||
{
|
||
await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt);
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
|
||
while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt))
|
||
{
|
||
if (_flightTaskManager.IsPaused == true)
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
|
||
dtNow = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
if (ts.TotalMilliseconds > 2000)
|
||
{
|
||
for (int j = 0; j < 2; j++)
|
||
{
|
||
await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt);
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
dtLastTime = dtNow;
|
||
}
|
||
}
|
||
}
|
||
|
||
// });
|
||
}
|
||
|
||
}
|
||
}
|