284 lines
10 KiB
C#
284 lines
10 KiB
C#
using Plane.Copters;
|
||
using Plane.Geography;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
using Plane.Windows.Messages;
|
||
|
||
namespace Plane.FormationCreator.Formation
|
||
{
|
||
public partial class FlightTask
|
||
{
|
||
private bool _StaggerRoutes = true;
|
||
|
||
//是否有交错
|
||
public bool StaggerRoutes
|
||
{
|
||
get { return _StaggerRoutes; }
|
||
set { Set(nameof(StaggerRoutes), ref _StaggerRoutes, value); }
|
||
}
|
||
|
||
//同一个任务每一架飞机的FlytoTime和LoiterTime保持统一
|
||
private int _FlytoTime = 10;
|
||
public int FlytoTime
|
||
{
|
||
get { return _FlytoTime; }
|
||
set
|
||
{
|
||
if (value > 4095) value = 4095;
|
||
if (value < 0) value = 0;
|
||
Set(nameof(FlytoTime), ref _FlytoTime, value);
|
||
}
|
||
}
|
||
private int _LoiterTime = 1;
|
||
public int LoiterTime
|
||
{
|
||
get { return _LoiterTime; }
|
||
set
|
||
{
|
||
if (value > 4095) value = 4095;
|
||
if (value < 0) value = 0;
|
||
Set(nameof(LoiterTime), ref _LoiterTime, value);
|
||
}
|
||
|
||
}
|
||
|
||
|
||
private bool _VerticalLift = false;
|
||
public bool VerticalLift // 垂直升降标志位，后面需要加入即使拖动地图上的飞机，也不会变化经纬度. added by ZJF
|
||
{
|
||
get { return _VerticalLift; }
|
||
set
|
||
{
|
||
if (value)
|
||
{
|
||
int currentIndex = _flightTaskManager.SelectedTaskIndex;
|
||
var currentCopterInfos = _flightTaskManager.SelectedTask.SingleCopterInfos;
|
||
var previousCopterInfos = _flightTaskManager.Tasks[currentIndex - 1].SingleCopterInfos;
|
||
for (int i = 0; i < currentCopterInfos.Count; i++)
|
||
{
|
||
currentCopterInfos[i].TargetLat = previousCopterInfos[i].TargetLat;
|
||
currentCopterInfos[i].TargetLng = previousCopterInfos[i].TargetLng;
|
||
currentCopterInfos[i].TargetAlt = previousCopterInfos[i].TargetAlt;
|
||
}
|
||
}
|
||
|
||
Set(nameof(VerticalLift), ref _VerticalLift, value);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
public async Task RunFlyToTaskAsync() // 全部飞到指定航点
|
||
{
|
||
//是否有交错
|
||
if (StaggerRoutes)
|
||
{
|
||
var infos = SingleCopterInfos;
|
||
var tasks = new Task[infos.Count];
|
||
for (int i = 0; i < infos.Count; i++)
|
||
{
|
||
var info = infos[i];
|
||
|
||
tasks[i] = await Task.Factory.StartNew(async () =>
|
||
{
|
||
var internalInfo = info;
|
||
//if (i1 > 0)
|
||
//{
|
||
// var prevCopter = infos[i1 - 1].Copter;
|
||
// while (CheckCrossing(infos, i1) &&
|
||
// prevCopter.Altitude - copter.Altitude < 2)
|
||
// {
|
||
// await Task.Delay(25).ConfigureAwait(false);
|
||
// }
|
||
//}
|
||
|
||
await FlyToTaskFlySingleCopterAsync(internalInfo);
|
||
});
|
||
}
|
||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
||
|
||
}
|
||
else
|
||
{
|
||
await Task.WhenAll(
|
||
SingleCopterInfos.Select(info => FlyToTaskFlySingleCopterAsync(info))
|
||
).ConfigureAwait(false);
|
||
}
|
||
}
|
||
|
||
|
||
private int RuningTaskRemaining = 0;
|
||
|
||
private async Task FlyToTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
|
||
{
|
||
DateTime dtNow = DateTime.Now;
|
||
DateTime dtLastTime = DateTime.Now;
|
||
TimeSpan ts = dtNow - dtLastTime;
|
||
|
||
|
||
FlightTask task = _flightTaskManager.CurrentRunningTask;
|
||
int flyToTime = task.FlytoTime * 1000;
|
||
int loiterTime = task.LoiterTime * 1000;
|
||
int taskIndex = _flightTaskManager.CurrentRunningTaskIndex;
|
||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
||
|
||
|
||
// 当该飞机被标记时，悬停并跳过飞行任务
|
||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
|
||
await info.Copter.SetShowLEDAsync(info.FlytoShowLED);
|
||
|
||
if (info.Copter.State != Plane.Copters.CopterState.CommandMode)
|
||
await info.Copter.GuidAsync();
|
||
|
||
|
||
|
||
|
||
/*
|
||
for (int i = 0; i < 5; i++)
|
||
{
|
||
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
*/
|
||
double targetLat = info.TargetLat;
|
||
double targetLng = info.TargetLng;
|
||
if (info.IsLandWaypoint)
|
||
{
|
||
targetLat = info.Copter.TakeOffPoint.Latitude;
|
||
targetLng = info.Copter.TakeOffPoint.Longitude;
|
||
}
|
||
//发送目标航点1次
|
||
|
||
await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt);
|
||
// await Task.Delay(10).ConfigureAwait(false);
|
||
|
||
dtNow = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
|
||
int sendFlyToTimes = 0;
|
||
|
||
//第0个任务为takeoff
|
||
if (taskIndex > 0)
|
||
{
|
||
FlightTask prevTask = _flightTaskManager.Tasks[taskIndex - 1];
|
||
if (prevTask.TaskType == FlightTaskType.FlyTo && prevTask.LoiterTime == 0)
|
||
flyToTime += prevTask.RuningTaskRemaining;
|
||
}
|
||
|
||
//while (!info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, info.TargetAlt)) //按航点飞 ：所有Copter到达目标点开始飞下个航点
|
||
while (ts.TotalMilliseconds < (flyToTime + loiterTime)) //按时间轴飞：当前任务时间到达后自动飞往下个航点
|
||
{
|
||
/*
|
||
//悬停时间等于0为快速航点 到达之后立即出发下个航点 切时间累积
|
||
if (loiterTime == 0 &&
|
||
info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, info.TargetAlt))
|
||
{
|
||
task.RuningTaskRemaining = flyToTime - (int)ts.TotalMilliseconds;
|
||
break;
|
||
}
|
||
*/
|
||
if (_flightTaskManager.IsPaused == true)
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
await Task.Delay(100).ConfigureAwait(false); //判断是否到达位置10hz
|
||
|
||
|
||
|
||
// if (info.LEDInfos.Count > 0)
|
||
// {
|
||
// string LEDRGB = "";
|
||
// List<LEDInfo> LedControl = info.LEDInfos.OrderBy(i=>i.Delay).ToList();
|
||
// for (int i = 0; i < LedControl.Count; i++)
|
||
// {
|
||
// var led = LedControl[i];
|
||
// if (ts.TotalMilliseconds >= led.Delay * 1000)
|
||
// LEDRGB = info.LEDInfos[i].LEDRGB;
|
||
// else
|
||
// break;
|
||
// }
|
||
// info.Copter.LEDColor = LEDRGB;
|
||
//
|
||
// }
|
||
|
||
if (info.LEDInfos.Count > 0)
|
||
{
|
||
string LEDRGB = "";
|
||
List<LEDInfo> LedControl = info.LEDInfos.ToList();
|
||
double time = 0;
|
||
for (int i = 0; i < LedControl.Count; i++)
|
||
{
|
||
|
||
var led = LedControl[i];
|
||
time += led.Delay * 1000;
|
||
if (ts.TotalMilliseconds >= time)
|
||
LEDRGB = info.LEDInfos[i].LEDRGB;
|
||
else
|
||
break;
|
||
}
|
||
info.Copter.LEDColor = LEDRGB;
|
||
|
||
}
|
||
|
||
|
||
/*
|
||
if (ts.TotalMilliseconds / 10 > sendFlyToTimes) // 每500ms发送一遍指点坐标
|
||
{
|
||
sendFlyToTimes++;
|
||
for (int i = 0; i < 2; i++)
|
||
{
|
||
await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt);
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
|
||
//dtLastTime = dtNow;
|
||
}
|
||
*/
|
||
// 当该飞机被标记时，悬停并跳过飞行任务
|
||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
return;
|
||
}
|
||
dtNow = DateTime.Now;
|
||
ts = dtNow - dtLastTime;
|
||
}
|
||
// await info.Copter.HoverAsync();
|
||
if (taskIndex == _flightTaskManager.Tasks.Count() - 1)
|
||
{
|
||
await info.Copter.HoverAsync();
|
||
}
|
||
}
|
||
|
||
|
||
private static bool CheckCrossing(List<FlightTaskSingleCopterInfo> infos, int currentIndex)
|
||
{
|
||
var info = infos[currentIndex];
|
||
for (int i = 0; i < currentIndex; i++)
|
||
{
|
||
var nextInfo = infos[i];
|
||
if (GeographyUtils.CheckCrossing2D(info.Copter.Latitude, info.Copter.Longitude, info.TargetLat, info.TargetLng,
|
||
nextInfo.Copter.Latitude, nextInfo.Copter.Longitude, nextInfo.TargetLat, nextInfo.TargetLng))
|
||
{
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
}
|
||
}
|