紧急返航功能,策略不对,临时保存
This commit is contained in:
parent
2614dc2f59
commit
36875bcdc2
@ -163,7 +163,11 @@ namespace Plane.FormationCreator.Formation
|
|||||||
get { return _ModifyingSingleCopterInfo; }
|
get { return _ModifyingSingleCopterInfo; }
|
||||||
set { Set(nameof(ModifyingSingleCopterInfo), ref _ModifyingSingleCopterInfo, value); }
|
set { Set(nameof(ModifyingSingleCopterInfo), ref _ModifyingSingleCopterInfo, value); }
|
||||||
}
|
}
|
||||||
|
/// <summary>
|
||||||
|
/// 实际执行某一个任务--包含等待
|
||||||
|
/// </summary>
|
||||||
|
/// <returns></returns>
|
||||||
|
/// <exception cref="InvalidOperationException"></exception>
|
||||||
public async Task RunAsync()
|
public async Task RunAsync()
|
||||||
{
|
{
|
||||||
switch (TaskType)
|
switch (TaskType)
|
||||||
@ -172,8 +176,6 @@ namespace Plane.FormationCreator.Formation
|
|||||||
await RunFlyToTaskAsync().ConfigureAwait(false);
|
await RunFlyToTaskAsync().ConfigureAwait(false);
|
||||||
break;
|
break;
|
||||||
case FlightTaskType.TakeOff:
|
case FlightTaskType.TakeOff:
|
||||||
//多架同时起飞
|
|
||||||
//await MutilRunTakeOffTaskAsync().ConfigureAwait(false);
|
|
||||||
await NewMutilRunTakeOffTaskAsync().ConfigureAwait(false);
|
await NewMutilRunTakeOffTaskAsync().ConfigureAwait(false);
|
||||||
break;
|
break;
|
||||||
case FlightTaskType.Land:
|
case FlightTaskType.Land:
|
||||||
|
@ -22,6 +22,10 @@ using Plane.CommunicationManagement;
|
|||||||
using Microsoft.Practices.ServiceLocation;
|
using Microsoft.Practices.ServiceLocation;
|
||||||
using Plane.FormationCreator.ViewModels;
|
using Plane.FormationCreator.ViewModels;
|
||||||
using MahApps.Metro.Controls;
|
using MahApps.Metro.Controls;
|
||||||
|
using Newtonsoft.Json.Linq;
|
||||||
|
using GMap.NET.MapProviders;
|
||||||
|
using GMap.NET;
|
||||||
|
using static Plane.FormationCreator.CalculationLogLatDistance;
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
namespace Plane.FormationCreator.Formation
|
||||||
{
|
{
|
||||||
@ -648,7 +652,21 @@ namespace Plane.FormationCreator.Formation
|
|||||||
// SelectedTaskIndex = 0;
|
// 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()
|
public async Task ForceNextTasks()
|
||||||
{
|
{
|
||||||
@ -2199,6 +2217,15 @@ namespace Plane.FormationCreator.Formation
|
|||||||
}
|
}
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
CancellationTokenSource cts ;
|
||||||
|
|
||||||
|
//绕行取消,用于3D绕行计算
|
||||||
|
public void cancel_ABypassBAsync()
|
||||||
|
{
|
||||||
|
if (cts!=null)
|
||||||
|
cts.Cancel(); //取消掉 异步执行的 绕行函数
|
||||||
|
}
|
||||||
|
|
||||||
//新开线程异步调用ABypassB并等待返回
|
//新开线程异步调用ABypassB并等待返回
|
||||||
public async Task<(List<List<FlightRouteV2.Vector3>>,bool)> ABypassBAsync(FlightRouteV2.Vector3[] aVecs, FlightRouteV2.Vector3[] bVecs)
|
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(() =>
|
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
|
try
|
||||||
{
|
{
|
||||||
@ -2852,8 +2880,24 @@ namespace Plane.FormationCreator.Formation
|
|||||||
Message.Show($"经纬方式计算用时:{stopWatch.Elapsed.TotalMilliseconds}ms,总飞行距离{sumlength}米,有{crosscount}个交叉");
|
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)
|
private PLLocation CenterLatLng(List<PLLocation> point1, List<PLLocation> point2)
|
||||||
{
|
{
|
||||||
PLLocation centerLatLng = new PLLocation(0,0,0);
|
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;
|
TimeSpan timeSpan;
|
||||||
DateTime taskStartTime;
|
public DateTime taskStartTime;
|
||||||
public async Task RunTaskAsync()
|
public async Task RunTaskAsync()
|
||||||
{
|
{
|
||||||
if (Tasks.Count == 0) return;
|
if (Tasks.Count == 0) return;
|
||||||
@ -3249,10 +3315,14 @@ namespace Plane.FormationCreator.Formation
|
|||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
/// <summary>
|
||||||
|
/// 开始运行模拟任务
|
||||||
|
/// </summary>
|
||||||
|
/// <returns></returns>
|
||||||
public async Task RunAsync()
|
public async Task RunAsync()
|
||||||
{
|
{
|
||||||
IsPaused = false;
|
IsPaused = false;
|
||||||
|
IsEmergencyRet = false;
|
||||||
for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++)
|
for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++)
|
||||||
{
|
{
|
||||||
var task = Tasks[i];
|
var task = Tasks[i];
|
||||||
@ -3261,6 +3331,10 @@ namespace Plane.FormationCreator.Formation
|
|||||||
}
|
}
|
||||||
AppEx.Current.AppMode = AppMode.RunningTasks;
|
AppEx.Current.AppMode = AppMode.RunningTasks;
|
||||||
StartAvoidingCrash(); //开始碰撞检测
|
StartAvoidingCrash(); //开始碰撞检测
|
||||||
|
|
||||||
|
//告诉所有飞机开始任务--模拟飞机不计算起飞延迟,直接传0
|
||||||
|
foreach (var copter in _copterManager.Copters)
|
||||||
|
await copter.MissionStartAsync(0, 0, 0, 0, 0);
|
||||||
TaskState = TasksStatus.Running;
|
TaskState = TasksStatus.Running;
|
||||||
for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++)
|
for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++)
|
||||||
{
|
{
|
||||||
@ -3269,16 +3343,13 @@ namespace Plane.FormationCreator.Formation
|
|||||||
task.Status = FlightTaskStatus.Running;
|
task.Status = FlightTaskStatus.Running;
|
||||||
CurrentRunningTask = task;
|
CurrentRunningTask = task;
|
||||||
CurrentRunningTaskIndex = i;
|
CurrentRunningTaskIndex = i;
|
||||||
|
//显示提示信息
|
||||||
//////////////////显示提示信息
|
|
||||||
int starttime = 0;
|
int starttime = 0;
|
||||||
for (int j = 0; j < task.TaskIndex; j++)
|
for (int j = 0; j < task.TaskIndex; j++)
|
||||||
starttime += GetTaskTime(j);
|
starttime += GetTaskTime(j);
|
||||||
TimeSpan ts = new TimeSpan(0, 0, Convert.ToInt32(starttime));
|
TimeSpan ts = new TimeSpan(0, 0, Convert.ToInt32(starttime));
|
||||||
Message.Show($"{ts}:任务{i+1} {task.TaskCnName } 开始执行,需{ GetTaskTime(task.TaskIndex)}秒");
|
Message.Show($"{ts}:任务{i+1} {task.TaskCnName } 开始执行,需{ GetTaskTime(task.TaskIndex)}秒");
|
||||||
/////////////////////////
|
//开始执行任务
|
||||||
|
|
||||||
|
|
||||||
await task.RunAsync().ConfigureAwait(false);
|
await task.RunAsync().ConfigureAwait(false);
|
||||||
// 1. 被暂停时,中断 RunAsync。继续运行时将把此时运行了一半的 CurrentRunningTask 重新运行一遍。
|
// 1. 被暂停时,中断 RunAsync。继续运行时将把此时运行了一半的 CurrentRunningTask 重新运行一遍。
|
||||||
if (IsPaused == true)
|
if (IsPaused == true)
|
||||||
@ -3288,10 +3359,17 @@ namespace Plane.FormationCreator.Formation
|
|||||||
Message.Show($"任务{i + 1} {task.TaskCnName } 暂停执行");
|
Message.Show($"任务{i + 1} {task.TaskCnName } 暂停执行");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
//紧急返航中-跳出任务循环
|
||||||
|
if (IsEmergencyRet == true) break;
|
||||||
task.Status = FlightTaskStatus.Stop;
|
task.Status = FlightTaskStatus.Stop;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 2. 正常结束时,重置 CurrentRunningTask、CurrentRunningTaskIndex 和 IsPaused。
|
//紧急返航中-等待返航结束
|
||||||
|
while (IsEmergencyRet) {
|
||||||
|
//延时10ms等待
|
||||||
|
await Task.Delay(10);
|
||||||
|
}
|
||||||
|
// 2. 正常结束时,重置 CurrentRunningTask、CurrentRunningTaskIndex 和 IsPaused。
|
||||||
TaskState = TasksStatus.Stop;
|
TaskState = TasksStatus.Stop;
|
||||||
CurrentRunningTask = null;
|
CurrentRunningTask = null;
|
||||||
CurrentRunningTaskIndex = 0;
|
CurrentRunningTaskIndex = 0;
|
||||||
@ -3306,8 +3384,176 @@ namespace Plane.FormationCreator.Formation
|
|||||||
#endregion Run and pause.
|
#endregion Run and pause.
|
||||||
|
|
||||||
|
|
||||||
Dictionary<int, string> AvoidCrashLog = new Dictionary<int, string >();
|
//计算飞行一段时间后的位置
|
||||||
|
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 >();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@ -86,20 +86,10 @@ namespace Plane.FormationCreator.Formation
|
|||||||
for (int i = 0; i < infos.Count; i++)
|
for (int i = 0; i < infos.Count; i++)
|
||||||
{
|
{
|
||||||
var info = infos[i];
|
var info = infos[i];
|
||||||
|
//为每架飞机创建一个航点任务
|
||||||
tasks[i] = await Task.Factory.StartNew(async () =>
|
tasks[i] = await Task.Factory.StartNew(async () =>
|
||||||
{
|
{
|
||||||
var internalInfo = info;
|
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 FlyToTaskFlySingleCopterAsync(internalInfo);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
@ -116,7 +106,11 @@ namespace Plane.FormationCreator.Formation
|
|||||||
|
|
||||||
|
|
||||||
// private int RuningTaskRemaining = 0;
|
// private int RuningTaskRemaining = 0;
|
||||||
|
/// <summary>
|
||||||
|
/// 单独飞机执行飞行任务
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="info"></param>
|
||||||
|
/// <returns></returns>
|
||||||
private async Task FlyToTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
|
private async Task FlyToTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
|
||||||
{
|
{
|
||||||
DateTime dtNow = DateTime.Now;
|
DateTime dtNow = DateTime.Now;
|
||||||
@ -152,61 +146,20 @@ namespace Plane.FormationCreator.Formation
|
|||||||
targetLng = info.Copter.TakeOffPoint.Longitude;
|
targetLng = info.Copter.TakeOffPoint.Longitude;
|
||||||
}
|
}
|
||||||
int flytype = _flightTaskManager.getflytype(taskIndex);
|
int flytype = _flightTaskManager.getflytype(taskIndex);
|
||||||
|
//指定目标位置
|
||||||
await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt, task.FlytoTime, flytype);
|
await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt, task.FlytoTime, flytype);
|
||||||
// await Task.Delay(10).ConfigureAwait(false);
|
|
||||||
|
|
||||||
dtNow = DateTime.Now;
|
dtNow = DateTime.Now;
|
||||||
ts = dtNow - dtLastTime;
|
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)) //按时间轴飞:当前任务时间到达后自动飞往下个航点
|
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)
|
if (_flightTaskManager.IsPaused == true)
|
||||||
{
|
{
|
||||||
await info.Copter.HoverAsync();
|
await info.Copter.HoverAsync();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
await Task.Delay(100).ConfigureAwait(false); //判断是否到达位置10hz
|
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)
|
if (info.LEDInfos.Count > 0)
|
||||||
{
|
{
|
||||||
string LEDRGB = "";
|
string LEDRGB = "";
|
||||||
@ -223,10 +176,7 @@ namespace Plane.FormationCreator.Formation
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
info.Copter.LEDColor = LEDRGB;
|
info.Copter.LEDColor = LEDRGB;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// 当该飞机被标记时,悬停并跳过飞行任务
|
// 当该飞机被标记时,悬停并跳过飞行任务
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
if ((bool)_copterManager.CopterStatus[copterIndex])
|
||||||
{
|
{
|
||||||
|
@ -66,20 +66,20 @@ namespace Plane.FormationCreator.Formation
|
|||||||
var tasksTakeOff = new Task[infos.Count];
|
var tasksTakeOff = new Task[infos.Count];
|
||||||
for (int i = 0; i < infos.Count; i++)
|
for (int i = 0; i < infos.Count; i++)
|
||||||
{
|
{
|
||||||
//tasksTakeOff[i] = NewSingleRunTaskOffTaskAsunc(infos[i]);
|
|
||||||
|
|
||||||
|
|
||||||
tasksTakeOff[i] = await Task.Factory.StartNew(async () =>
|
tasksTakeOff[i] = await Task.Factory.StartNew(async () =>
|
||||||
{
|
{
|
||||||
var internalInfo = infos[i];
|
var internalInfo = infos[i];
|
||||||
|
|
||||||
await NewSingleRunTaskOffTaskAsunc(internalInfo);
|
await NewSingleRunTaskOffTaskAsunc(internalInfo);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
|
await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
|
||||||
//await Task.Delay(100);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 单独飞机 执行起飞任务
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="info"></param>
|
||||||
|
/// <returns></returns>
|
||||||
private async Task NewSingleRunTaskOffTaskAsunc(FlightTaskSingleCopterInfo info)
|
private async Task NewSingleRunTaskOffTaskAsunc(FlightTaskSingleCopterInfo info)
|
||||||
{
|
{
|
||||||
DateTime dtNow = DateTime.Now;
|
DateTime dtNow = DateTime.Now;
|
||||||
@ -113,7 +113,7 @@ namespace Plane.FormationCreator.Formation
|
|||||||
//info.Copter.takeOffTargetAltitude = takeOffAlt;
|
//info.Copter.takeOffTargetAltitude = takeOffAlt;
|
||||||
FlightTask task = _flightTaskManager.CurrentRunningTask;
|
FlightTask task = _flightTaskManager.CurrentRunningTask;
|
||||||
float takeflytime = task.TakeOffTime - info.TakeOffWaitTime;
|
float takeflytime = task.TakeOffTime - info.TakeOffWaitTime;
|
||||||
//开始往上飞
|
//飞行目标,开始往上飞
|
||||||
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt, takeflytime, 1); //秒
|
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt, takeflytime, 1); //秒
|
||||||
//解锁起飞用暗紫色
|
//解锁起飞用暗紫色
|
||||||
info.Copter.LEDColor = CopterManager.CopterTakeoffColor;
|
info.Copter.LEDColor = CopterManager.CopterTakeoffColor;
|
||||||
@ -121,7 +121,7 @@ namespace Plane.FormationCreator.Formation
|
|||||||
dtNow = DateTime.Now;
|
dtNow = DateTime.Now;
|
||||||
ts = dtNow - dtLastTime;
|
ts = dtNow - dtLastTime;
|
||||||
|
|
||||||
|
//等待起飞时间完成,并模拟灯光--不移动飞机-飞机自己计算并移动位置
|
||||||
while (ts.TotalMilliseconds < task.TakeOffTime * 1000)
|
while (ts.TotalMilliseconds < task.TakeOffTime * 1000)
|
||||||
{
|
{
|
||||||
if (_flightTaskManager.IsPaused == true)
|
if (_flightTaskManager.IsPaused == true)
|
||||||
@ -154,7 +154,6 @@ namespace Plane.FormationCreator.Formation
|
|||||||
}
|
}
|
||||||
//起飞完成用默认颜色
|
//起飞完成用默认颜色
|
||||||
info.Copter.LEDColor = CopterManager.CopterFlyingColor;
|
info.Copter.LEDColor = CopterManager.CopterFlyingColor;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -395,6 +395,42 @@ namespace Plane.FormationCreator.ViewModels
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private ICommand _EmergencyRetCommand;
|
||||||
|
public ICommand EmergencyRetCommand
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
return _EmergencyRetCommand ?? (_EmergencyRetCommand = new RelayCommand(async () =>
|
||||||
|
{
|
||||||
|
if (Alert.Show("您确定要紧急返航吗?紧急返航可能导致飞行器碰撞!!!!", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning)
|
||||||
|
!= MessageBoxResult.OK)
|
||||||
|
return; //计算当前图案中心点,起飞图案中心点,图案距离起飞图案中心点最近距离
|
||||||
|
FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
|
||||||
|
//计算无人机返航方向和最近的飞机距离
|
||||||
|
PLLocation takeoffcentloc;
|
||||||
|
PLLocation taskcentloc;
|
||||||
|
double mindistance; //单位米
|
||||||
|
bool overlapping; //图案否重叠
|
||||||
|
int rettime= (int)(DateTime.Now - _flightTaskManager.taskStartTime).TotalSeconds;
|
||||||
|
if (!_flightTaskManager.EmergencyRet(rettime,out takeoffcentloc,out taskcentloc, out mindistance,out overlapping))
|
||||||
|
{
|
||||||
|
Alert.Show("紧急返航数据计算失败!", "提示");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
rettime += 5; //加5秒延迟用于通讯重复,有些飞机通讯不成功多次发送才收到,比别的飞机慢
|
||||||
|
|
||||||
|
//模拟测试
|
||||||
|
if (_flightTaskManager.TaskState != TasksStatus.Stop)
|
||||||
|
await _flightTaskManager.sim_DoMissionEmergencyRetAsync(takeoffcentloc, taskcentloc, (float)mindistance, rettime, overlapping);
|
||||||
|
for (int i = 0; i < 10; i++)
|
||||||
|
{
|
||||||
|
await _commModuleManager.DoMissionEmergencyRetAsync(takeoffcentloc,taskcentloc,(float)mindistance, rettime, overlapping);
|
||||||
|
}
|
||||||
|
}));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
private ICommand _LEDFlickerCommand;
|
private ICommand _LEDFlickerCommand;
|
||||||
public ICommand LEDFlickerCommand
|
public ICommand LEDFlickerCommand
|
||||||
{
|
{
|
||||||
|
@ -198,8 +198,11 @@
|
|||||||
|
|
||||||
<WrapPanel>
|
<WrapPanel>
|
||||||
|
|
||||||
<Button Content="全部降落"
|
<Button Content="全部降落"
|
||||||
Command="{Binding AllLandCommand}" />
|
Command="{Binding AllLandCommand}" />
|
||||||
|
<Button Content="紧急返航"
|
||||||
|
Command="{Binding EmergencyRetCommand}" />
|
||||||
|
|
||||||
|
|
||||||
</WrapPanel>
|
</WrapPanel>
|
||||||
</StackPanel>
|
</StackPanel>
|
||||||
|
Loading…
Reference in New Issue
Block a user