477 lines
18 KiB
C#
477 lines
18 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 byte _TakeOffTime = 10;
|
||
public byte TakeOffTime
|
||
{
|
||
get { return _TakeOffTime; }
|
||
set { Set(nameof(TakeOffTime), ref _TakeOffTime, 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;
|
||
}
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
//新版的起飞方案
|
||
public async Task NewMutilRunTakeOffTaskAsync()
|
||
{
|
||
var infos = SingleCopterInfos;
|
||
var tasksTakeOff = new Task[infos.Count];
|
||
for (int i = 0; i < infos.Count; i++)
|
||
{
|
||
//tasksTakeOff[i] = NewSingleRunTaskOffTaskAsunc(infos[i]);
|
||
|
||
|
||
tasksTakeOff[i] = await Task.Factory.StartNew(async () =>
|
||
{
|
||
var internalInfo = infos[i];
|
||
|
||
await NewSingleRunTaskOffTaskAsunc(internalInfo);
|
||
});
|
||
}
|
||
await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
|
||
//await Task.Delay(100);
|
||
}
|
||
|
||
private async Task NewSingleRunTaskOffTaskAsunc(FlightTaskSingleCopterInfo info)
|
||
{
|
||
DateTime dtNow = DateTime.Now;
|
||
DateTime dtLastTime = DateTime.Now;
|
||
TimeSpan ts = dtNow - dtLastTime;
|
||
|
||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
||
var copter = info.Copter;
|
||
await copter.UnlockAsync();
|
||
//等待起飞时间
|
||
while ((int)ts.TotalMilliseconds < (int)info.TakeOffWaitTime * 1000)
|
||
{
|
||
if (_flightTaskManager.IsPaused == true)
|
||
{
|
||
await copter.UnlockAsync();
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
await Task.Delay(100);
|
||
dtNow = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
}
|
||
|
||
//虚拟飞机5秒后不起飞会自动上锁
|
||
await copter.UnlockAsync();
|
||
|
||
for (int i = 0; i < 5; i++) // added by ZJF
|
||
{
|
||
await copter.TakeOffAsync();
|
||
await Task.Delay(50).ConfigureAwait(false);
|
||
}
|
||
|
||
var copterNextTask = _flightTaskManager.Tasks[TaskIndex + 1].SingleCopterInfos[copterIndex];
|
||
float takeOffAlt = copterNextTask.TargetAlt;
|
||
|
||
info.TargetLat = info.Copter.Latitude;
|
||
info.TargetLng = info.Copter.Longitude;
|
||
|
||
// for (int j = 0; j < 3; j++)
|
||
// {
|
||
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt);
|
||
// await Task.Delay(10).ConfigureAwait(false);
|
||
// }
|
||
//解锁起飞用暗紫色
|
||
info.Copter.LEDColor = "FF00FF";
|
||
|
||
dtNow = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
FlightTask task = _flightTaskManager.CurrentRunningTask;
|
||
|
||
while (ts.TotalMilliseconds < task.TakeOffTime * 1000)
|
||
{
|
||
if (_flightTaskManager.IsPaused == true)
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
await Task.Delay(100).ConfigureAwait(false);
|
||
dtNow = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
}
|
||
//起飞完成用暗蓝色
|
||
info.Copter.LEDColor = "0000FF";
|
||
|
||
}
|
||
|
||
|
||
//老方案 ----- 按起飞数量起飞
|
||
// 几架飞机同时起飞，参数为takeOffCount-----------------使用中
|
||
//起飞分三个阶段：
|
||
//1阶段：分批起飞到15米(目前15米高度是飞控起飞航点决定)，上一批起飞超过5米下一批开始起飞，
|
||
// 等待全部起飞完成后执行第二阶段，
|
||
//2阶段：等待高度超过9米(可能已到达15米)然后平飞到第一个航点位置,高度为15米的位置，
|
||
//3阶段：垂直上升到第一个航点指定高度
|
||
//
|
||
//修改方案：针对高度
|
||
//1.上升到起飞高度(目前没有地面站设置高度，高度在飞控中)
|
||
//2.直接飞往第一个航点的高度
|
||
public async Task MutilRunTakeOffTaskAsync()
|
||
{
|
||
|
||
int TaskCount = _flightTaskManager.Tasks.Count();
|
||
if (TaskCount > 1)
|
||
{
|
||
var infos = SingleCopterInfos;
|
||
|
||
//不再使用起飞数量 强制设置起飞总数等于所有飞机
|
||
int takeOffCount = _copterManager.Copters.Count();
|
||
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)
|
||
{
|
||
DateTime dtNow = DateTime.Now;
|
||
DateTime dtLastTime = DateTime.Now;
|
||
TimeSpan ts = dtNow - dtLastTime;
|
||
|
||
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;
|
||
|
||
//等待起飞时间
|
||
while (ts.TotalMilliseconds < info.TakeOffWaitTime * 1000 )
|
||
{
|
||
await Task.Delay(100);
|
||
dtNow = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
}
|
||
|
||
//开始起飞
|
||
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)
|
||
{
|
||
|
||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
||
var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex];
|
||
float takeOffAlt = copterNextTask.TargetAlt;// 15; 起飞高度到下个任务的高度
|
||
// 当该飞机被标记时，悬停并跳过飞行任务
|
||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
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 ((bool)_copterManager.CopterStatus[copterIndex])
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
}
|
||
if (_flightTaskManager.IsPaused == false)
|
||
{
|
||
info.takeOffStage++;
|
||
}
|
||
}
|
||
|
||
DateTime dtNow = DateTime.Now;
|
||
DateTime dtLastTime = DateTime.Now;
|
||
TimeSpan ts = dtNow - dtLastTime;
|
||
|
||
#region 屏蔽掉第二个阶段直接进入第三个阶 到达起飞高度直接飞往第一个航点
|
||
|
||
// 第二阶段：超过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(info.TargetLat, info.TargetLng, takeOffAlt);
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
//直到到达第一个航点并高度15米
|
||
while (!info.Copter.ArrivedTarget(info.TargetLat, info.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(info.TargetLat, info.TargetLng, takeOffAlt);
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
dtLastTime = dtNow;
|
||
}
|
||
// 当该飞机被标记时，悬停并跳过飞行任务
|
||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
}
|
||
if (_flightTaskManager.IsPaused == false)
|
||
{
|
||
info.takeOffStage++; //当前变为2
|
||
}
|
||
}
|
||
|
||
dtNow = DateTime.Now;
|
||
dtLastTime = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
|
||
#endregion
|
||
|
||
info.takeOffStage++;
|
||
|
||
// 第三阶段：从第一个航点位置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;
|
||
}
|
||
// 当该飞机被标记时，悬停并跳过飞行任务
|
||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
// });
|
||
}
|
||
|
||
}
|
||
}
|