345 lines
14 KiB
C#
345 lines
14 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-----------------使用中
|
||
//起飞分三个阶段：1阶段分批起飞到15米(目前15米高度是飞控起飞航点决定)，上一批起飞超过5米下一批开始起飞，
|
||
//等待全部起飞完成后执行第二阶段，
|
||
//2阶段等待高度超过9米(可能已到达15米)然后平飞到第一个航点位置,高度为15米的位置，
|
||
//3阶段垂直上升到第一个航点指定高度
|
||
//
|
||
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];
|
||
//执行n架同时起飞
|
||
for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++)
|
||
{
|
||
var info = infos[j];
|
||
|
||
int indexTmp = j - takeOffCount * i;
|
||
if (info.takeOffStage == 0) // 第一阶段：起飞到15m
|
||
{
|
||
tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info);
|
||
}
|
||
else
|
||
{
|
||
tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
||
}
|
||
}
|
||
//等待多架起飞任务-起飞高度超过5米完成
|
||
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) // 第一阶段：起飞到15m
|
||
{
|
||
tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info);
|
||
}
|
||
else
|
||
{
|
||
tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
||
}
|
||
}
|
||
//等待起飞超过5米
|
||
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;
|
||
//}
|
||
//8秒内每1000毫秒尝试解锁一次
|
||
//解锁间隔一定要超过1s否则导致飞控以后无法解锁
|
||
|
||
if (i > 320)
|
||
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)
|
||
//低于5米任务一直等待
|
||
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 () =>
|
||
// {
|
||
// 小于9米时等待----按起飞高度算此时可能已到达15米起飞高度
|
||
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;
|
||
|
||
// 第二阶段：超过9米开始水平飞行
|
||
if (info.takeOffStage == 1)
|
||
{
|
||
//切换到guided模式
|
||
await info.Copter.GuidAsync();
|
||
//设置灯光为航点要求的灯光
|
||
await info.Copter.SetShowLEDAsync(copterNextTask.FlytoShowLED );
|
||
//异步执行飞到第一个航点高度固定为15米的任务
|
||
for (int j = 0; j < 3; j++)
|
||
{
|
||
await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt);
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
//直到到达第一个航点并高度15米
|
||
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;
|
||
//每2秒再异步执行一次到航点高度15米的任务
|
||
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++; //当前变为2
|
||
}
|
||
}
|
||
|
||
dtNow = DateTime.Now;
|
||
dtLastTime = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
// 第三阶段：从第一个航点位置15米垂直飞行
|
||
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;
|
||
//每2秒发一次第一个航点飞行任务
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
// });
|
||
}
|
||
|
||
}
|
||
}
|