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:

270
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,9 +3359,16 @@ namespace Plane.FormationCreator.Formation
Message.Show($"任务{i + 1} {task.TaskCnName } 暂停执行");
return;
}
//紧急返航中-跳出任务循环
if (IsEmergencyRet == true) break;
task.Status = FlightTaskStatus.Stop;
}
//紧急返航中-等待返航结束
while (IsEmergencyRet) {
//延时10ms等待
await Task.Delay(10);
}
// 2. 正常结束时，重置 CurrentRunningTask、CurrentRunningTaskIndex 和 IsPaused。
TaskState = TasksStatus.Stop;
CurrentRunningTask = null;
@ -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 >();

68
Plane.FormationCreator/Formation/FlightTask_FlyTo.cs
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])
{

15
Plane.FormationCreator/Formation/FlightTask_TakeOff.cs
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;
}

36
Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs
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
{

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

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