group/Plane.FormationCreator
1
0
Fork 0
Code Issues 3 Pull Requests Actions Packages Projects Releases Wiki Activity

紧急返航功能，策略不对，临时保存

Browse Source
This commit is contained in:
pxzleo 2024-01-17 22:37:25 +08:00
parent 2614dc2f59
commit 36875bcdc2
7 changed files with 3376 additions and 3140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Plane.FormationCreator/Formation/FlightTask.cs
View File

@ -163,7 +163,11 @@ namespace Plane.FormationCreator.Formation
get { return _ModifyingSingleCopterInfo; }
set { Set(nameof(ModifyingSingleCopterInfo), ref _ModifyingSingleCopterInfo, value); }
}
/// <summary>
/// 实际执行某一个任务--包含等待
/// </summary>
/// <returns></returns>
/// <exception cref="InvalidOperationException"></exception>
public async Task RunAsync()
{
switch (TaskType)
@ -172,8 +176,6 @@ namespace Plane.FormationCreator.Formation
await RunFlyToTaskAsync().ConfigureAwait(false);
break;
case FlightTaskType.TakeOff:
//多架同时起飞
//await MutilRunTakeOffTaskAsync().ConfigureAwait(false);
await NewMutilRunTakeOffTaskAsync().ConfigureAwait(false);
break;
case FlightTaskType.Land:

274
Plane.FormationCreator/Formation/FlightTaskManager.cs
View File

@ -22,6 +22,10 @@ using Plane.CommunicationManagement;
using Microsoft.Practices.ServiceLocation;
using Plane.FormationCreator.ViewModels;
using MahApps.Metro.Controls;
using Newtonsoft.Json.Linq;
using GMap.NET.MapProviders;
using GMap.NET;
using static Plane.FormationCreator.CalculationLogLatDistance;
namespace Plane.FormationCreator.Formation
{
@ -648,7 +652,21 @@ namespace Plane.FormationCreator.Formation
// SelectedTaskIndex = 0;
}
//模拟紧急返航
public async Task sim_DoMissionEmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance,int rettime, bool descending)
{
var copters = _copterManager.Copters;
if (TaskState == TasksStatus.Stop)
return;
IsEmergencyRet = true;
//设置所有模拟飞机的位置
for (int j = 0; j < copters.Count; j++)
{
var copter = copters[j];
var fc = copter as FakeCopter;
await fc.EmergencyRetAsync(takeoffcentloc, taskcentloc, mindistance, rettime, descending);
}
}
public async Task ForceNextTasks()
{
@ -2199,6 +2217,15 @@ namespace Plane.FormationCreator.Formation
}
return ret;
}
CancellationTokenSource cts ;
//绕行取消，用于3D绕行计算
public void cancel_ABypassBAsync()
{
if (cts!=null)
cts.Cancel(); //取消掉 异步执行的 绕行函数
}
//新开线程异步调用ABypassB并等待返回
public async Task<(List<List<FlightRouteV2.Vector3>>,bool)> ABypassBAsync(FlightRouteV2.Vector3[] aVecs, FlightRouteV2.Vector3[] bVecs)
{
@ -2207,7 +2234,8 @@ namespace Plane.FormationCreator.Formation
var task = Task.Run(() =>
{
ret = FlyVecFun.ABypassB(aVecs, bVecs, Routecallback, Cronograma, out isPasstmp);
cts = new CancellationTokenSource();
ret = FlyVecFun.ABypassB(aVecs, bVecs, Routecallback, Cronograma, cts.Token, out isPasstmp);
});
try
{
@ -2852,8 +2880,24 @@ namespace Plane.FormationCreator.Formation
Message.Show($"经纬方式计算用时：{stopWatch.Elapsed.TotalMilliseconds}ms,总飞行距离{sumlength}米,有{crosscount}个交叉");
}
//计算中心点 (三维的)
//计算一个图案的重心点 (三维的)
private PLLocation CenterGravityLatLng(List<PLLocation> points)
{
double aLatitude = 0;
double aLongitude = 0;
double aAlt = 0;
foreach (var item in points)
{
aLatitude += item.Latitude;
aLongitude += item.Longitude;
aAlt += item.Altitude;
}
aLatitude = aLatitude / points.Count;
aLongitude = aLongitude / points.Count;
aAlt = aAlt / points.Count;
return new PLLocation(aLatitude, aLongitude, (float)aAlt);
}
//计算两个图案的共同几何中心点 (三维的)
private PLLocation CenterLatLng(List<PLLocation> point1, List<PLLocation> point2)
{
PLLocation centerLatLng = new PLLocation(0,0,0);
@ -3195,8 +3239,30 @@ namespace Plane.FormationCreator.Formation
}
}
private bool _IsEmergencyRet=false;
public bool IsEmergencyRet
{
get { return _IsEmergencyRet; }
private set
{
if (Set(nameof(IsEmergencyRet), ref _IsEmergencyRet, value))
{
if (_IsEmergencyRet)
{
MessageText = "任务紧急返航中！";
}
else
{
MessageText = "任务运行中";
}
}
}
}
TimeSpan timeSpan;
DateTime taskStartTime;
public DateTime taskStartTime;
public async Task RunTaskAsync()
{
if (Tasks.Count == 0) return;
@ -3249,10 +3315,14 @@ namespace Plane.FormationCreator.Formation
}
}
/// <summary>
/// 开始运行模拟任务
/// </summary>
/// <returns></returns>
public async Task RunAsync()
{
IsPaused = false;
IsEmergencyRet = false;
for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++)
{
var task = Tasks[i];
@ -3261,6 +3331,10 @@ namespace Plane.FormationCreator.Formation
}
AppEx.Current.AppMode = AppMode.RunningTasks;
StartAvoidingCrash(); //开始碰撞检测
//告诉所有飞机开始任务--模拟飞机不计算起飞延迟，直接传0
foreach (var copter in _copterManager.Copters)
await copter.MissionStartAsync(0, 0, 0, 0, 0);
TaskState = TasksStatus.Running;
for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++)
{
@ -3269,16 +3343,13 @@ namespace Plane.FormationCreator.Formation
task.Status = FlightTaskStatus.Running;
CurrentRunningTask = task;
CurrentRunningTaskIndex = i;
//////////////////显示提示信息
//显示提示信息
int starttime = 0;
for (int j = 0; j < task.TaskIndex; j++)
starttime += GetTaskTime(j);
TimeSpan ts = new TimeSpan(0, 0, Convert.ToInt32(starttime));
Message.Show($"{ts}:任务{i+1} {task.TaskCnName } 开始执行，需{ GetTaskTime(task.TaskIndex)}秒");
/////////////////////////
//开始执行任务
await task.RunAsync().ConfigureAwait(false);
// 1. 被暂停时，中断 RunAsync。继续运行时将把此时运行了一半的 CurrentRunningTask 重新运行一遍。
if (IsPaused == true)
@ -3288,10 +3359,17 @@ namespace Plane.FormationCreator.Formation
Message.Show($"任务{i + 1} {task.TaskCnName } 暂停执行");
return;
}
//紧急返航中-跳出任务循环
if (IsEmergencyRet == true) break;
task.Status = FlightTaskStatus.Stop;
}
// 2. 正常结束时，重置 CurrentRunningTask、CurrentRunningTaskIndex 和 IsPaused。
//紧急返航中-等待返航结束
while (IsEmergencyRet) {
//延时10ms等待
await Task.Delay(10);
}
// 2. 正常结束时，重置 CurrentRunningTask、CurrentRunningTaskIndex 和 IsPaused。
TaskState = TasksStatus.Stop;
CurrentRunningTask = null;
CurrentRunningTaskIndex = 0;
@ -3306,11 +3384,179 @@ namespace Plane.FormationCreator.Formation
#endregion Run and pause.
//计算飞行一段时间后的位置
private PLLocation TaskFlyLoc(PLLocation startloc, PLLocation tarloc,int currflytime,int flytime )
{
PLLocation flyLatLng;
if (currflytime >= flytime)
{
flyLatLng=new PLLocation(tarloc.Latitude, tarloc.Longitude, tarloc.Altitude);
return flyLatLng;
}
double _direction = startloc.CalcDirection2D(tarloc.Latitude, tarloc.Longitude);
double _Lng_delta = (float)(Math.Sin(_direction) * GeographyUtils.CalcMetersToLngSpan(tarloc.Latitude));
double _Lat_delta = (float)(Math.Cos(_direction) * GeographyUtils.METERS_TO_LAT_SPAN);
//计算xy和z方向距离
float _distance_xy = (float)startloc.CalcDistance2D(tarloc);
float _distance_z = tarloc.Altitude - startloc.Altitude;
float currdis_xy= _distance_xy/ flytime* currflytime;
float currdis_z = _distance_z / flytime * currflytime;
// 更新位置
float Altitude = startloc.Altitude + currdis_z;
double Longitude = startloc.Longitude + currdis_xy * _Lng_delta;
double Latitude = startloc.Latitude + currdis_xy * _Lat_delta;
flyLatLng = new PLLocation(Latitude, Longitude, Altitude);
return flyLatLng;
}
//计算紧急返航数据
//rettime :返航时间点--从起飞开始计算的秒数
//takeoffcentloc:起飞图案重心点
//taskcentloc:返航图案重心点
//mindistance:离起飞重心点最近飞机距离
//overlapping:两个图案是否有重叠--用来决定飞机是先降低高度还是先横着飞
//返回值：是否可以计算返航--起飞不能用这个函数，直接降落
public bool EmergencyRet(int rettime, out PLLocation takeoffcentloc, out PLLocation taskcentloc, out double mindistance, out bool overlapping)
{
mindistance = 0;
int starttime = 0;
int flyedtasktime = 0;
int rettaskindex = -1;
int taskflytime = 0;
taskcentloc = null;
overlapping = false;
double takeoffmaxlat=0;
double takeoffmaxlng = 0;
double takeoffminlat = double.MaxValue;
double takeoffminlng = double.MaxValue;
double taskmaxlat = 0;
double taskmaxlng = 0;
double taskminlat = double.MaxValue;
double taskminlng = double.MaxValue;
Dictionary<int, PLLocation> curTaskPoint = new Dictionary<int, PLLocation>();
//计算地面起飞矩阵的中心点
for (int i = 0; i < _copterManager.Copters.Count; i++)
{
//起飞任务
var curinfo = Tasks[0].SingleCopterInfos[i];
PLLocation curLoc = new PLLocation(curinfo.TargetLat, curinfo.TargetLng,0);
if (curinfo.TargetLat > takeoffmaxlat)
takeoffmaxlat = curinfo.TargetLat;
if (curinfo.TargetLng > takeoffmaxlng)
takeoffmaxlng = curinfo.TargetLng;
if (curinfo.TargetLat < takeoffminlat)
takeoffminlat = curinfo.TargetLat;
if (curinfo.TargetLng < takeoffminlng)
takeoffminlng = curinfo.TargetLng;
curTaskPoint.Add(i, curLoc);
}
takeoffcentloc = CenterGravityLatLng(curTaskPoint.Values.ToList());
curTaskPoint.Clear();
//计算返航时间正在飞行哪个航点，飞了多久
for (int i = 0; i < Tasks.Count; i++)
{
flyedtasktime= GetTaskTime(i);
if ((starttime+ flyedtasktime) > rettime)
{
rettaskindex = i;
taskflytime = rettime - starttime;
}
starttime += flyedtasktime;
}
//起飞不能用这个函数，直接降落
if (rettaskindex<1) return false;
mindistance = double.MaxValue;
//计算悬停图案的重心点
for (int i = 0; i < _copterManager.Copters.Count; i++)
{
var curinfo = Tasks[rettaskindex].SingleCopterInfos[i];
var prvcurinfo = Tasks[rettaskindex - 1].SingleCopterInfos[i];
PLLocation curLoc;
if (taskflytime < Tasks[rettaskindex].FlytoTime)
{
PLLocation coptercurLoc = new PLLocation(curinfo.TargetLat, curinfo.TargetLng, curinfo.TargetAlt);
PLLocation copterprvLoc = new PLLocation(prvcurinfo.TargetLat, prvcurinfo.TargetLng, prvcurinfo.TargetAlt);
//计算飞行一段时间后的位置
curLoc = TaskFlyLoc(copterprvLoc, coptercurLoc, taskflytime, Tasks[rettaskindex].FlytoTime);
}
else //正在悬停
curLoc = new PLLocation(curinfo.TargetLat, curinfo.TargetLng, 0);
curTaskPoint.Add(i, curLoc);
if (curinfo.TargetLat > taskmaxlat)
taskmaxlat = curinfo.TargetLat;
if (curinfo.TargetLng > taskmaxlng)
taskmaxlng = curinfo.TargetLng;
if (curinfo.TargetLat < taskminlat)
taskminlat = curinfo.TargetLat;
if (curinfo.TargetLng < taskminlng)
taskminlng = curinfo.TargetLng;
double taskdistance = takeoffcentloc.CalcDistance(curLoc);
if (taskdistance < mindistance)
mindistance = taskdistance;
}
taskcentloc = CenterGravityLatLng(curTaskPoint.Values.ToList());
curTaskPoint.Clear();
//判断两个图案是否有重叠
overlapping = DoRectanglesOverlap(takeoffminlat, takeoffminlng, takeoffmaxlat, takeoffmaxlng,
taskminlat, taskminlng, taskmaxlat, taskmaxlng);
return true;
}
/// <summary>
/// 判断两个矩形是否有重叠
/// </summary>
/// <param name="minLat1"></param>
/// <param name="minLng1"></param>
/// <param name="maxLat1"></param>
/// <param name="maxLng1"></param>
/// <param name="minLat2"></param>
/// <param name="minLng2"></param>
/// <param name="maxLat2"></param>
/// <param name="maxLng2"></param>
/// <returns></returns>
static bool DoRectanglesOverlap(double minLat1, double minLng1, double maxLat1, double maxLng1,
double minLat2, double minLng2, double maxLat2, double maxLng2)
{
// 判断第一个矩形在第二个矩形的左边或右边
if (minLng1 > maxLng2 || minLng2 > maxLng1)
{
return false;
}
// 判断第一个矩形在第二个矩形的上边或下边
if (minLat1 > maxLat2 || minLat2 > maxLat1)
{
return false;
}
return true;
}
Dictionary<int, string> AvoidCrashLog = new Dictionary<int, string >();
private async void StartAvoidingCrash()
{

24
Plane.FormationCreator/Formation/FlightTaskStatus.cs
View File

@ -1,13 +1,13 @@
public enum FlightTaskStatus
{
Stop,
Running,
Paused,
Selected
}
public enum TasksStatus
{
Stop,
Running,
Paused
public enum FlightTaskStatus
{
Stop,
Running,
Paused,
Selected
}
public enum TasksStatus
{
Stop,
Running,
Paused
}

474
Plane.FormationCreator/Formation/FlightTask_FlyTo.cs
View File

@ -1,262 +1,212 @@
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();
double targetLat = info.TargetLat;
double targetLng = info.TargetLng;
if (info.IsLandWaypoint)
{
targetLat = info.Copter.TakeOffPoint.Latitude;
targetLng = info.Copter.TakeOffPoint.Longitude;
}
int flytype = _flightTaskManager.getflytype(taskIndex);
await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt, task.FlytoTime, flytype);
// 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 ((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;
}
}
}
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;
await FlyToTaskFlySingleCopterAsync(internalInfo);
});
}
await Task.WhenAll(tasks).ConfigureAwait(false);
}
else
{
await Task.WhenAll(
SingleCopterInfos.Select(info => FlyToTaskFlySingleCopterAsync(info))
).ConfigureAwait(false);
}
}
// private int RuningTaskRemaining = 0;
/// <summary>
/// 单独飞机执行飞行任务
/// </summary>
/// <param name="info"></param>
/// <returns></returns>
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();
double targetLat = info.TargetLat;
double targetLng = info.TargetLng;
if (info.IsLandWaypoint)
{
targetLat = info.Copter.TakeOffPoint.Latitude;
targetLng = info.Copter.TakeOffPoint.Longitude;
}
int flytype = _flightTaskManager.getflytype(taskIndex);
//指定目标位置
await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt, task.FlytoTime, flytype);
dtNow = DateTime.Now;
ts = dtNow - dtLastTime;
//等待时间到达，并执行灯光模拟--不移动飞机，移动飞机是飞机自己计算的
while (ts.TotalMilliseconds < (flyToTime + loiterTime)) //按时间轴飞：当前任务时间到达后自动飞往下个航点
{
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.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 ((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;
}
}
}

973
Plane.FormationCreator/Formation/FlightTask_TakeOff.cs
View File

@ -1,487 +1,486 @@
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();
await copter.TakeOffAsync();
var copterNextTask = _flightTaskManager.Tasks[TaskIndex + 1].SingleCopterInfos[copterIndex];
float takeOffAlt = copterNextTask.TargetAlt;
info.TargetLat = info.Copter.Latitude;
info.TargetLng = info.Copter.Longitude;
//info.Copter.takeOffTargetAltitude = takeOffAlt;
FlightTask task = _flightTaskManager.CurrentRunningTask;
float takeflytime = task.TakeOffTime - info.TakeOffWaitTime;
//开始往上飞
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt, takeflytime, 1); //秒
//解锁起飞用暗紫色
info.Copter.LEDColor = CopterManager.CopterTakeoffColor;
dtNow = DateTime.Now;
ts = dtNow - dtLastTime;
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;
//灯光控制
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;
///灯光控制结束
}
}
//起飞完成用默认颜色
info.Copter.LEDColor = CopterManager.CopterFlyingColor;
}
//老方案 ----- 按起飞数量起飞
// 几架飞机同时起飞，参数为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;
}
}
}
// });
}
}
}
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] = await Task.Factory.StartNew(async () =>
{
var internalInfo = infos[i];
await NewSingleRunTaskOffTaskAsunc(internalInfo);
});
}
await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
}
/// <summary>
/// 单独飞机 执行起飞任务
/// </summary>
/// <param name="info"></param>
/// <returns></returns>
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();
await copter.TakeOffAsync();
var copterNextTask = _flightTaskManager.Tasks[TaskIndex + 1].SingleCopterInfos[copterIndex];
float takeOffAlt = copterNextTask.TargetAlt;
info.TargetLat = info.Copter.Latitude;
info.TargetLng = info.Copter.Longitude;
//info.Copter.takeOffTargetAltitude = takeOffAlt;
FlightTask task = _flightTaskManager.CurrentRunningTask;
float takeflytime = task.TakeOffTime - info.TakeOffWaitTime;
//飞行目标，开始往上飞
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt, takeflytime, 1); //秒
//解锁起飞用暗紫色
info.Copter.LEDColor = CopterManager.CopterTakeoffColor;
dtNow = DateTime.Now;
ts = dtNow - dtLastTime;
//等待起飞时间完成,并模拟灯光--不移动飞机-飞机自己计算并移动位置
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;
//灯光控制
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;
///灯光控制结束
}
}
//起飞完成用默认颜色
info.Copter.LEDColor = CopterManager.CopterFlyingColor;
}
//老方案 ----- 按起飞数量起飞
// 几架飞机同时起飞，参数为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;
}
}
}
// });
}
}
}

4290
Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs
View File

File diff suppressed because it is too large Load Diff

473
Plane.FormationCreator/Views/ControlPanelView.xaml
View File

@ -1,235 +1,238 @@
<UserControl x:Class="Plane.FormationCreator.Views.ControlPanelView"
xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
xmlns:d="http://schemas.microsoft.com/expression/blend/2008"
xmlns:local="clr-namespace:Plane.FormationCreator.Views"
xmlns:fc="clr-namespace:Plane.FormationCreator"
xmlns:ec="clr-namespace:Plane.Copters;assembly=PlaneGcsSdk_Private_NET46"
xmlns:vm="clr-namespace:Plane.FormationCreator.ViewModels"
xmlns:m="clr-namespace:Plane.FormationCreator.Formation"
mc:Ignorable="d"
d:DesignHeight="300"
d:DesignWidth="300">
<StackPanel Margin="0,0,-8,0">
<StackPanel.Resources>
<Style TargetType="Button"
BasedOn="{StaticResource {x:Type Button}}">
<Setter Property="Margin"
Value="0,0,3,5" />
</Style>
</StackPanel.Resources>
<!--<TextBlock Text="连接" />
<WrapPanel>
<Button Content="连接"
Command="{Binding ConnectCommand}" />
<Button Content="断开"
Command="{Binding DisconnectCommand}" />
</WrapPanel>-->
<TabControl Margin="0,0,13,0">
<TabItem Header="飞行前准备">
<StackPanel Orientation="Vertical" Margin="0,5,0,0">
<WrapPanel>
<Button Content="电压检测"
Command="{Binding DetectionVoltage}" />
<Button Content="版本检测"
Command="{Binding GetVersionsCommand}" />
<Button Content="通讯版本" Command="{Binding DetectionCommModuleVersion}" />
<Button Content="统计返回"
Command="{Binding DetectionReturnData}" />
<Button Content="通讯统计"
Command="{Binding GetCommsumCommand}" />
<TextBlock
Margin="5,5,5,5" Foreground ="Red" VerticalAlignment="Center"
Text="{Binding RTKState}"
/>
</WrapPanel>
<WrapPanel>
<Button Content="参数设置"
Command="{Binding ParamModify}" />
<Button Content="读入参数"
Command="{Binding LoadParamfile}" />
<Button Content="状态统计" Command="{Binding ReportGPSTypeCommand}"/>
<Button Content="飞机校准"
Command="{Binding CalibrationSingleCommand}" />
<Button Content="正式参数" Command="{Binding TurnOffTestLightsCommand}" Visibility="Collapsed" />
<Button Content="电机" Visibility="Collapsed"
Command="{Binding MotorTestCommand}" />
<Button Content="删除飞机"
Command="{Binding DelCommand}" />
<Label Visibility="Collapsed" Content="Lat"/>
<TextBox Visibility="Collapsed" Text="{Binding LatOffset}" Width="50"/>
<Label Visibility="Collapsed" Content="Lng"/>
<TextBox Visibility="Collapsed" Text="{Binding LngOffset}" Width="50"/>
<Button Visibility="Collapsed" Content="设置返航点" Command="{Binding RLTOffsetCommand}" />
</WrapPanel>
<WrapPanel>
<Button Content="写入航点"
Command="{Binding WriteMissionCommand}" />
<Button Content="航点续写"
Command="{Binding WriteMissionFailedCommand}" />
<Button Content="选写航点" Command="{Binding WriteMissionSingleCommand}" />
<Button Content="统计航点"
Command="{Binding DetectionMissionData}" />
<TextBox
Grid.Column="1"
Width="55"
Margin="5, 5, 5, 5"
HorizontalContentAlignment="Right"
Text="{Binding RTKcomvalue, UpdateSourceTrigger=PropertyChanged}" Visibility="Collapsed"
/>
<Button Content="{Binding RTKbtntxt}"
Command="{Binding SendRTKCommand}" Visibility="Collapsed" />
<Button Content="{Binding NTRIPbtntxt}"
Command="{Binding SendRTCMCommand}" Visibility="Collapsed"/>
<Button Content="RTK 控制" Command="{Binding OpenRtcmManageCommand}" />
</WrapPanel>
</StackPanel>
</TabItem>
<TabItem Header="飞行控制">
<StackPanel Orientation="Vertical" Margin="0,5,0,0">
<WrapPanel >
<Button Content="解锁"
Command="{Binding UnlockCommand}" />
<Button Content="加锁"
Command="{Binding LockCommand}" />
<Button Content="单独任务"
Command="{Binding TakeOffCommand}" />
<Button Content="抛物"
Command="{Binding ThrowoutCommand}" />
</WrapPanel>
<WrapPanel>
<Button Content="起飞"
Command="{Binding GuidAsyncCommand}" />
<Button Content="悬停"
Command="{Binding HoverCommand}" />
<Button Content="手动"
Command="{Binding FloatCommand}" />
<Button Content="返航"
Command="{Binding ReturnToLaunchCommand}" />
<Button Content="降落"
Command="{Binding LandCommand}" />
<Button Content="跳过" Visibility="Collapsed"
Command="{Binding FlagCommand}" />
<TextBox Width="50"
Visibility="Collapsed"
Text="{Binding AltP, UpdateSourceTrigger=PropertyChanged}" />
</WrapPanel>
<WrapPanel>
<Button Content="闪灯"
Command="{Binding LEDFlickerCommand}" />
<Button Content="测试" Visibility="Collapsed"
Command="{Binding TestCommand}" />
<Button Content="开灯"
Command="{Binding LEDOnOffCommand}"
CommandParameter="0"/>
<Button Content="关灯"
Command="{Binding LEDOnOffCommand}"
CommandParameter="1"/>
<TextBlock Margin="8,-3,5,0" Text="起飞高度" VerticalAlignment="Center"/>
<TextBox VerticalContentAlignment="Center" Margin="0,0,0,5" Width="30" Height="25" Text="{Binding TaskOffAlt}" />
<TextBlock Margin="5,-3,5,0" Text="米" VerticalAlignment="Center"/>
</WrapPanel>
</StackPanel>
</TabItem>
<TabItem Header="任务控制">
<StackPanel Orientation="Vertical" Margin="0,5,0,0">
<WrapPanel>
<Button Content="全部解锁"
Command="{Binding UnlockAllCommand}"
IsEnabled="{Binding AllowMissionStart, UpdateSourceTrigger=PropertyChanged}" />
<Button Content="全部加锁"
Command="{Binding LockAllCommand}" />
</WrapPanel>
<WrapPanel>
<Button Content="开始任务"
Command="{Binding MissionStartCommand}"
IsEnabled="{Binding AllowMissionStart, UpdateSourceTrigger=PropertyChanged}"/>
<Button Content="暂停任务"
Command="{Binding MissionPauseCommand}" />
<Button Content="继续任务"
Command="{Binding MissionResumeCommand}" />
</WrapPanel>
<WrapPanel>
<Button Content="全部降落"
Command="{Binding AllLandCommand}" />
</WrapPanel>
</StackPanel>
</TabItem>
<TabItem Header="飞行报告" Visibility="Collapsed">
<StackPanel Orientation="Vertical" Margin="0,5,0,0">
<WrapPanel>
<Button Content="添加报告"
Command="{Binding UnlockAllCommand}"
IsEnabled="{Binding AllowMissionStart, UpdateSourceTrigger=PropertyChanged}" />
<Button Content="查看报告"
Command="{Binding LockAllCommand}" />
</WrapPanel>
</StackPanel>
</TabItem>
</TabControl>
</StackPanel>
</UserControl>
<UserControl x:Class="Plane.FormationCreator.Views.ControlPanelView"
xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
xmlns:d="http://schemas.microsoft.com/expression/blend/2008"
xmlns:local="clr-namespace:Plane.FormationCreator.Views"
xmlns:fc="clr-namespace:Plane.FormationCreator"
xmlns:ec="clr-namespace:Plane.Copters;assembly=PlaneGcsSdk_Private_NET46"
xmlns:vm="clr-namespace:Plane.FormationCreator.ViewModels"
xmlns:m="clr-namespace:Plane.FormationCreator.Formation"
mc:Ignorable="d"
d:DesignHeight="300"
d:DesignWidth="300">
<StackPanel Margin="0,0,-8,0">
<StackPanel.Resources>
<Style TargetType="Button"
BasedOn="{StaticResource {x:Type Button}}">
<Setter Property="Margin"
Value="0,0,3,5" />
</Style>
</StackPanel.Resources>
<!--<TextBlock Text="连接" />
<WrapPanel>
<Button Content="连接"
Command="{Binding ConnectCommand}" />
<Button Content="断开"
Command="{Binding DisconnectCommand}" />
</WrapPanel>-->
<TabControl Margin="0,0,13,0">
<TabItem Header="飞行前准备">
<StackPanel Orientation="Vertical" Margin="0,5,0,0">
<WrapPanel>
<Button Content="电压检测"
Command="{Binding DetectionVoltage}" />
<Button Content="版本检测"
Command="{Binding GetVersionsCommand}" />
<Button Content="通讯版本" Command="{Binding DetectionCommModuleVersion}" />
<Button Content="统计返回"
Command="{Binding DetectionReturnData}" />
<Button Content="通讯统计"
Command="{Binding GetCommsumCommand}" />
<TextBlock
Margin="5,5,5,5" Foreground ="Red" VerticalAlignment="Center"
Text="{Binding RTKState}"
/>
</WrapPanel>
<WrapPanel>
<Button Content="参数设置"
Command="{Binding ParamModify}" />
<Button Content="读入参数"
Command="{Binding LoadParamfile}" />
<Button Content="状态统计" Command="{Binding ReportGPSTypeCommand}"/>
<Button Content="飞机校准"
Command="{Binding CalibrationSingleCommand}" />
<Button Content="正式参数" Command="{Binding TurnOffTestLightsCommand}" Visibility="Collapsed" />
<Button Content="电机" Visibility="Collapsed"
Command="{Binding MotorTestCommand}" />
<Button Content="删除飞机"
Command="{Binding DelCommand}" />
<Label Visibility="Collapsed" Content="Lat"/>
<TextBox Visibility="Collapsed" Text="{Binding LatOffset}" Width="50"/>
<Label Visibility="Collapsed" Content="Lng"/>
<TextBox Visibility="Collapsed" Text="{Binding LngOffset}" Width="50"/>
<Button Visibility="Collapsed" Content="设置返航点" Command="{Binding RLTOffsetCommand}" />
</WrapPanel>
<WrapPanel>
<Button Content="写入航点"
Command="{Binding WriteMissionCommand}" />
<Button Content="航点续写"
Command="{Binding WriteMissionFailedCommand}" />
<Button Content="选写航点" Command="{Binding WriteMissionSingleCommand}" />
<Button Content="统计航点"
Command="{Binding DetectionMissionData}" />
<TextBox
Grid.Column="1"
Width="55"
Margin="5, 5, 5, 5"
HorizontalContentAlignment="Right"
Text="{Binding RTKcomvalue, UpdateSourceTrigger=PropertyChanged}" Visibility="Collapsed"
/>
<Button Content="{Binding RTKbtntxt}"
Command="{Binding SendRTKCommand}" Visibility="Collapsed" />
<Button Content="{Binding NTRIPbtntxt}"
Command="{Binding SendRTCMCommand}" Visibility="Collapsed"/>
<Button Content="RTK 控制" Command="{Binding OpenRtcmManageCommand}" />
</WrapPanel>
</StackPanel>
</TabItem>
<TabItem Header="飞行控制">
<StackPanel Orientation="Vertical" Margin="0,5,0,0">
<WrapPanel >
<Button Content="解锁"
Command="{Binding UnlockCommand}" />
<Button Content="加锁"
Command="{Binding LockCommand}" />
<Button Content="单独任务"
Command="{Binding TakeOffCommand}" />
<Button Content="抛物"
Command="{Binding ThrowoutCommand}" />
</WrapPanel>
<WrapPanel>
<Button Content="起飞"
Command="{Binding GuidAsyncCommand}" />
<Button Content="悬停"
Command="{Binding HoverCommand}" />
<Button Content="手动"
Command="{Binding FloatCommand}" />
<Button Content="返航"
Command="{Binding ReturnToLaunchCommand}" />
<Button Content="降落"
Command="{Binding LandCommand}" />
<Button Content="跳过" Visibility="Collapsed"
Command="{Binding FlagCommand}" />
<TextBox Width="50"
Visibility="Collapsed"
Text="{Binding AltP, UpdateSourceTrigger=PropertyChanged}" />
</WrapPanel>
<WrapPanel>
<Button Content="闪灯"
Command="{Binding LEDFlickerCommand}" />
<Button Content="测试" Visibility="Collapsed"
Command="{Binding TestCommand}" />
<Button Content="开灯"
Command="{Binding LEDOnOffCommand}"
CommandParameter="0"/>
<Button Content="关灯"
Command="{Binding LEDOnOffCommand}"
CommandParameter="1"/>
<TextBlock Margin="8,-3,5,0" Text="起飞高度" VerticalAlignment="Center"/>
<TextBox VerticalContentAlignment="Center" Margin="0,0,0,5" Width="30" Height="25" Text="{Binding TaskOffAlt}" />
<TextBlock Margin="5,-3,5,0" Text="米" VerticalAlignment="Center"/>
</WrapPanel>
</StackPanel>
</TabItem>
<TabItem Header="任务控制">
<StackPanel Orientation="Vertical" Margin="0,5,0,0">
<WrapPanel>
<Button Content="全部解锁"
Command="{Binding UnlockAllCommand}"
IsEnabled="{Binding AllowMissionStart, UpdateSourceTrigger=PropertyChanged}" />
<Button Content="全部加锁"
Command="{Binding LockAllCommand}" />
</WrapPanel>
<WrapPanel>
<Button Content="开始任务"
Command="{Binding MissionStartCommand}"
IsEnabled="{Binding AllowMissionStart, UpdateSourceTrigger=PropertyChanged}"/>
<Button Content="暂停任务"
Command="{Binding MissionPauseCommand}" />
<Button Content="继续任务"
Command="{Binding MissionResumeCommand}" />
</WrapPanel>
<WrapPanel>
<Button Content="全部降落"
Command="{Binding AllLandCommand}" />
<Button Content="紧急返航"
Command="{Binding EmergencyRetCommand}" />
</WrapPanel>
</StackPanel>
</TabItem>
<TabItem Header="飞行报告" Visibility="Collapsed">
<StackPanel Orientation="Vertical" Margin="0,5,0,0">
<WrapPanel>
<Button Content="添加报告"
Command="{Binding UnlockAllCommand}"
IsEnabled="{Binding AllowMissionStart, UpdateSourceTrigger=PropertyChanged}" />
<Button Content="查看报告"
Command="{Binding LockAllCommand}" />
</WrapPanel>
</StackPanel>
</TabItem>
</TabControl>
</StackPanel>
</UserControl>