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 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 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 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; } } }