2319 lines
89 KiB
C#
2319 lines
89 KiB
C#
using Plane.Collections;
|
||
using Plane.Copters;
|
||
using Plane.Geography;
|
||
using Plane.Windows.Messages;
|
||
using GalaSoft.MvvmLight;
|
||
using Newtonsoft.Json;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Collections.ObjectModel;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
using Plane.CommunicationManagement;
|
||
using FlightRoute;
|
||
using System.IO;
|
||
using System.Windows.Media.Media3D;
|
||
using System.Diagnostics;
|
||
using Plane.FormationCreator.Util;
|
||
using System.Collections;
|
||
|
||
namespace Plane.FormationCreator.Formation
|
||
{
|
||
public class FlightTaskManager : ObservableObject
|
||
{
|
||
public FlightTaskManager(CopterManager copterManager)
|
||
{
|
||
LoadIni();
|
||
_copterManager = copterManager;
|
||
|
||
//AddTakeOffTask(_copterManager.Copters);
|
||
|
||
_copterManager.Copters.CollectionChanged += (sender, e) =>
|
||
{
|
||
// TODO: 林俊清, 20150724, 需要改为正确的做法（清除旧飞机的任务，补充新飞机的任务）。
|
||
//AddTakeOffTask(e.NewItems?.Cast<ICopter>());
|
||
};
|
||
|
||
_copterManager.SelectedCoptersChanged += (sender, e) =>
|
||
{
|
||
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() == 1)
|
||
CommModuleManager.Instance.LED_FlickerAsync(_copterManager.AcceptingControlCopters);
|
||
|
||
/*
|
||
foreach (ICopter copter in _copterManager.AcceptingControlCopters)
|
||
{
|
||
copter.LEDAsync();
|
||
}
|
||
*/
|
||
|
||
// TODO: 林俊清, 20150803, 处理选中多个飞行器的情况。
|
||
if (_copterManager.SelectedCopters.Count() > 1)
|
||
{
|
||
return;
|
||
}
|
||
var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault();
|
||
|
||
foreach (var task in Tasks)
|
||
{
|
||
foreach (var info in task.SingleCopterInfos)
|
||
{
|
||
if (info.Copter == selectedCopter)
|
||
{
|
||
task.ModifyingSingleCopterInfo = info;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if (selectedCopter!=null)
|
||
selectedCopter.SetShowLEDFlashAsync(1, 100);
|
||
|
||
//if (selectedCopter != null)
|
||
//selectedCopter.LEDAsync();
|
||
};
|
||
|
||
TaskAdded += (sender, e) =>
|
||
{
|
||
// TODO: 林俊清, 20150803, 处理选中多个飞行器的情况。
|
||
if (_copterManager.SelectedCopters.Count() > 1)
|
||
{
|
||
return;
|
||
}
|
||
var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault();
|
||
|
||
var task = e.AddedTask;
|
||
foreach (var info in task.SingleCopterInfos)
|
||
{
|
||
if (info.Copter == selectedCopter)
|
||
{
|
||
task.ModifyingSingleCopterInfo = info;
|
||
break;
|
||
}
|
||
}
|
||
};
|
||
}
|
||
|
||
private CopterManager _copterManager;
|
||
|
||
|
||
|
||
private string _MessageText="test";
|
||
public string MessageText
|
||
{
|
||
get { return _MessageText; }
|
||
set
|
||
{
|
||
if (Set(nameof(MessageText), ref _MessageText, value))
|
||
{
|
||
// _lastUpdateStatusTextTime = DateTime.Now;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
private bool _TaskRun_2D = true;
|
||
public bool TaskRun_2D
|
||
{
|
||
get { return _TaskRun_2D; }
|
||
set
|
||
{
|
||
if (Set(nameof(TaskRun_2D), ref _TaskRun_2D, value))
|
||
{
|
||
|
||
}
|
||
}
|
||
}
|
||
|
||
private void AddTakeOffTask(IEnumerable<ICopter> copters)
|
||
{
|
||
if (copters == null || !copters.Any()) return;
|
||
|
||
bool takeOffTaskExisted = Tasks.Count >= 1;
|
||
FlightTask takeOffTask;
|
||
if (takeOffTaskExisted)
|
||
{
|
||
takeOffTask = Tasks[0];
|
||
}
|
||
else
|
||
{
|
||
takeOffTask = new FlightTask(FlightTaskType.TakeOff);
|
||
Tasks.Add(takeOffTask);
|
||
RaiseTaskAdded(null, takeOffTask);
|
||
//TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { AddedTask = takeOffTask });
|
||
}
|
||
foreach (var copter in copters)
|
||
{
|
||
takeOffTask.SingleCopterInfos.Add(FlightTaskSingleCopterInfo.CreateForTakeOffTask(copter, targetAlt: 15));
|
||
}
|
||
}
|
||
|
||
public ObservableCollection<FlightTask> Tasks { get; } = new ObservableCollection<FlightTask>();
|
||
|
||
|
||
|
||
public int GetTaskTime(int TaskIndex)
|
||
{
|
||
int tasktime = 0;
|
||
if ((Tasks==null)|| TaskIndex> Tasks.Count-1) return tasktime;
|
||
|
||
FlightTask value = Tasks[TaskIndex];
|
||
switch (value.TaskType)
|
||
{
|
||
case FlightTaskType.TakeOff: tasktime = value.TakeOffTime; ; break;
|
||
case FlightTaskType.FlyTo: tasktime = value.FlytoTime + value.LoiterTime; break;
|
||
//降落时间计算前一个任务目标高度最高的飞机的降落时间按1.5米/秒下降
|
||
case FlightTaskType.Land:
|
||
float maxalt = 0.0f;
|
||
for (int i = 0; i < Tasks[TaskIndex-1].SingleCopterInfos.Count; i++)
|
||
{
|
||
var copterInfo = Tasks[TaskIndex - 1].SingleCopterInfos[i];
|
||
if (maxalt < copterInfo.TargetAlt) maxalt = copterInfo.TargetAlt;
|
||
}
|
||
tasktime =(int) Math.Round(maxalt / 1.5,0);
|
||
break;
|
||
}
|
||
return tasktime;
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
private FlightTask _SelectedTask;
|
||
public FlightTask SelectedTask
|
||
{
|
||
get { return _SelectedTask; }
|
||
set
|
||
{
|
||
if (_SelectedTask != value)
|
||
{
|
||
int starttime=0;
|
||
|
||
if (_SelectedTask != null)
|
||
{
|
||
|
||
_SelectedTask.IsSelected = false;
|
||
if (TaskState == TasksStatus.Stop)
|
||
_SelectedTask.Status = FlightTaskStatus.Stop;
|
||
}
|
||
|
||
if (value != null)
|
||
{
|
||
value.IsSelected = true;
|
||
value.IsRightSelected = true;
|
||
RightSelect(value);
|
||
if (TaskState== TasksStatus.Stop)
|
||
value.Status = FlightTaskStatus.Selected;
|
||
|
||
for (int i = 0; i < value.TaskIndex; i++)
|
||
starttime += GetTaskTime(i);
|
||
|
||
TimeSpan ts = new TimeSpan(0, 0, Convert.ToInt32(starttime));
|
||
string str = "";
|
||
if (ts.Hours > 0)
|
||
{
|
||
str = ts.Hours.ToString() + "小时" + ts.Minutes.ToString() + "分" + ts.Seconds + "秒";
|
||
}
|
||
if (ts.Hours == 0 && ts.Minutes > 0)
|
||
{
|
||
str = ts.Minutes.ToString() + "分" + ts.Seconds + "秒";
|
||
}
|
||
if (ts.Hours == 0 && ts.Minutes == 0)
|
||
{
|
||
str = ts.Seconds + "秒";
|
||
}
|
||
Message.ShowStatus($"选中 [{value.TaskIndex+1} {value.TaskCnName }] 从{str}开始执行，需{ GetTaskTime(value.TaskIndex)}秒");
|
||
}
|
||
else Message.ShowStatus($"无任务选中");
|
||
|
||
}
|
||
Set(nameof(SelectedTask), ref _SelectedTask, value);
|
||
}
|
||
}
|
||
|
||
private int _SelectedTaskIndex;
|
||
public int SelectedTaskIndex
|
||
{
|
||
get { return _SelectedTaskIndex; }
|
||
set { Set(nameof(SelectedTaskIndex), ref _SelectedTaskIndex, value);
|
||
// this.SelectedTask = Tasks[value];
|
||
}
|
||
}
|
||
|
||
// 右键单击任务，用于隐藏任务图标, added by ZJF
|
||
private int _RightSelectedTaskIndex;
|
||
public int RightSelectedTaskIndex
|
||
{
|
||
get { return _RightSelectedTaskIndex; }
|
||
set { Set(nameof(RightSelectedTaskIndex), ref _RightSelectedTaskIndex, value); }
|
||
}
|
||
|
||
private FlightTask _CurrentRunningTask;
|
||
public FlightTask CurrentRunningTask
|
||
{
|
||
get { return _CurrentRunningTask; }
|
||
private set { Set(nameof(CurrentRunningTask), ref _CurrentRunningTask, value); }
|
||
}
|
||
|
||
private int _CurrentRunningTaskIndex;
|
||
public int CurrentRunningTaskIndex
|
||
{
|
||
get { return _CurrentRunningTaskIndex; }
|
||
private set { Set(nameof(CurrentRunningTaskIndex), ref _CurrentRunningTaskIndex, value); }
|
||
}
|
||
|
||
private int _ColumnCount = 5;
|
||
public int ColumnCount
|
||
{
|
||
get { return _ColumnCount; }
|
||
set { Set(nameof(ColumnCount), ref _ColumnCount, value); }
|
||
}
|
||
|
||
|
||
private float _ColumnDistance = 4.0f;
|
||
public float ColumnDistance
|
||
{
|
||
get { return _ColumnDistance; }
|
||
set { Set(nameof(ColumnDistance), ref _ColumnDistance, value); }
|
||
}
|
||
|
||
private float _RowDistance = 4.0f;
|
||
public float RowDistance
|
||
{
|
||
get { return _RowDistance; }
|
||
set { Set(nameof(RowDistance), ref _RowDistance, value); }
|
||
}
|
||
|
||
private int _Orientation = 0;
|
||
public int Orientation
|
||
{
|
||
get { return _Orientation; }
|
||
set { Set(nameof(Orientation), ref _Orientation, value); }
|
||
}
|
||
|
||
|
||
private TasksStatus _TaskState = TasksStatus.Stop;
|
||
public TasksStatus TaskState
|
||
{
|
||
get { return _TaskState; }
|
||
private set { Set(nameof(TaskState), ref _TaskState, value); }
|
||
}
|
||
|
||
public event EventHandler<FlightTaskAddedEventArgs> TaskAdded;
|
||
public event EventHandler<FlightTaskDeledEventArgs> TaskDeled;
|
||
|
||
public void RaiseTaskDeled(FlightTask vDeledTask,int vTaskIndex)
|
||
{
|
||
try
|
||
{
|
||
TaskDeled?.Invoke(this, new FlightTaskDeledEventArgs { DeledTask = vDeledTask, TaskIndex= vTaskIndex });
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
//RaiseExceptionThrown(ex);
|
||
}
|
||
|
||
|
||
}
|
||
public void RaiseTaskAdded(FlightTask lastTask ,FlightTask newTask)
|
||
{
|
||
try
|
||
{
|
||
TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask });
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
//RaiseExceptionThrown(ex);
|
||
}
|
||
newTask.PropertyChanged += (sender, e) =>
|
||
{
|
||
switch (e.PropertyName)
|
||
{
|
||
case nameof(FlightTask.TaskTypeIndex):
|
||
case nameof(FlightTask.TaskCnName):
|
||
TaskTypeChanged?.Invoke(this, new FlightTaskTypeChangedEventArgs((FlightTask)sender));
|
||
TaskcnNameChanged?.Invoke(this, new FlightTaskcnNameChangedEventArgs((FlightTask)sender));
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
};
|
||
}
|
||
|
||
|
||
|
||
|
||
public event EventHandler<FlightTaskAddedOriginalEventArgs> OnOriginalSet;
|
||
|
||
public event EventHandler<SingleCopterInfoChangedEventArgs> SingleCopterInfoChanged;
|
||
|
||
public void RaiseSingleCopterTaskChanged(FlightTaskSingleCopterInfo singleCopterInfo)
|
||
{
|
||
SingleCopterInfoChanged?.Invoke(this, new SingleCopterInfoChangedEventArgs(singleCopterInfo));
|
||
}
|
||
|
||
|
||
public event EventHandler<FlightTaskTypeChangedEventArgs> TaskTypeChanged;
|
||
public event EventHandler<FlightTaskcnNameChangedEventArgs> TaskcnNameChanged;
|
||
|
||
public event EventHandler TasksCleared;
|
||
|
||
public void AddTask()
|
||
{
|
||
//if (AppEx.Instance.CurrentMode == AppMode.PreparedForRunningTasks)
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
if (!copters.Any()) return;
|
||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
||
if (Tasks.Count == 0)
|
||
AddTakeOffTask(copters);
|
||
var lastTask = Tasks.LastOrDefault();
|
||
if (SelectedTask !=null)
|
||
lastTask = SelectedTask;
|
||
var nullableCenter = copters.GetCenter();
|
||
if (nullableCenter == null) return;
|
||
var center = nullableCenter.Value;
|
||
var newTask = new FlightTask(FlightTaskType.FlyTo);
|
||
int coptindex = 0;
|
||
|
||
int colnum = ColumnCount; //自动生成列数=4
|
||
float coldis = ColumnDistance;//列相距5米
|
||
float rowdis = RowDistance;//行相距5米
|
||
float matrixdis = 20; //生成方阵距离30米
|
||
int orientation = Orientation;
|
||
|
||
int currcol = 0; //当前列号
|
||
int currrow = 0; //当前行
|
||
Tuple<double, double> colLatLng = new Tuple<double, double>(0, 0);
|
||
Tuple<double, double> targetLatLng = new Tuple<double, double>(0, 0);
|
||
FlightTaskSingleCopterInfo lastSingleCopterInfo = null;
|
||
FlightTaskSingleCopterInfo preSingleCopterInfo = null;
|
||
|
||
foreach (var copter in copters)
|
||
{
|
||
preSingleCopterInfo = lastTask.SingleCopterInfos.Find(info => info.Copter == copter);
|
||
if (coptindex == 0)
|
||
{
|
||
lastSingleCopterInfo = lastTask.SingleCopterInfos.Find(info => info.Copter == copter);
|
||
targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(lastSingleCopterInfo.TargetLat, lastSingleCopterInfo.TargetLng, orientation, matrixdis);
|
||
colLatLng = targetLatLng;
|
||
}
|
||
else
|
||
{
|
||
if (currcol < colnum)
|
||
targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Item1, colLatLng.Item2, orientation + 90, currcol * coldis);
|
||
else
|
||
{
|
||
currrow++;
|
||
currcol = 0;
|
||
targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Item1, colLatLng.Item2, orientation + 180, rowdis);
|
||
colLatLng = targetLatLng;
|
||
}
|
||
}
|
||
currcol++;
|
||
coptindex++;
|
||
|
||
|
||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(copter, targetLatLng.Item1, targetLatLng.Item2, preSingleCopterInfo.TargetAlt);
|
||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
|
||
}
|
||
int selindex = SelectedTaskIndex+1;
|
||
Tasks.Insert(SelectedTaskIndex+1, newTask);
|
||
RaiseTaskAdded(lastTask, newTask);
|
||
SelectTask(selindex);
|
||
|
||
|
||
/*
|
||
Tasks.Add(newTask);
|
||
RaiseTaskAdded(lastTask, newTask);
|
||
SelectedTask = newTask;
|
||
SelectedTaskIndex = Tasks.Count - 1;
|
||
*/
|
||
|
||
|
||
|
||
}
|
||
}
|
||
|
||
//是否显示计划航线
|
||
private bool _showroute = true;
|
||
public bool showroute
|
||
{
|
||
get { return _showroute; }
|
||
set { Set(nameof(showroute), ref _showroute, value); }
|
||
}
|
||
|
||
private double _OriginLat = 0;
|
||
public double OriginLat
|
||
{
|
||
get { return _OriginLat; }
|
||
set { Set(nameof(OriginLat), ref _OriginLat, value); }
|
||
}
|
||
|
||
private double _OriginLng = 0;
|
||
public double OriginLng
|
||
{
|
||
get { return _OriginLng; }
|
||
set { Set(nameof(OriginLng), ref _OriginLng, value); }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 起始点作为参考，设置起始点后飞行写入的数据将为相对坐标
|
||
/// </summary>
|
||
public void SetOriginal()
|
||
{
|
||
OnOriginalSet?.Invoke(this, new FlightTaskAddedOriginalEventArgs { Lat = OriginLat, Lng = OriginLng });
|
||
}
|
||
|
||
|
||
public void ClearTasks()
|
||
{
|
||
ResetTasks();
|
||
this.Tasks.Clear();
|
||
SelectedTask = null;
|
||
SelectedTaskIndex = 0;
|
||
//取消删除事件
|
||
/*
|
||
if (TaskDeled != null)
|
||
{
|
||
Delegate[] dels = TaskDeled.GetInvocationList();
|
||
if (dels != null)
|
||
{
|
||
foreach (Delegate d in dels)
|
||
{
|
||
TaskDeled -= d as EventHandler<FlightTaskDeledEventArgs>;
|
||
}
|
||
}
|
||
}
|
||
*/
|
||
|
||
TasksCleared?.Invoke(this, EventArgs.Empty);
|
||
// AddTakeOffTask(_copterManager.Copters);
|
||
}
|
||
//删除选中的任务
|
||
public void DelSelectedTask()
|
||
{
|
||
if (SelectedTask == null) return;
|
||
if (SelectedTask.TaskType == FlightTaskType.TakeOff) return;
|
||
int selindex = SelectedTaskIndex;
|
||
|
||
// ResetTasks();
|
||
SelectedTask.SingleCopterInfos.Clear();
|
||
Tasks.RemoveAt(SelectedTaskIndex);
|
||
RaiseTaskDeled(SelectedTask,SelectedTaskIndex);
|
||
|
||
SelectTask(selindex - 1);
|
||
|
||
|
||
// SelectedTaskIndex = 0;
|
||
}
|
||
|
||
|
||
|
||
public async Task ForceNextTasks()
|
||
{
|
||
if (CurrentRunningTaskIndex == Tasks.Count - 1)
|
||
return;
|
||
|
||
Pause();
|
||
int i = 0;
|
||
//等待暂停或2s超时(80*25ms)
|
||
while ((TaskState != TasksStatus.Paused)||(i>80))
|
||
{
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
i++;
|
||
}
|
||
|
||
if (TaskState == TasksStatus.Paused)
|
||
{
|
||
CurrentRunningTask.Status = FlightTaskStatus.Stop;
|
||
CurrentRunningTask = null;
|
||
|
||
//起飞任务需要跳过
|
||
// if (CurrentRunningTaskIndex == 0)
|
||
// CurrentRunningTaskIndex++;
|
||
CurrentRunningTaskIndex++;
|
||
await RunTaskAsync();
|
||
}
|
||
}
|
||
|
||
//指定任务开始模拟
|
||
public async Task FlyToTasks()
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
if ((SelectedTaskIndex==-1) ||(TaskState == TasksStatus.Stop))
|
||
return;
|
||
|
||
int vSelectedTaskIndex= SelectedTaskIndex;
|
||
|
||
Pause();
|
||
int i = 0;
|
||
//等待暂停或2s超时(80*25ms)
|
||
while ((TaskState != TasksStatus.Paused) || (i > 80))
|
||
{
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
i++;
|
||
}
|
||
if (SelectedTaskIndex == -1)
|
||
return;
|
||
|
||
if (TaskState == TasksStatus.Paused)
|
||
{
|
||
CurrentRunningTask.Status = FlightTaskStatus.Stop;
|
||
CurrentRunningTask = null;
|
||
CurrentRunningTaskIndex = SelectedTaskIndex;
|
||
|
||
|
||
|
||
//设置所有模拟飞机的位置
|
||
for (int j = 0; j < copters.Count; j++)
|
||
{
|
||
var copter = copters[j];
|
||
var fc = copter as FakeCopter;
|
||
float targalt = 0.0f;
|
||
double lat = 0.0f;
|
||
double lng = 0.0f;
|
||
|
||
//飞机回到前一个任务位置，
|
||
|
||
//起飞任务直接用当前起飞任务位置
|
||
if (Tasks[SelectedTaskIndex].TaskType == FlightTaskType.TakeOff)
|
||
{
|
||
lat = Tasks[SelectedTaskIndex].SingleCopterInfos[j].TargetLat;
|
||
lng = Tasks[SelectedTaskIndex].SingleCopterInfos[j].TargetLng;
|
||
targalt = 0.0f;
|
||
}
|
||
else
|
||
{
|
||
//返航点直接设置起飞位置
|
||
if (Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j].IsLandWaypoint)
|
||
{
|
||
lat = Tasks[0].SingleCopterInfos[j].TargetLat;
|
||
lng = Tasks[0].SingleCopterInfos[j].TargetLng;
|
||
}
|
||
else
|
||
{
|
||
|
||
//用前一个任务目标位置
|
||
lat = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j].TargetLat;
|
||
lng = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j].TargetLng;
|
||
}
|
||
|
||
//如果前一个是起飞任务,没有目标高度，用当前的
|
||
if (Tasks[SelectedTaskIndex - 1].TaskType == FlightTaskType.TakeOff)
|
||
targalt = Tasks[SelectedTaskIndex].SingleCopterInfos[j].TargetAlt;
|
||
else
|
||
targalt = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j].TargetAlt;
|
||
}
|
||
fc.SetProperties(
|
||
latitude: lat,
|
||
longitude: lng,
|
||
altitude: targalt
|
||
|
||
);
|
||
|
||
}
|
||
|
||
|
||
|
||
//起飞任务需要跳过
|
||
// if (CurrentRunningTaskIndex == 0)
|
||
// CurrentRunningTaskIndex++;
|
||
|
||
await RunTaskAsync();
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
public async Task ResetTasks()
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
if (Tasks.Count == 0) return;
|
||
if (TaskState == TasksStatus.Running)
|
||
{
|
||
|
||
Pause();
|
||
//等待暂停或2s超时(80*25ms)
|
||
int k = 0;
|
||
while ((TaskState != TasksStatus.Paused) || (k > 80))
|
||
{
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
k++;
|
||
}
|
||
}
|
||
TaskState = TasksStatus.Stop;
|
||
|
||
CurrentRunningTaskIndex = 0;
|
||
if (CurrentRunningTask != null)
|
||
{
|
||
CurrentRunningTask.Status = FlightTaskStatus.Stop;
|
||
CurrentRunningTask = null;
|
||
}
|
||
|
||
for (int i = 0; i < Tasks.Count; i++)
|
||
{
|
||
// 将起飞的阶段标志位都置位0
|
||
if (Tasks[i].TaskType == FlightTaskType.TakeOff)
|
||
{
|
||
var infos = Tasks[i].SingleCopterInfos;
|
||
for (int j = 0; j < infos.Count; j++)
|
||
{
|
||
var info = infos[j];
|
||
info.takeOffStage = 0;
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
|
||
|
||
//设置所有模拟飞机的位置
|
||
for (int j = 0; j < copters.Count; j++)
|
||
{
|
||
var copter = copters[j];
|
||
var fc = copter as FakeCopter;
|
||
if (fc != null)
|
||
{
|
||
fc.SetProperties(
|
||
latitude: Tasks[0].SingleCopterInfos[j].TargetLat,
|
||
longitude: Tasks[0].SingleCopterInfos[j].TargetLng,
|
||
altitude: 0 );
|
||
//设置灯光为默认颜色
|
||
fc.LEDColor = CopterManager.CopterDefaultColor;// "000000";
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
public string ExportC4DFlytoTask()
|
||
{
|
||
int count = SelectedTask.SingleCopterInfos.Count();
|
||
//string[] lines = new string[count];
|
||
StringBuilder sb = new StringBuilder();
|
||
for (int i = 0; i < count; i++)
|
||
{
|
||
var singleCopterInfo = SelectedTask.SingleCopterInfos[i];
|
||
double x = 100 * GeographyUtils.CalcDistance(OriginLat, OriginLng, 0,
|
||
OriginLat, singleCopterInfo.TargetLng, 0);
|
||
if (singleCopterInfo.TargetLng < OriginLng) x = -x;
|
||
double y = 100 * singleCopterInfo.TargetAlt;
|
||
|
||
double z = 100 * GeographyUtils.CalcDistance(OriginLat, OriginLng, 0,
|
||
singleCopterInfo.TargetLat, OriginLng, 0);
|
||
if (singleCopterInfo.TargetLat < OriginLat) z = -z;
|
||
sb.AppendLine($"{i + 1} 0 {x} {y} {z}");
|
||
//lines[i] = $"{i+1} {x} {y} {z}";
|
||
}
|
||
|
||
return sb.ToString().Trim();
|
||
}
|
||
|
||
//导入外部航点
|
||
public void ImportDlltoTask(string filename)
|
||
{
|
||
Vector3[] vc;
|
||
|
||
|
||
string extname = Path.GetExtension(filename);
|
||
if (extname == ".svg")
|
||
{
|
||
vc = FlyBase.svgToPos(filename);
|
||
}
|
||
else if (extname == ".obj")
|
||
{
|
||
vc = FlyBase.objToPos(filename);
|
||
}
|
||
else return;
|
||
|
||
if (vc.Count()!= _copterManager.Copters.Count)
|
||
{
|
||
Alert.Show($"飞机数量不匹配！导入{vc.Count()}架，实际{_copterManager.Copters.Count}");
|
||
return;
|
||
}
|
||
var lastTask = Tasks.LastOrDefault();
|
||
var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = true, FlytoTime = 10, LoiterTime = 1 };
|
||
newTask.TaskCnName = Path.GetFileNameWithoutExtension(filename).Substring(0,6);
|
||
int id =0;
|
||
foreach (Vector3 item in vc)
|
||
{
|
||
Tuple<double, double> observationLatLng = null;
|
||
observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
|
||
OriginLat,
|
||
OriginLng,
|
||
90,
|
||
(float)item.x / 100);
|
||
|
||
observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
|
||
observationLatLng.Item1,
|
||
observationLatLng.Item2,
|
||
0,
|
||
(float)item.z / 100);
|
||
|
||
var thisSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
|
||
_copterManager.Copters[id++], new LatLng(observationLatLng.Item1 - OriginLat, observationLatLng.Item2 - OriginLng),
|
||
(float)item.y / 100, false);
|
||
newTask.SingleCopterInfos.Add(thisSingleCopterInfo);
|
||
}
|
||
Tasks.Add(newTask);
|
||
RaiseTaskAdded(lastTask, newTask);
|
||
}
|
||
|
||
|
||
public void ImportC4DFlytoTask(string txt)
|
||
{
|
||
string[] lines = txt.Replace("\r\n", "\n").Split('\n');
|
||
|
||
if (lines.Length % (_copterManager.Copters.Count + 1) != 0)
|
||
{
|
||
Alert.Show("文件内容错误！");
|
||
return;
|
||
}
|
||
|
||
int taskCount = lines.Length / (_copterManager.Copters.Count + 1);
|
||
int startIndex;
|
||
string line;
|
||
Dictionary<int, string[]> PointDic;
|
||
for (int i = 0; i < taskCount; i++)
|
||
{
|
||
startIndex = i * (_copterManager.Copters.Count + 1);
|
||
string taskName = lines[startIndex];
|
||
PointDic = new Dictionary<int, string[]>();
|
||
for (int j = 0; j < _copterManager.Copters.Count; j++)
|
||
{
|
||
line = lines[startIndex + j + 1];
|
||
string[] parameters = line.Split(' ');
|
||
int id = Convert.ToInt32(parameters[0]);
|
||
string frame = parameters[1];
|
||
string[] point = new string[3];
|
||
point[0] = parameters[1]; //左右 经度
|
||
point[1] = parameters[2]; //上下 高度
|
||
point[2] = parameters[3]; //前后 纬度
|
||
PointDic.Add(id, point);
|
||
}
|
||
|
||
var lastTask = Tasks.LastOrDefault();
|
||
var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = true, FlytoTime = 10, LoiterTime = 1 };
|
||
newTask.TaskCnName = taskName;
|
||
for (int k = 0; k < PointDic.Count; k++)
|
||
{
|
||
string[] point = PointDic[k + 1];
|
||
|
||
//string frame = parameters[1];
|
||
string x = point[0]; //左右 经度
|
||
string y = point[1]; //上下 高度
|
||
string z = point[2]; //前后 纬度
|
||
Tuple<double, double> observationLatLng = null;
|
||
observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
|
||
OriginLat,
|
||
OriginLng,
|
||
90,
|
||
Convert.ToSingle(x) / 100);
|
||
|
||
observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
|
||
observationLatLng.Item1,
|
||
observationLatLng.Item2,
|
||
0,
|
||
Convert.ToSingle(z) / 100);
|
||
|
||
var thisSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
|
||
_copterManager.Copters[k], new LatLng(observationLatLng.Item1 - OriginLat, observationLatLng.Item2 - OriginLng),
|
||
Convert.ToSingle(y) / 100, false);
|
||
newTask.SingleCopterInfos.Add(thisSingleCopterInfo);
|
||
|
||
}
|
||
Tasks.Add(newTask);
|
||
RaiseTaskAdded(lastTask, newTask);
|
||
}
|
||
// Alert.Show("导入完成！");
|
||
/*
|
||
Dictionary<int, string[]> PointDic = new Dictionary<int, string[]>();
|
||
foreach (string line in lines)
|
||
{
|
||
string[] parameters = line.Split(' ');
|
||
int id = Convert.ToInt32(parameters[0]);
|
||
string frame = parameters[1];
|
||
string[] point = new string[3];
|
||
point[0] = parameters[1]; //左右 经度
|
||
point[1] = parameters[2]; //上下 高度
|
||
point[2] = parameters[3]; //前后 纬度
|
||
PointDic.Add(id, point);
|
||
}
|
||
|
||
|
||
var lastTask = Tasks.LastOrDefault();
|
||
var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = true, FlytoTime = 10, LoiterTime = 10 };
|
||
for (int i = 0; i < PointDic.Count; i++)
|
||
{
|
||
string[] point = PointDic[i + 1];
|
||
|
||
//string frame = parameters[1];
|
||
string x = point[0]; //左右 经度
|
||
string y = point[1]; //上下 高度
|
||
string z = point[2]; //前后 纬度
|
||
Tuple<double, double> observationLatLng = null;
|
||
observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
|
||
OriginLat,
|
||
OriginLng,
|
||
90,
|
||
Convert.ToSingle(x) / 100);
|
||
|
||
observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
|
||
observationLatLng.Item1,
|
||
observationLatLng.Item2,
|
||
0,
|
||
Convert.ToSingle(z) / 100);
|
||
|
||
var thisSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
|
||
_copterManager.Copters[i], new LatLng(observationLatLng.Item1 - OriginLat, observationLatLng.Item2 - OriginLng),
|
||
Convert.ToSingle(y)/100, false);
|
||
newTask.SingleCopterInfos.Add(thisSingleCopterInfo);
|
||
|
||
}
|
||
Tasks.Add(newTask);
|
||
TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask });
|
||
*/
|
||
|
||
}
|
||
|
||
public void ImportBlenderFlyToTask(string blenderVectors)
|
||
{
|
||
string[] lineVectors = blenderVectors.Replace("\r\n","\n").Split('\n');
|
||
|
||
var lastTask = Tasks.LastOrDefault();
|
||
float alt = lastTask.SingleCopterInfos[0].TargetAlt;
|
||
double lat = lastTask.SingleCopterInfos[0].LatLngOffset.Lat;
|
||
double lng = lastTask.SingleCopterInfos[0].LatLngOffset.Lng;
|
||
|
||
|
||
foreach (var item in lineVectors)
|
||
{
|
||
string vector = item.Replace("Vector((", "").Replace("))", "");
|
||
var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = true, FlytoTime = 1, LoiterTime = 0 };
|
||
|
||
string[] vectors = vector.Split(',');
|
||
double x = double.Parse(vectors[1]);
|
||
double y = double.Parse(vectors[0]);
|
||
int x_directionDegrees = x > 0 ? 90 : -90;
|
||
int y_directionDegrees = y > 0 ? 0 : 180;
|
||
|
||
Tuple<double, double> observationLatLng = null;
|
||
observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
|
||
lat,
|
||
lng,
|
||
90,
|
||
(float)x);
|
||
|
||
observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
|
||
observationLatLng.Item1,
|
||
observationLatLng.Item2,
|
||
0,
|
||
(float)y);
|
||
|
||
|
||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
|
||
_copterManager.Copters[0], new LatLng((double)observationLatLng.Item1,
|
||
(double)observationLatLng.Item2), alt, false);
|
||
|
||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
Tasks.Add(newTask);
|
||
RaiseTaskAdded(lastTask, newTask);
|
||
Message.Show(vector);
|
||
}
|
||
}
|
||
|
||
public void RestoreFlyToTask(bool staggerRoutes, int flytoTime, int loiterTime, string taskName, dynamic singleCopterInfos, bool isMeter)
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
float tagalt = 15;
|
||
if (!copters.Any()) return;
|
||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
||
var lastTask = Tasks.LastOrDefault();
|
||
var nullableCenter = copters.GetCenter();
|
||
if (nullableCenter == null) return;
|
||
var center = nullableCenter.Value;
|
||
var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = staggerRoutes, FlytoTime = flytoTime, LoiterTime = loiterTime };
|
||
if(taskName != null) newTask.TaskCnName = taskName;
|
||
// TODO: 林俊清, 20150801, 处理实际飞行器数目与记录中数目不一致的情况。
|
||
for (int i = 0; i < copters.Count; i++)
|
||
{
|
||
var copter = copters[i];
|
||
FlightTaskSingleCopterInfo newSingleCopterInfo;
|
||
if (i < singleCopterInfos.Count)
|
||
{
|
||
var singleCopterInfoObj = singleCopterInfos[i];
|
||
tagalt = (float)singleCopterInfoObj.targetAlt;
|
||
if (isMeter)
|
||
newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
|
||
copter,
|
||
(double)singleCopterInfoObj.x,
|
||
(double)singleCopterInfoObj.y,
|
||
(float)singleCopterInfoObj.targetAlt - copter.GroundAlt,
|
||
(bool)singleCopterInfoObj.isLandWaypoint);
|
||
else
|
||
newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
|
||
copter,
|
||
new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset),
|
||
(float)singleCopterInfoObj.targetAlt - copter.GroundAlt, (bool)singleCopterInfoObj.isLandWaypoint);
|
||
|
||
var jsonArray = singleCopterInfoObj.ledInfos as Newtonsoft.Json.Linq.JArray;
|
||
ObservableCollection<LEDInfo> ledList = jsonArray.ToObject<ObservableCollection<LEDInfo>>();
|
||
foreach(LEDInfo info in ledList)
|
||
{
|
||
newSingleCopterInfo.AddLEDInfo(info);
|
||
}
|
||
if (singleCopterInfoObj.isChangeSpeed != null)
|
||
{
|
||
bool isChangeSpeed = (bool)singleCopterInfoObj.isChangeSpeed;
|
||
if (isChangeSpeed)
|
||
{
|
||
newSingleCopterInfo.IsChangeSpeed = isChangeSpeed;
|
||
newSingleCopterInfo.LevelSpeed = (float)singleCopterInfoObj.levelSpeed;
|
||
newSingleCopterInfo.UpSpeed = (float)singleCopterInfoObj.upSpeed;
|
||
newSingleCopterInfo.DownSpeed = (float)singleCopterInfoObj.downSpeed;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//实际飞机比保存的任务计划的飞机多，多的飞机设置默认航点
|
||
newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask
|
||
(copter, (double)copter.Latitude, (double)copter.Longitude, tagalt);
|
||
|
||
}
|
||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
}
|
||
Tasks.Add(newTask);
|
||
RaiseTaskAdded(lastTask, newTask);
|
||
}
|
||
|
||
|
||
private void RestoreTakeOffTask(byte takeOffTime, dynamic singleCopterInfos)
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
if (copters == null || !copters.Any()) return;
|
||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
||
var lastTask = Tasks.LastOrDefault();
|
||
FlightTask takeOffTask = Tasks[0];
|
||
takeOffTask.TakeOffTime = takeOffTime;
|
||
for (int i = 0; i < copters.Count; i++)
|
||
{
|
||
var singleCopterInfoObj = singleCopterInfos[i];
|
||
takeOffTask.SingleCopterInfos[i].TakeOffWaitTime = (ushort)singleCopterInfoObj.waitTime;
|
||
if (singleCopterInfoObj.ledInfos!= null)
|
||
{
|
||
var jsonArray = singleCopterInfoObj.ledInfos as Newtonsoft.Json.Linq.JArray;
|
||
ObservableCollection<LEDInfo> ledList = jsonArray.ToObject<ObservableCollection<LEDInfo>>();
|
||
foreach (LEDInfo info in ledList)
|
||
{
|
||
takeOffTask.SingleCopterInfos[i].AddLEDInfo(info);
|
||
}
|
||
}
|
||
|
||
//Message.Show(((ushort)singleCopterInfoObj.waitTime).ToString());
|
||
}
|
||
}
|
||
|
||
private void RestoreLandTask(dynamic singleCopterInfos)
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
if (copters == null || !copters.Any()) return;
|
||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
||
var lastTask = Tasks.LastOrDefault();
|
||
|
||
var LandTask = new FlightTask(FlightTaskType.Land);
|
||
for (int i = 0; i < copters.Count; i++)
|
||
{
|
||
var copter = copters[i];
|
||
FlightTaskSingleCopterInfo newSingleCopterInfo;
|
||
var singleCopterInfoObj = singleCopterInfos[i];
|
||
newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForLandTask(copter, (int)singleCopterInfoObj.waitTime);
|
||
LandTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
}
|
||
Tasks.Add(LandTask);
|
||
RaiseTaskAdded(lastTask, LandTask);
|
||
|
||
}
|
||
|
||
|
||
|
||
//导出任务 (单位：米)
|
||
public IEnumerable<object> ExportTasksToMeter()
|
||
{
|
||
var tasks = Tasks.Select<FlightTask, object>(task =>
|
||
{
|
||
var type = task.TaskType;
|
||
switch (type)
|
||
{
|
||
case FlightTaskType.TakeOff:
|
||
return new {
|
||
type = type,
|
||
takeoffnumber = 1,// TakeOffNumAttr,
|
||
takeoffTime = task.TakeOffTime,
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
return new
|
||
{
|
||
waitTime = info.TakeOffWaitTime,
|
||
ledInfos = info.LEDInfos
|
||
};
|
||
})
|
||
};
|
||
case FlightTaskType.FlyTo:
|
||
return new
|
||
{
|
||
type = type,
|
||
staggerRoutes = task.StaggerRoutes,
|
||
flytoTime = task.FlytoTime,
|
||
loiterTime = task.LoiterTime,
|
||
taskname = task.TaskCnName,
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
var offset = info.LatLngOffset;
|
||
return new
|
||
{
|
||
x = info.X,
|
||
y = info.Y,
|
||
targetAlt = info.TargetAlt + info.Copter.GroundAlt, //导出时加上地面摆放高度，导入后若没有摆放高度，模拟就会清楚的看到高度错误
|
||
//showLED = info.FlytoShowLED,
|
||
ledInfos = info.LEDInfos,
|
||
isLandWaypoint = info.IsLandWaypoint,
|
||
isChangeSpeed = info.IsChangeSpeed,
|
||
levelSpeed = info.LevelSpeed,
|
||
upSpeed = info.UpSpeed,
|
||
downSpeed = info.DownSpeed
|
||
};
|
||
})
|
||
};
|
||
|
||
|
||
case FlightTaskType.Land:
|
||
{
|
||
return new
|
||
{
|
||
type = type,
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
return new
|
||
{
|
||
waitTime = info.LandWaitTime
|
||
};
|
||
})
|
||
|
||
};
|
||
}
|
||
default:
|
||
throw new NotImplementedException(type + " task export not implemented.");
|
||
}
|
||
});
|
||
return tasks;
|
||
}
|
||
|
||
//导出任务
|
||
public IEnumerable<object> ExportTasks()
|
||
{
|
||
// For reference.
|
||
//var tasks = new object[]
|
||
//{
|
||
// new
|
||
// {
|
||
// type = FlightTaskType.TakeOff,
|
||
// },
|
||
// new
|
||
// {
|
||
// type = FlightTaskType.FlyTo,
|
||
// staggerRoutes = false,
|
||
// singleCopterInfos = new object[] {
|
||
// new {
|
||
// latOffset = 0.0001,
|
||
// lngOffset = 0.0001,
|
||
// targetAlt = 10
|
||
// },
|
||
// new {
|
||
// latOffset = 0.0003,
|
||
// lngOffset = 0.0003,
|
||
// targetAlt = 15
|
||
// }
|
||
// }
|
||
// },
|
||
// new
|
||
// {
|
||
// type = FlightTaskType.Turn,
|
||
// singleCopterInfos = new object[] {
|
||
// new {
|
||
// latOffset = 0.0001,
|
||
// lngOffset = 0.0001,
|
||
// targetAlt = 10,
|
||
// targetHeading = 90
|
||
// },
|
||
// new {
|
||
// latOffset = 0.0001,
|
||
// lngOffset = 0.0001,
|
||
// targetAlt = 10,
|
||
// targetHeading = 270
|
||
// }
|
||
// }
|
||
// },
|
||
// new
|
||
// {
|
||
// type = FlightTaskType.Circle,
|
||
// singleCopterInfos = new object[] {
|
||
// new {
|
||
// latOffset = 0.0001,
|
||
// lngOffset = 0.0001,
|
||
// targetAlt = 10,
|
||
// centerDirectionDeg = 90,
|
||
// radius = 1000,
|
||
// rate = 20,
|
||
// turns = 1,
|
||
// channel3 = 1500
|
||
// },
|
||
// new {
|
||
// latOffset = 0.0003,
|
||
// lngOffset = 0.0003,
|
||
// targetAlt = 15,
|
||
// centerDirectionDeg = 270,
|
||
// radius = 1000,
|
||
// rate = 20,
|
||
// turns = 1,
|
||
// channel3 = 1500
|
||
// }
|
||
// }
|
||
// },
|
||
// new
|
||
// {
|
||
// type = FlightTaskType.SimpleCircle,
|
||
// singleCopterInfos = new object[] {
|
||
// new {
|
||
// latOffset = 0.0001,
|
||
// lngOffset = 0.0001,
|
||
// targetAlt = 10,
|
||
// rate = 20,
|
||
// turns = 1,
|
||
// channel3 = 1500
|
||
// },
|
||
// new {
|
||
// latOffset = 0.0003,
|
||
// lngOffset = 0.0003,
|
||
// targetAlt = 15,
|
||
// rate = 20,
|
||
// turns = 1,
|
||
// channel3 = 1500
|
||
// }
|
||
// }
|
||
// }
|
||
//};
|
||
var tasks = Tasks.Select<FlightTask, object>(task =>
|
||
{
|
||
var type = task.TaskType;
|
||
switch (type)
|
||
{
|
||
case FlightTaskType.TakeOff:
|
||
return new {
|
||
type = type,
|
||
takeoffnumber = 1,//TakeOffNumAttr,
|
||
takeoffTime = task.TakeOffTime,
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
return new
|
||
{
|
||
waitTime = info.TakeOffWaitTime,
|
||
ledInfos = info.LEDInfos
|
||
};
|
||
})
|
||
};
|
||
case FlightTaskType.FlyTo:
|
||
return new
|
||
{
|
||
type = type,
|
||
staggerRoutes = task.StaggerRoutes,
|
||
flytoTime = task.FlytoTime,
|
||
loiterTime = task.LoiterTime,
|
||
taskname = task.TaskCnName,
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
var offset = info.LatLngOffset;
|
||
return new
|
||
{
|
||
latOffset = offset.Lat,
|
||
lngOffset = offset.Lng,
|
||
targetAlt = info.TargetAlt + info.Copter.GroundAlt, //导出时加上地面摆放高度，导入后若没有摆放高度，模拟就会清楚的看到高度错误
|
||
//showLED = info.FlytoShowLED,
|
||
ledInfos = info.LEDInfos,
|
||
isLandWaypoint = info.IsLandWaypoint,
|
||
isChangeSpeed = info.IsChangeSpeed,
|
||
levelSpeed = info.LevelSpeed,
|
||
upSpeed = info.UpSpeed,
|
||
downSpeed = info.DownSpeed
|
||
};
|
||
})
|
||
};
|
||
|
||
|
||
case FlightTaskType.Land:
|
||
{
|
||
return new
|
||
{
|
||
type = type,
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
return new
|
||
{
|
||
waitTime = info.LandWaitTime
|
||
};
|
||
})
|
||
|
||
};
|
||
}
|
||
default:
|
||
throw new NotImplementedException(type + " task export not implemented.");
|
||
}
|
||
});
|
||
return tasks;
|
||
}
|
||
|
||
|
||
//导入任务,可设置导入哪些步骤
|
||
public void ImportTasksindex(dynamic tasks, int startindex,int endindex, bool isMeter)
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
|
||
if (Tasks.Count == 0)
|
||
AddTakeOffTask(copters);
|
||
|
||
|
||
int i =1;
|
||
foreach (var task in tasks)
|
||
{
|
||
if ((i >= startindex)&& (i <= endindex))
|
||
{
|
||
switch ((FlightTaskType)task.type)
|
||
{
|
||
case FlightTaskType.TakeOff:
|
||
// AddTakeOffTask(copters); // added by ZJF
|
||
// TakeOffNumAttr = task.takeoffnumber;
|
||
// TakeOffNumAttr= task.takeoffnumber;
|
||
RestoreTakeOffTask((byte)task.takeoffTime, task.singleCopterInfos);
|
||
break;
|
||
case FlightTaskType.FlyTo:
|
||
RestoreFlyToTask((bool)task.staggerRoutes, (int)task.flytoTime, (int)task.loiterTime, (string)task.taskname, task.singleCopterInfos, isMeter);
|
||
break;
|
||
case FlightTaskType.Land:
|
||
RestoreLandTask(task.singleCopterInfos);
|
||
break;
|
||
}
|
||
}
|
||
i++;
|
||
}
|
||
}
|
||
|
||
|
||
//计算中心点 (三维的)
|
||
private Point3D CenterFormMeter(List<Point3D> point1, List<Point3D> point2)
|
||
{
|
||
Point3D centerPoint = new Point3D(0, 0, 0);
|
||
if (point1.Count != point2.Count) return centerPoint;
|
||
|
||
double minX = point1[0].X;
|
||
double maxX = point1[0].X;
|
||
|
||
double minY = point1[0].Y;
|
||
double maxY = point1[0].Y;
|
||
|
||
|
||
double minZ = point1[0].Z;
|
||
double maxZ = point1[0].Z;
|
||
|
||
|
||
|
||
int count = point1.Count;
|
||
for (int i = 0; i < count; i++)
|
||
{
|
||
minX = Math.Min(minX, Math.Min(point1[i].X, point2[i].X));
|
||
maxX = Math.Max(maxX, Math.Min(point1[i].X, point2[i].X));
|
||
|
||
minY = Math.Min(minY, Math.Min(point1[i].Y, point2[i].Y));
|
||
maxY = Math.Max(maxY, Math.Max(point1[i].Y, point2[i].Y));
|
||
|
||
minZ = Math.Min(minZ, Math.Min(point1[i].Z, point2[i].Z));
|
||
maxZ = Math.Max(maxZ, Math.Max(point1[i].Z, point2[i].Z));
|
||
|
||
|
||
}
|
||
centerPoint.X = (minX + maxX) / 2;
|
||
centerPoint.Y = (minY + maxY) / 2;
|
||
centerPoint.Z = (minZ + maxZ) / 2;
|
||
return centerPoint;
|
||
}
|
||
|
||
|
||
public double SumFlyLines(List<FlightTaskSingleCopterInfo> preinfos, List<FlightTaskSingleCopterInfo> currinfos)
|
||
{
|
||
double sumdis = 0;
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
//当前任务
|
||
Point3D preLoc = new Point3D(preinfos[i].X, preinfos[i].Y, preinfos[i].TargetAlt);
|
||
Point3D curLoc = new Point3D(currinfos[i].X, currinfos[i].Y, currinfos[i].TargetAlt);
|
||
//算距离
|
||
sumdis += Point3D.Subtract(curLoc, preLoc).Length;
|
||
}
|
||
return sumdis;
|
||
}
|
||
public void SetAllTaskFlytime()
|
||
{
|
||
for (int taskIndex = 1; taskIndex < Tasks.Count; taskIndex++)
|
||
SetTaskFlytime(taskIndex);
|
||
}
|
||
|
||
|
||
public void SetTaskFlytime(int taskIndex,bool settime=true)
|
||
{
|
||
if ((taskIndex > 0 )&&(taskIndex < Tasks.Count) &&( Tasks[taskIndex].TaskType == FlightTaskType.FlyTo))
|
||
{
|
||
if (_copterManager.Copters.Count() > 0)
|
||
{
|
||
double maxDistance = 0.0f;
|
||
string copterName = "";
|
||
double speed = 0.0f;
|
||
foreach (var copter in _copterManager.Copters)
|
||
{
|
||
var prevWaypoint = Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
|
||
var curWaypoint = Tasks[taskIndex].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
|
||
if ((curWaypoint.IsChangeSpeed) && (curWaypoint.LevelSpeed == -1))
|
||
{
|
||
Message.Show($"任务{taskIndex + 1}飞机编号:{copter.Id}是自动速度，无法计算时间!");
|
||
return ;
|
||
}
|
||
double distance = GeographyUtils.CalcDistance(
|
||
prevWaypoint.TargetLat, prevWaypoint.TargetLng, prevWaypoint.TargetAlt,
|
||
curWaypoint.TargetLat, curWaypoint.TargetLng, curWaypoint.TargetAlt);
|
||
if (distance > maxDistance)
|
||
{
|
||
maxDistance = distance;
|
||
copterName = copter.Name;
|
||
speed = curWaypoint.LevelSpeed;
|
||
}
|
||
}
|
||
|
||
double time = CalculateFlyIime(maxDistance, speed);
|
||
Message.Show($"任务{taskIndex+1}最大航点间距 = {Math.Round(maxDistance, 2)}米, 水平速度={Math.Round(speed, 2)}, 飞行时间 = {Math.Round(time, 2)}秒, 飞机编号:{copterName}");
|
||
if (settime)
|
||
Tasks[taskIndex].FlytoTime = (int)Math.Round(time, 2);
|
||
}
|
||
}
|
||
|
||
return ;
|
||
}
|
||
|
||
|
||
public static double CalculateFlyIime(double s, double v)
|
||
{
|
||
double t;
|
||
double a = 1; //加速度1米每秒
|
||
double at = v / a;
|
||
double a_s = 0.5f * a * at * at;
|
||
if (a_s > (s / 2)) //还没到特定速度就到了中点了
|
||
{
|
||
t = (float)System.Math.Sqrt(s / a) * 2;
|
||
}
|
||
else
|
||
{
|
||
t = (s - a_s * 2) / v + at * 2;
|
||
}
|
||
|
||
return t;
|
||
} //计算优化线路，采用米计算
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="Is3d">3D计算</param>
|
||
/// <param name="Ischange">是否改变线路的结束点顺序--返回起飞点航线不能交换</param>
|
||
public void OptimizeRouteMeter(bool Is3d=false,bool Ischange=true)
|
||
{
|
||
Dictionary<int, Point3D> curTaskPoint = new Dictionary<int, Point3D>();
|
||
Dictionary<int, Point3D> prevTaskPoint = new Dictionary<int, Point3D>();
|
||
var stopWatch = new Stopwatch();
|
||
stopWatch.Start();
|
||
//获取当前航点与前一航点所有经纬高
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
//当前任务
|
||
var curinfo = SelectedTask.SingleCopterInfos[i];
|
||
Point3D curLoc = new Point3D(curinfo.X, curinfo.Y, curinfo.TargetAlt);
|
||
curTaskPoint.Add(i, curLoc);
|
||
|
||
//前一任务
|
||
var prevInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i];
|
||
Point3D prevLoc = new Point3D(prevInfo.X, prevInfo.Y, prevInfo.TargetAlt);
|
||
prevTaskPoint.Add(i, prevLoc);
|
||
}
|
||
//Dictionary<int, Point3D> RouteRes = Util.OptimizeRoute.GenRoute(curTaskPoint, prevTaskPoint);
|
||
if (Is3d)
|
||
{
|
||
ArrayList resarray = Util.OptimizeRoute.Gen3DRoute(curTaskPoint.Values.ToArray(), prevTaskPoint.Values.ToArray());
|
||
Vector3[] RouteRes;
|
||
//最终
|
||
RouteRes = (Vector3[])resarray[0];
|
||
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
SelectedTask.SingleCopterInfos[i].X = RouteRes[i].x;
|
||
SelectedTask.SingleCopterInfos[i].Y = RouteRes[i].y;
|
||
SelectedTask.SingleCopterInfos[i].TargetAlt = (float)RouteRes[i].z;
|
||
}
|
||
|
||
SetTaskFlytime(SelectedTaskIndex);
|
||
|
||
|
||
}
|
||
else
|
||
{
|
||
ArrayList resarray = Util.OptimizeRoute.Gen2DRoute(curTaskPoint.Values.ToArray(), prevTaskPoint.Values.ToArray(), Ischange);
|
||
Vector3[] RouteRes;
|
||
//有错层，需要插入2个错层任务
|
||
if (resarray.Count == 3)
|
||
{
|
||
//选中前一个任务，插入错层
|
||
SelectTask(SelectedTaskIndex - 1);
|
||
AddTask();
|
||
|
||
//第一个错层
|
||
RouteRes = (Vector3[])resarray[0];
|
||
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
SelectedTask.SingleCopterInfos[i].X = RouteRes[i].x;
|
||
SelectedTask.SingleCopterInfos[i].Y = RouteRes[i].y;
|
||
SelectedTask.SingleCopterInfos[i].TargetAlt = (float)RouteRes[i].z;
|
||
}
|
||
SetTaskFlytime(SelectedTaskIndex);
|
||
|
||
|
||
//第二个错层
|
||
AddTask();
|
||
|
||
RouteRes = (Vector3[])resarray[1];
|
||
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
SelectedTask.SingleCopterInfos[i].X = RouteRes[i].x;
|
||
SelectedTask.SingleCopterInfos[i].Y = RouteRes[i].y;
|
||
SelectedTask.SingleCopterInfos[i].TargetAlt = (float)RouteRes[i].z;
|
||
}
|
||
|
||
SetTaskFlytime(SelectedTaskIndex);
|
||
|
||
|
||
SelectTask(SelectedTaskIndex + 1);
|
||
|
||
//最终
|
||
RouteRes = (Vector3[])resarray[2];
|
||
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
SelectedTask.SingleCopterInfos[i].X = RouteRes[i].x;
|
||
SelectedTask.SingleCopterInfos[i].Y = RouteRes[i].y;
|
||
SelectedTask.SingleCopterInfos[i].TargetAlt = (float)RouteRes[i].z;
|
||
}
|
||
SetTaskFlytime(SelectedTaskIndex);
|
||
|
||
|
||
}
|
||
else
|
||
{
|
||
//最终
|
||
RouteRes = (Vector3[])resarray[0];
|
||
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
SelectedTask.SingleCopterInfos[i].X = RouteRes[i].x;
|
||
SelectedTask.SingleCopterInfos[i].Y = RouteRes[i].y;
|
||
SelectedTask.SingleCopterInfos[i].TargetAlt = (float)RouteRes[i].z;
|
||
}
|
||
SetTaskFlytime(SelectedTaskIndex);
|
||
|
||
}
|
||
}
|
||
stopWatch.Stop();
|
||
|
||
double sumlength = SumFlyLines(Tasks[SelectedTaskIndex - 1].SingleCopterInfos, SelectedTask.SingleCopterInfos);
|
||
|
||
Message.Show($"计算用时：{stopWatch.Elapsed.TotalMilliseconds}ms,总飞行距离{sumlength}米");
|
||
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
|
||
|
||
//计算优化线路，采用经纬度计算
|
||
public void OptimizeRouteNew()
|
||
{
|
||
Dictionary<int, PLLocation> curTaskPoint = new Dictionary<int, PLLocation>();
|
||
Dictionary<int, PLLocation> prevTaskPoint = new Dictionary<int, PLLocation>();
|
||
|
||
var stopWatch = new Stopwatch();
|
||
stopWatch.Start();
|
||
//获取当前航点与前一航点所有经纬高
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
//当前任务
|
||
var curinfo = SelectedTask.SingleCopterInfos[i];
|
||
PLLocation curLoc = new PLLocation(curinfo.TargetLat, curinfo.TargetLng, curinfo.TargetAlt);
|
||
curTaskPoint.Add(i, curLoc);
|
||
|
||
//前一任务
|
||
var prevInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i];
|
||
PLLocation prevLoc = new PLLocation(prevInfo.TargetLat, prevInfo.TargetLng, prevInfo.TargetAlt);
|
||
prevTaskPoint.Add(i, prevLoc);
|
||
}
|
||
|
||
|
||
// int sss = Plane.AutoLine.CalAutoLine(curTaskPoint, prevTaskPoint);
|
||
|
||
// Message.Show($"中心点：{sss}");
|
||
|
||
double farDistance;
|
||
double nearDistance;
|
||
int index;
|
||
|
||
Dictionary<int, PLLocation> recordLatLng = new Dictionary<int, PLLocation>();
|
||
while (curTaskPoint.Count > 0)
|
||
{
|
||
farDistance = double.MinValue;
|
||
PLLocation centerLatLng = CenterLatLng(curTaskPoint.Values.ToList(), prevTaskPoint.Values.ToList());
|
||
index = 0;
|
||
//计算两个列表距离中心最远距离
|
||
// Message.Show($"有{curTaskPoint.Count}个点，当前中心点：{centerLatLng.Longitude},{centerLatLng.Latitude},{centerLatLng.Altitude}");
|
||
double distance1;
|
||
bool farIsCurTaskPoint = true;
|
||
|
||
foreach (var item in curTaskPoint)
|
||
{
|
||
distance1 =Math.Round(GeographyUtils.CalcDistance(centerLatLng,item.Value),4);
|
||
if (distance1 > farDistance)
|
||
{
|
||
index = item.Key;
|
||
farDistance = distance1;
|
||
farIsCurTaskPoint = true;
|
||
// Message.Show($"调整距离{farDistance}是{farIsCurTaskPoint}的{index}");
|
||
|
||
}
|
||
}
|
||
|
||
foreach (var item in prevTaskPoint)
|
||
{
|
||
distance1 = Math.Round(GeographyUtils.CalcDistance(centerLatLng,item.Value),4);
|
||
if (distance1 > farDistance)
|
||
{
|
||
index = item.Key;
|
||
farDistance = distance1;
|
||
farIsCurTaskPoint = false;
|
||
// Message.Show($"调整距离{farDistance}是{farIsCurTaskPoint}的{index}");
|
||
|
||
}
|
||
}
|
||
// Message.Show($"最远{farDistance}是{farIsCurTaskPoint}的{index}");
|
||
|
||
|
||
|
||
double tempDistance1;
|
||
nearDistance = double.MaxValue;
|
||
int nearIndex = 0;
|
||
if (farIsCurTaskPoint)
|
||
{
|
||
//最远的航点在当前任务
|
||
foreach (var item in prevTaskPoint)
|
||
{
|
||
tempDistance1 = Math.Round(GeographyUtils.CalcDistance(curTaskPoint[index],item.Value),4);
|
||
if (tempDistance1 < nearDistance)
|
||
{
|
||
nearDistance = tempDistance1;
|
||
nearIndex = item.Key;
|
||
}
|
||
}
|
||
//最远的航点在当前任务
|
||
// Message.Show($"最远是当前任务{index}距离它最近的前一任务点：ind={nearIndex},记录前一任务序号和当前任务位置");
|
||
|
||
recordLatLng.Add(nearIndex, curTaskPoint[index]);
|
||
curTaskPoint.Remove(index);
|
||
prevTaskPoint.Remove(nearIndex);
|
||
|
||
}
|
||
else
|
||
{
|
||
//最远的航点在前一任务
|
||
double prevLat = prevTaskPoint[index].Latitude;
|
||
double prevLng = prevTaskPoint[index].Longitude;
|
||
float prevAlt = prevTaskPoint[index].Altitude;
|
||
//最远的航点在当前任务
|
||
//Message.Show($"最远点在前一任务：ind={index},距离{farDistance}");
|
||
foreach (var item in curTaskPoint)
|
||
{
|
||
tempDistance1 = Math.Round(GeographyUtils.CalcDistance(prevTaskPoint[index],item.Value),4);
|
||
if (tempDistance1 < nearDistance)
|
||
{
|
||
nearDistance = tempDistance1;
|
||
nearIndex = item.Key;
|
||
}
|
||
}
|
||
// Message.Show($"最远是前一任务{index}距离它最近的当前任务点：ind={nearIndex},记录前一任务序号和当前任务位置");
|
||
recordLatLng.Add(index, curTaskPoint[nearIndex]);
|
||
curTaskPoint.Remove(nearIndex);
|
||
prevTaskPoint.Remove(index);
|
||
}
|
||
|
||
|
||
}
|
||
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
SelectedTask.SingleCopterInfos[i].TargetLat = recordLatLng[i].Latitude;
|
||
SelectedTask.SingleCopterInfos[i].TargetLng = recordLatLng[i].Longitude ;
|
||
SelectedTask.SingleCopterInfos[i].TargetAlt = recordLatLng[i].Altitude;
|
||
}
|
||
stopWatch.Stop();
|
||
double sumlength = SumFlyLines(Tasks[SelectedTaskIndex - 1].SingleCopterInfos, SelectedTask.SingleCopterInfos);
|
||
|
||
|
||
|
||
|
||
double Dist;
|
||
int crosscount = 0;
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
|
||
{
|
||
for (int j = i + 1; j < _copterManager.Copters.Count; j++)
|
||
{
|
||
|
||
|
||
Dist = Util.OptimizeRoute.SqureDistanceSegmentToSegment(
|
||
new Point3D(Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i].X,
|
||
Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i].Y,
|
||
Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i].TargetAlt),
|
||
|
||
new Point3D(Tasks[SelectedTaskIndex].SingleCopterInfos[i].X,
|
||
Tasks[SelectedTaskIndex].SingleCopterInfos[i].Y,
|
||
Tasks[SelectedTaskIndex].SingleCopterInfos[i].TargetAlt),
|
||
|
||
new Point3D(Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j].X,
|
||
Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j].Y,
|
||
Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j].TargetAlt),
|
||
|
||
new Point3D(Tasks[SelectedTaskIndex].SingleCopterInfos[j].X,
|
||
Tasks[SelectedTaskIndex].SingleCopterInfos[j].Y,
|
||
Tasks[SelectedTaskIndex].SingleCopterInfos[j].TargetAlt)
|
||
);
|
||
//交叉
|
||
if (Dist < 0.01)
|
||
{
|
||
crosscount++;
|
||
// Message.Show($"飞机{i}和飞机{j}有交叉，距离{Dist}");
|
||
|
||
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
Message.Show($"经纬方式计算用时：{stopWatch.Elapsed.TotalMilliseconds}ms,总飞行距离{sumlength}米,有{crosscount}个交叉");
|
||
|
||
}
|
||
|
||
//计算中心点 (三维的)
|
||
private PLLocation CenterLatLng(List<PLLocation> point1, List<PLLocation> point2)
|
||
{
|
||
PLLocation centerLatLng = new PLLocation(0,0,0);
|
||
if (point1.Count != point2.Count) return centerLatLng;
|
||
|
||
double minLat = point1[0].Latitude;
|
||
double maxLat = point1[0].Latitude;
|
||
|
||
double minLng = point1[0].Longitude;
|
||
double maxLng = point1[0].Longitude;
|
||
|
||
|
||
double minAlt = point1[0].Altitude;
|
||
double maxAlt = point1[0].Altitude;
|
||
|
||
|
||
|
||
int count = point1.Count;
|
||
for (int i = 0; i < count; i++)
|
||
{
|
||
minLat = Math.Min(minLat, Math.Min(point1[i].Latitude, point2[i].Latitude));
|
||
maxLat = Math.Max(maxLat, Math.Min(point1[i].Latitude, point2[i].Latitude));
|
||
|
||
minLng = Math.Min(minLng, Math.Min(point1[i].Longitude, point2[i].Longitude));
|
||
maxLng = Math.Max(maxLng, Math.Max(point1[i].Longitude, point2[i].Longitude));
|
||
|
||
minAlt = Math.Min(minAlt, Math.Min(point1[i].Altitude, point2[i].Altitude));
|
||
maxAlt = Math.Max(maxAlt, Math.Max(point1[i].Altitude, point2[i].Altitude));
|
||
|
||
|
||
}
|
||
centerLatLng.Latitude = (minLat + maxLat) / 2;
|
||
centerLatLng.Longitude = (minLng + maxLng) / 2;
|
||
centerLatLng.Altitude =(float)(minAlt + maxAlt) / 2;
|
||
return centerLatLng;
|
||
}
|
||
|
||
public void OptimizeRoute2()
|
||
{
|
||
double minLat = SelectedTask.SingleCopterInfos[0].TargetLat;
|
||
double maxLat = SelectedTask.SingleCopterInfos[0].TargetLat;
|
||
double minLng = SelectedTask.SingleCopterInfos[0].TargetLng;
|
||
double maxLng = SelectedTask.SingleCopterInfos[0].TargetLng;
|
||
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
var curinfo = SelectedTask.SingleCopterInfos[i];
|
||
//recordLatLng.Add(i, new LatLng(curinfo.TargetLat, curinfo.TargetLng));
|
||
var lastInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i];
|
||
minLat = Math.Min(minLat, Math.Min(curinfo.TargetLat, lastInfo.TargetLat));
|
||
maxLat = Math.Max(maxLat, Math.Max(curinfo.TargetLat, lastInfo.TargetLat));
|
||
|
||
minLng = Math.Min(minLng, Math.Min(curinfo.TargetLng, lastInfo.TargetLng));
|
||
maxLng = Math.Max(maxLng, Math.Max(curinfo.TargetLng, lastInfo.TargetLng));
|
||
}
|
||
double CenterLat = (maxLat + minLat) / 2;
|
||
double CenterLng = (maxLng + minLng) / 2;
|
||
|
||
Message.Show($"中心点：{CenterLat} {CenterLng}");
|
||
|
||
Dictionary<int, double> distanceDic = new Dictionary<int, double>();
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
var curinfo = SelectedTask.SingleCopterInfos[i];
|
||
var lastInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i];
|
||
|
||
|
||
double distance1 = GeographyUtils.CalcDistance(CenterLat, CenterLng, 1,
|
||
curinfo.TargetLat, curinfo.TargetLng, 1);
|
||
//int[] nums = new int[] { SelectedTaskIndex, i};
|
||
int nums = SelectedTaskIndex << 16 ^ i;
|
||
distanceDic.Add(nums, distance1);
|
||
double distance2 = GeographyUtils.CalcDistance(CenterLat, CenterLng, 1,
|
||
lastInfo.TargetLat, lastInfo.TargetLng, 1);
|
||
nums = (SelectedTaskIndex - 1) << 16 ^ i;
|
||
distanceDic.Add(nums, distance2);
|
||
}
|
||
distanceDic = distanceDic.OrderByDescending(o => o.Value).ToDictionary(p => p.Key, o => o.Value);
|
||
|
||
|
||
Dictionary<int, LatLng> recordLatLng = new Dictionary<int, LatLng>();
|
||
while (distanceDic.Count > 0)
|
||
{
|
||
KeyValuePair<int, double> kv = distanceDic.First();
|
||
int taskIndex = kv.Key >> 16;
|
||
int copterID = kv.Key & 0xffff;
|
||
var curInfo = Tasks[taskIndex].SingleCopterInfos[copterID];
|
||
if (taskIndex == SelectedTaskIndex)
|
||
{
|
||
double mindistance = double.MaxValue;
|
||
int index = 0;
|
||
for (int i = 0; i < Tasks[taskIndex - 1].SingleCopterInfos.Count; i++)
|
||
{
|
||
if (distanceDic.ContainsKey((taskIndex - 1) << 16 ^ i))
|
||
{
|
||
var destInfo = Tasks[taskIndex - 1].SingleCopterInfos[i];
|
||
double distance = GeographyUtils.CalcDistance(curInfo.TargetLat, curInfo.TargetLng, 1,
|
||
destInfo.TargetLat, destInfo.TargetLng, 1);
|
||
if (distance < mindistance)
|
||
{
|
||
mindistance = distance;
|
||
index = i;
|
||
}
|
||
}
|
||
}
|
||
recordLatLng.Add(index,
|
||
new LatLng(SelectedTask.SingleCopterInfos[copterID].TargetLat,
|
||
SelectedTask.SingleCopterInfos[copterID].TargetLng));
|
||
|
||
Message.Show($"航点{taskIndex} ID{copterID + 1} --航点{taskIndex - 1} id{index + 1}");
|
||
distanceDic.Remove(kv.Key);
|
||
distanceDic.Remove(( taskIndex - 1) << 16 ^ index );
|
||
}
|
||
else
|
||
{
|
||
double mindistance = double.MaxValue;
|
||
int index = 0;
|
||
for (int i = 0; i < Tasks[taskIndex + 1].SingleCopterInfos.Count; i++)
|
||
{
|
||
if (distanceDic.ContainsKey((taskIndex + 1) << 16 ^ i))
|
||
{
|
||
var destInfo = Tasks[taskIndex + 1].SingleCopterInfos[i];
|
||
double distance = GeographyUtils.CalcDistance(curInfo.TargetLat, curInfo.TargetLng, 1,
|
||
destInfo.TargetLat, destInfo.TargetLng, 1);
|
||
if (distance < mindistance && distanceDic.ContainsKey((SelectedTaskIndex) << 16 ^ i))
|
||
{
|
||
mindistance = distance;
|
||
index = i;
|
||
}
|
||
}
|
||
}
|
||
recordLatLng.Add(copterID,
|
||
new LatLng(SelectedTask.SingleCopterInfos[index].TargetLat,
|
||
SelectedTask.SingleCopterInfos[index].TargetLng));
|
||
|
||
Message.Show($"航点{taskIndex} ID{copterID + 1} --航点{taskIndex + 1} id{index + 1}");
|
||
distanceDic.Remove(kv.Key);
|
||
distanceDic.Remove(( taskIndex + 1) << 16 ^ index );
|
||
}
|
||
|
||
}
|
||
|
||
for (int i = 0; i < _copterManager.Copters.Count; i++)
|
||
{
|
||
SelectedTask.SingleCopterInfos[i].TargetLat = recordLatLng[i].Lat;
|
||
SelectedTask.SingleCopterInfos[i].TargetLng = recordLatLng[i].Lng;
|
||
}
|
||
|
||
}
|
||
public void OptimizeRoute()
|
||
{
|
||
Dictionary<int[], double> distanceDic = new Dictionary<int[], double>();
|
||
Dictionary<int, LatLng> recordLatLng = new Dictionary<int, LatLng>();
|
||
for (int i = 0; i < SelectedTask.SingleCopterInfos.Count; i++)
|
||
{
|
||
var curinfo = SelectedTask.SingleCopterInfos[i];
|
||
recordLatLng.Add(i, new LatLng(curinfo.TargetLat, curinfo.TargetLng));
|
||
for (int j = 0; j < Tasks[SelectedTaskIndex - 1].SingleCopterInfos.Count; j++)
|
||
{
|
||
var lastInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j];
|
||
|
||
double distance = GeographyUtils.CalcDistance(curinfo.TargetLat, curinfo.TargetLng, curinfo.TargetLng,
|
||
lastInfo.TargetLat, lastInfo.TargetLng, lastInfo.TargetLng);
|
||
int[] nums = new int[] {i,j};
|
||
distanceDic.Add(nums, distance);
|
||
}
|
||
}
|
||
distanceDic = distanceDic.OrderBy(o=>o.Value).ToDictionary(p=>p.Key,o=>o.Value);
|
||
List<int> movedCopters = new List<int>();
|
||
List<int> usedPoints = new List<int>();
|
||
foreach (KeyValuePair <int[], double> kv in distanceDic)
|
||
{
|
||
int moveCopterNum = kv.Key[1];
|
||
int destNum = kv.Key[0];
|
||
if (movedCopters.Contains(moveCopterNum) || usedPoints.Contains(destNum))
|
||
continue;
|
||
SelectedTask.SingleCopterInfos[moveCopterNum].TargetLat = recordLatLng[destNum].Lat;
|
||
SelectedTask.SingleCopterInfos[moveCopterNum].TargetLng = recordLatLng[destNum].Lng;
|
||
movedCopters.Add(moveCopterNum);
|
||
usedPoints.Add(destNum);
|
||
}
|
||
}
|
||
|
||
public void ImportWaypoint(string tasksText)
|
||
{
|
||
dynamic jsonfile = JsonConvert.DeserializeObject(tasksText);
|
||
dynamic points = jsonfile.points;
|
||
for (int i = 0; i < SelectedTask.SingleCopterInfos.Count; i++)
|
||
{
|
||
MapManager _mapManager = Microsoft.Practices.ServiceLocation.ServiceLocator.Current.GetInstance<MapManager>();
|
||
|
||
var pointjson= points[i];
|
||
System.Windows.Point point = new System.Windows.Point((int)pointjson.x, (int)pointjson.y);
|
||
var loc = _mapManager.MapView.gmap.FromLocalToLatLng((int)point.X , (int)point.Y);
|
||
SelectedTask.SingleCopterInfos[i].TargetLat = loc.Lat;
|
||
SelectedTask.SingleCopterInfos[i].TargetLng = loc.Lng;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// 导入任务
|
||
public void ImportTasks(dynamic tasks, bool isMeter)
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
|
||
if (Tasks.Count == 0)
|
||
AddTakeOffTask(copters);
|
||
|
||
foreach (var task in tasks)
|
||
{
|
||
switch ((FlightTaskType)task.type)
|
||
{
|
||
case FlightTaskType.TakeOff:
|
||
// AddTakeOffTask(copters); // added by ZJF
|
||
// TakeOffNumAttr = task.takeoffnumber;
|
||
RestoreTakeOffTask((byte)task.takeoffTime, task.singleCopterInfos);
|
||
break;
|
||
case FlightTaskType.FlyTo:
|
||
RestoreFlyToTask((bool)task.staggerRoutes, (int)task.flytoTime, (int)task.loiterTime, (string)task.taskname, task.singleCopterInfos, isMeter);
|
||
break;
|
||
|
||
case FlightTaskType.Land:
|
||
RestoreLandTask(task.singleCopterInfos);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
// private void ImportCoptersLocate(dynamic coptersLocate)
|
||
// {
|
||
//
|
||
// }
|
||
|
||
private void ImportGroupsInfo(dynamic coptersLocate)
|
||
{
|
||
|
||
}
|
||
|
||
public void SelectTask(int taskIndex)
|
||
{
|
||
if (taskIndex == -1) return;
|
||
this.SelectedTaskIndex = taskIndex;
|
||
this.SelectedTask = Tasks[taskIndex];
|
||
|
||
}
|
||
|
||
/**
|
||
* 在悬停任务时，右键waypoint为取消选中
|
||
*/
|
||
public void RightSelect(int taskIndex, ICopter copter)
|
||
{
|
||
this.SelectedTaskIndex = taskIndex;
|
||
this.SelectedTask = Tasks[taskIndex];
|
||
|
||
}
|
||
|
||
private int Name2Index(string name) // 获取指定copter名字对应的序号
|
||
{
|
||
int index = -1;
|
||
for (int i = 0; i < SelectedTask.SingleCopterInfos.Count; i++)
|
||
{
|
||
if (name.Equals(SelectedTask.SingleCopterInfos[i].Copter.Name))
|
||
{
|
||
return i;
|
||
}
|
||
}
|
||
return index;
|
||
}
|
||
|
||
private string DeleteSelectedName( string nameArray, string name ) // 删除字符串中的指定项
|
||
{
|
||
string copterStr = nameArray;
|
||
if (copterStr.Equals(""))
|
||
return "";
|
||
string[] copterNameArray = copterStr.Split(',');
|
||
copterNameArray = copterNameArray.Where(str => !str.Equals(name)).ToArray();
|
||
|
||
copterStr = "";
|
||
for (int i = 0; i < copterNameArray.Length; i++)
|
||
{
|
||
copterStr = copterStr + copterNameArray[i];
|
||
if (i < (copterNameArray.Length - 1))
|
||
{
|
||
copterStr = copterStr + ",";
|
||
}
|
||
}
|
||
return copterStr;
|
||
}
|
||
|
||
//左键选中任务
|
||
public void Select(FlightTask flightTask)
|
||
{
|
||
this.SelectedTaskIndex = Tasks.IndexOf(flightTask);
|
||
this.SelectedTask = flightTask;
|
||
|
||
|
||
|
||
|
||
}
|
||
|
||
// 右键选中任务
|
||
public void RightSelect(FlightTask flightTask)
|
||
{
|
||
int RightSelectedTaskIndexTmp = Tasks.IndexOf(flightTask);
|
||
if (this.RightSelectedTaskIndex == RightSelectedTaskIndexTmp)
|
||
{
|
||
this.RightSelectedTaskIndex = -RightSelectedTaskIndexTmp;
|
||
}
|
||
else
|
||
{
|
||
this.RightSelectedTaskIndex = RightSelectedTaskIndexTmp;
|
||
}
|
||
}
|
||
|
||
#region Run and pause.
|
||
|
||
private bool? _IsPaused;
|
||
public bool? IsPaused
|
||
{
|
||
get { return _IsPaused; }
|
||
private set {
|
||
if (Set(nameof(IsPaused), ref _IsPaused, value))
|
||
{
|
||
if (_IsPaused??false)
|
||
{
|
||
MessageText = "任务暂停！";
|
||
|
||
}else
|
||
{
|
||
MessageText = "任务运行中";
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
}
|
||
|
||
TimeSpan timeSpan;
|
||
DateTime taskStartTime;
|
||
public async Task RunTaskAsync()
|
||
{
|
||
if (Tasks.Count == 0) return;
|
||
if (CurrentRunningTaskIndex == 0)
|
||
{
|
||
taskStartTime = DateTime.Now;
|
||
Message.Show($"{DateTime.Now.ToString("HH:mm:ss")}：任务开始");
|
||
}
|
||
await RunAsync();
|
||
if ((IsPaused ?? false) == false)
|
||
{
|
||
timeSpan = DateTime.Now - taskStartTime;
|
||
Message.Show($"{DateTime.Now.ToString("HH:mm:ss")}：任务结束");
|
||
Message.Show($"总时长 = {timeSpan.Minutes}分{timeSpan.Seconds}秒");
|
||
|
||
}
|
||
}
|
||
|
||
public async Task RunAsync()
|
||
{
|
||
IsPaused = false;
|
||
for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++)
|
||
{
|
||
var task = Tasks[i];
|
||
//task.Status目前只用于任务条下面状态显示不同颜色
|
||
task.Status = FlightTaskStatus.Stop;
|
||
}
|
||
AppEx.Current.AppMode = AppMode.RunningTasks;
|
||
StartAvoidingCrash(); //开始碰撞检测
|
||
TaskState = TasksStatus.Running;
|
||
for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++)
|
||
{
|
||
var task = Tasks[i];
|
||
//task.Status目前只用于任务条下面状态显示不同颜色
|
||
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)
|
||
{
|
||
task.Status = FlightTaskStatus.Paused;
|
||
TaskState = TasksStatus.Paused;
|
||
Message.Show($"任务{i + 1} {task.TaskCnName } 暂停执行");
|
||
return;
|
||
}
|
||
task.Status = FlightTaskStatus.Stop;
|
||
}
|
||
|
||
// 2. 正常结束时，重置 CurrentRunningTask、CurrentRunningTaskIndex 和 IsPaused。
|
||
TaskState = TasksStatus.Stop;
|
||
CurrentRunningTask = null;
|
||
CurrentRunningTaskIndex = 0;
|
||
IsPaused = null;
|
||
}
|
||
|
||
public void Pause()
|
||
{
|
||
IsPaused = true;
|
||
}
|
||
|
||
#endregion Run and pause.
|
||
|
||
|
||
Dictionary<int, string> AvoidCrashLog = new Dictionary<int, string >();
|
||
|
||
|
||
|
||
|
||
|
||
private async void StartAvoidingCrash()
|
||
{
|
||
await Task.Factory.StartNew(AvoidCrashtoshow);
|
||
await Task.Factory.StartNew(LoopToAvoidCrash);
|
||
}
|
||
private async void AvoidCrashtoshow()
|
||
{
|
||
while (IsPaused == false)
|
||
{
|
||
try
|
||
{
|
||
foreach (KeyValuePair<int, string> kv in AvoidCrashLog)
|
||
{
|
||
string moveCopterNum = kv.Value;
|
||
int destNum = kv.Key;
|
||
Message.Show(moveCopterNum);
|
||
}
|
||
AvoidCrashLog.Clear(); //可能有丢失
|
||
}
|
||
catch // 林俊清, 20151102, 通常是“集合已修改”异常。
|
||
{
|
||
}
|
||
finally
|
||
{
|
||
await Task.Delay(100).ConfigureAwait(false);
|
||
}
|
||
}
|
||
}
|
||
private async void LoopToAvoidCrash()
|
||
{
|
||
while (IsPaused == false)
|
||
{
|
||
try
|
||
{
|
||
|
||
double distance;
|
||
foreach (var copter in _copterManager.Copters)
|
||
{
|
||
foreach (var anotherCopter in _copterManager.Copters)
|
||
{
|
||
if (copter != anotherCopter && copter.IsTooCloseTo(anotherCopter, out distance))
|
||
{
|
||
//防止卡顿,单加任务来显示
|
||
AvoidCrashLog.Add(1, $"{copter.Name} 与 {anotherCopter.Name} 距离过近，间距{distance}米。");
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
catch // 林俊清, 20151102, 通常是“集合已修改”异常。
|
||
{
|
||
}
|
||
finally
|
||
{
|
||
await Task.Delay(50).ConfigureAwait(false);
|
||
}
|
||
}
|
||
}
|
||
|
||
private void LoadIni()
|
||
{
|
||
Windows.IniHelper.IniFiles inifilse = new Windows.IniHelper.IniFiles();
|
||
string readvalue = "";
|
||
int intTemp;
|
||
float floatTemp;
|
||
readvalue = inifilse.IniReadvalue("Default", "ColumnCount");
|
||
if (readvalue!= "" && int.TryParse(readvalue,out intTemp))
|
||
ColumnCount = int.Parse(readvalue);
|
||
|
||
readvalue = inifilse.IniReadvalue("Default", "ColumnDistance");
|
||
if (readvalue != "" && float.TryParse(readvalue, out floatTemp))
|
||
ColumnDistance = float.Parse(readvalue);
|
||
|
||
readvalue = inifilse.IniReadvalue("Default", "RowDistance");
|
||
if (readvalue != "" && float.TryParse(readvalue, out floatTemp))
|
||
RowDistance = float.Parse(readvalue);
|
||
|
||
readvalue = inifilse.IniReadvalue("Default", "Orientation");
|
||
if (readvalue != "" && int.TryParse(readvalue, out intTemp))
|
||
Orientation = int.Parse(readvalue);
|
||
|
||
|
||
}
|
||
|
||
public void SaveIni()
|
||
{
|
||
Windows.IniHelper.IniFiles inifilse = new Windows.IniHelper.IniFiles();
|
||
inifilse.IniWritevalue("Default", "ColumnCount", ColumnCount.ToString());
|
||
inifilse.IniWritevalue("Default", "ColumnDistance", ColumnDistance.ToString());
|
||
inifilse.IniWritevalue("Default", "RowDistance", RowDistance.ToString());
|
||
inifilse.IniWritevalue("Default", "Orientation", Orientation.ToString());
|
||
}
|
||
}
|
||
|
||
public class FlightTaskAddedEventArgs : EventArgs
|
||
{
|
||
public FlightTask LastTask { get; set; }
|
||
public FlightTask AddedTask { get; set; }
|
||
}
|
||
|
||
public class FlightTaskDeledEventArgs : EventArgs
|
||
{
|
||
public FlightTask DeledTask { get; set; }
|
||
public int TaskIndex { get; set; }
|
||
|
||
}
|
||
|
||
public class FlightTaskTypeChangedEventArgs : EventArgs
|
||
{
|
||
public FlightTaskTypeChangedEventArgs(FlightTask changedFlightTask)
|
||
{
|
||
this.ChangedFlightTask = changedFlightTask;
|
||
}
|
||
public FlightTask ChangedFlightTask { get; set; }
|
||
|
||
}
|
||
|
||
|
||
public class FlightTaskcnNameChangedEventArgs : EventArgs
|
||
{
|
||
public FlightTaskcnNameChangedEventArgs(FlightTask changedFlightTask)
|
||
{
|
||
this.ChangedFlightTask = changedFlightTask;
|
||
}
|
||
public FlightTask ChangedFlightTask { get; set; }
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
public class FlightTaskAddedOriginalEventArgs : EventArgs
|
||
{
|
||
public double Lat { get; set; }
|
||
public double Lng { get; set; }
|
||
}
|
||
|
||
public class SingleCopterInfoChangedEventArgs : EventArgs
|
||
{
|
||
public SingleCopterInfoChangedEventArgs(FlightTaskSingleCopterInfo changedSingleCopterInfo)
|
||
{
|
||
this.ChangedSingleCopterInfo = changedSingleCopterInfo;
|
||
}
|
||
|
||
public FlightTaskSingleCopterInfo ChangedSingleCopterInfo { get; set; }
|
||
}
|
||
}
|