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

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

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:

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,8 +3384,176 @@ namespace Plane.FormationCreator.Formation
#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 >();

View File

@ -86,20 +86,10 @@ namespace Plane.FormationCreator.Formation
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);
});
}
@ -116,7 +106,11 @@ namespace Plane.FormationCreator.Formation
// private int RuningTaskRemaining = 0;
/// <summary>
/// 单独飞机执行飞行任务
/// </summary>
/// <param name="info"></param>
/// <returns></returns>
private async Task FlyToTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
{
DateTime dtNow = DateTime.Now;
@ -152,61 +146,20 @@ namespace Plane.FormationCreator.Formation
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 = "";
@ -223,10 +176,7 @@ namespace Plane.FormationCreator.Formation
break;
}
info.Copter.LEDColor = LEDRGB;
}
// 当该飞机被标记时,悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{

View File

@ -66,20 +66,20 @@ namespace Plane.FormationCreator.Formation
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);
}
/// <summary>
/// 单独飞机 执行起飞任务
/// </summary>
/// <param name="info"></param>
/// <returns></returns>
private async Task NewSingleRunTaskOffTaskAsunc(FlightTaskSingleCopterInfo info)
{
DateTime dtNow = DateTime.Now;
@ -113,7 +113,7 @@ namespace Plane.FormationCreator.Formation
//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;
@ -121,7 +121,7 @@ namespace Plane.FormationCreator.Formation
dtNow = DateTime.Now;
ts = dtNow - dtLastTime;
//等待起飞时间完成,并模拟灯光--不移动飞机-飞机自己计算并移动位置
while (ts.TotalMilliseconds < task.TakeOffTime * 1000)
{
if (_flightTaskManager.IsPaused == true)
@ -154,7 +154,6 @@ namespace Plane.FormationCreator.Formation
}
//起飞完成用默认颜色
info.Copter.LEDColor = CopterManager.CopterFlyingColor;
}

View File

@ -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;
public ICommand LEDFlickerCommand
{

View File

@ -198,8 +198,11 @@
<WrapPanel>
<Button Content="全部降落"
<Button Content="全部降落"
Command="{Binding AllLandCommand}" />
<Button Content="紧急返航"
Command="{Binding EmergencyRetCommand}" />
</WrapPanel>
</StackPanel>