1094 lines
44 KiB
C#
1094 lines
44 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;
|
||
|
||
namespace Plane.FormationCreator.Formation
|
||
{
|
||
public class FlightTaskManager : ObservableObject
|
||
{
|
||
public FlightTaskManager(CopterManager copterManager)
|
||
{
|
||
_copterManager = copterManager;
|
||
|
||
//AddTakeOffTask(_copterManager.Copters);
|
||
|
||
_copterManager.Copters.CollectionChanged += (sender, e) =>
|
||
{
|
||
// TODO: 林俊清, 20150724, 需要改为正确的做法（清除旧飞机的任务，补充新飞机的任务）。
|
||
//AddTakeOffTask(e.NewItems?.Cast<ICopter>());
|
||
};
|
||
|
||
_copterManager.SelectedCoptersChanged += (sender, e) =>
|
||
{
|
||
// 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);
|
||
};
|
||
|
||
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 int _TakeOffNumAttr = 1;
|
||
public int TakeOffNumAttr
|
||
{
|
||
get { return _TakeOffNumAttr; }
|
||
set
|
||
{
|
||
Set(nameof(TakeOffNumAttr), ref _TakeOffNumAttr, value);
|
||
|
||
bool takeOffTaskExisted = Tasks.Count >= 1;
|
||
FlightTask takeOffTask;
|
||
if (takeOffTaskExisted)
|
||
{
|
||
takeOffTask = Tasks[0];
|
||
takeOffTask.TakeOffNumAttr = TakeOffNumAttr;
|
||
}
|
||
|
||
|
||
}
|
||
}
|
||
|
||
private string _MessageText="test";
|
||
public string MessageText
|
||
{
|
||
get { return _MessageText; }
|
||
set
|
||
{
|
||
if (Set(nameof(MessageText), ref _MessageText, value))
|
||
{
|
||
// _lastUpdateStatusTextTime = DateTime.Now;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
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);
|
||
takeOffTask.TakeOffNumAttr = TakeOffNumAttr;
|
||
Tasks.Add(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>();
|
||
|
||
private FlightTask _SelectedTask;
|
||
public FlightTask SelectedTask
|
||
{
|
||
get { return _SelectedTask; }
|
||
set
|
||
{
|
||
if (_SelectedTask != value)
|
||
{
|
||
if (_SelectedTask != null) _SelectedTask.IsSelected = false;
|
||
if (value != null) value.IsSelected = true;
|
||
}
|
||
Set(nameof(SelectedTask), ref _SelectedTask, value);
|
||
}
|
||
}
|
||
|
||
private int _SelectedTaskIndex;
|
||
public int SelectedTaskIndex
|
||
{
|
||
get { return _SelectedTaskIndex; }
|
||
set { Set(nameof(SelectedTaskIndex), ref _SelectedTaskIndex, 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 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<FlightTaskAddedOriginalEventArgs> OnOriginalSet;
|
||
|
||
public event EventHandler<SingleCopterInfoChangedEventArgs> SingleCopterInfoChanged;
|
||
|
||
public void RaiseSingleCopterTaskChanged(FlightTaskSingleCopterInfo singleCopterInfo)
|
||
{
|
||
SingleCopterInfoChanged?.Invoke(this, new SingleCopterInfoChangedEventArgs(singleCopterInfo));
|
||
}
|
||
|
||
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();
|
||
var nullableCenter = copters.GetCenter();
|
||
if (nullableCenter == null) return;
|
||
var center = nullableCenter.Value;
|
||
var newTask = new FlightTask(FlightTaskType.FlyTo);
|
||
int coptindex = 0;
|
||
|
||
int colnum = 5; //自动生成列数=4
|
||
float coldis = 5;//列相距5米
|
||
float rowdis = 5;//行相距5米
|
||
float matrixdis = 20; //生成方阵距离30米
|
||
|
||
|
||
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, 0, matrixdis);
|
||
colLatLng = targetLatLng;
|
||
}
|
||
else
|
||
{
|
||
if (currcol < colnum)
|
||
targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Item1, colLatLng.Item2, 90, currcol * coldis);
|
||
else
|
||
{
|
||
currrow++;
|
||
currcol = 0;
|
||
targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Item1, colLatLng.Item2, 180, rowdis);
|
||
colLatLng = targetLatLng;
|
||
}
|
||
}
|
||
currcol++;
|
||
coptindex++;
|
||
|
||
|
||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(copter, targetLatLng.Item1, targetLatLng.Item2, preSingleCopterInfo.TargetAlt);
|
||
newSingleCopterInfo.TargetHeading = lastSingleCopterInfo.TargetHeading;
|
||
newSingleCopterInfo.CenterDirectionDeg = lastSingleCopterInfo.TargetHeading;
|
||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
|
||
}
|
||
|
||
Tasks.Add(newTask);
|
||
try
|
||
{
|
||
TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask });
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
//RaiseExceptionThrown(ex);
|
||
}
|
||
|
||
SelectedTask = newTask;
|
||
SelectedTaskIndex = Tasks.Count - 1;
|
||
}
|
||
}
|
||
|
||
|
||
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;
|
||
TasksCleared?.Invoke(this, EventArgs.Empty);
|
||
AddTakeOffTask(_copterManager.Copters);
|
||
}
|
||
|
||
|
||
|
||
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 void ResetTasks()
|
||
{
|
||
Pause();
|
||
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;
|
||
}
|
||
}
|
||
|
||
// 将降落的阶段标志位都置位0
|
||
if (Tasks[i].TaskType == FlightTaskType.ReturnToLand)
|
||
{
|
||
var infos = Tasks[i].SingleCopterInfos;
|
||
for (int j = 0; j < infos.Count; j++)
|
||
{
|
||
var info = infos[j];
|
||
info.RTLStage = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
public void RestoreFlyToTask(bool staggerRoutes, int flytoTime, int loiterTime, dynamic singleCopterInfos)
|
||
{
|
||
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 };
|
||
|
||
// 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;
|
||
newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
|
||
copter, new LatLng((double)singleCopterInfoObj.latOffset,
|
||
(double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt);
|
||
|
||
var jsonArray = singleCopterInfoObj.ledInfos as Newtonsoft.Json.Linq.JArray;
|
||
newSingleCopterInfo.LEDInfos = jsonArray.ToObject<ObservableCollection<LEDInfo>>();
|
||
|
||
}
|
||
else
|
||
{
|
||
//实际飞机比保存的任务计划的飞机多，多的飞机设置默认航点
|
||
newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask
|
||
(copter, (double)copter.Latitude, (double)copter.Longitude, tagalt);
|
||
|
||
}
|
||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
}
|
||
Tasks.Add(newTask);
|
||
TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask });
|
||
SelectedTask = newTask;
|
||
SelectedTaskIndex = Tasks.Count - 1;
|
||
}
|
||
|
||
private void RestoreTurnTask(dynamic singleCopterInfos)
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
if (!copters.Any()) return;
|
||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
||
var lastTask = Tasks.LastOrDefault();
|
||
var newTask = new FlightTask(FlightTaskType.Turn);
|
||
// TODO: 林俊清, 20150806, 处理实际飞行器数目与记录中数目不一致的情况。
|
||
for (int i = 0; i < copters.Count && i < singleCopterInfos.Count; i++)
|
||
{
|
||
var copter = copters[i];
|
||
var singleCopterInfoObj = singleCopterInfos[i];
|
||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForTurnTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (short)singleCopterInfoObj.targetHeading);
|
||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
}
|
||
Tasks.Add(newTask);
|
||
TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask });
|
||
SelectedTask = newTask;
|
||
SelectedTaskIndex = Tasks.Count - 1;
|
||
}
|
||
|
||
private void RestoreCircleTask(dynamic singleCopterInfos)
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
if (!copters.Any()) return;
|
||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
||
var lastTask = Tasks.LastOrDefault();
|
||
var newTask = new FlightTask(FlightTaskType.Circle);
|
||
// TODO: 林俊清, 20150806, 处理实际飞行器数目与记录中数目不一致的情况。
|
||
for (int i = 0; i < copters.Count && i < singleCopterInfos.Count; i++)
|
||
{
|
||
var copter = copters[i];
|
||
var singleCopterInfoObj = singleCopterInfos[i];
|
||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForCircleTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (short)singleCopterInfoObj.centerDirectionDeg, (int)singleCopterInfoObj.radius, (int)singleCopterInfoObj.rate, (int)singleCopterInfoObj.turns, (ushort)singleCopterInfoObj.channel3);
|
||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
}
|
||
Tasks.Add(newTask);
|
||
TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask });
|
||
SelectedTask = newTask;
|
||
SelectedTaskIndex = Tasks.Count - 1;
|
||
}
|
||
|
||
private void RestoreSimpleCircleTask(dynamic singleCopterInfos)
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
if (!copters.Any()) return;
|
||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
||
var lastTask = Tasks.LastOrDefault();
|
||
var newTask = new FlightTask(FlightTaskType.SimpleCircle);
|
||
// TODO: 林俊清, 20150806, 处理实际飞行器数目与记录中数目不一致的情况。
|
||
for (int i = 0; i < copters.Count && i < singleCopterInfos.Count; i++)
|
||
{
|
||
var copter = copters[i];
|
||
var singleCopterInfoObj = singleCopterInfos[i];
|
||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForSimpleCircleTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (int)singleCopterInfoObj.rate, (int)singleCopterInfoObj.turns, (ushort)singleCopterInfoObj.channel3);
|
||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
}
|
||
Tasks.Add(newTask);
|
||
TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask });
|
||
SelectedTask = newTask;
|
||
SelectedTaskIndex = Tasks.Count - 1;
|
||
}
|
||
|
||
// added by ZJF
|
||
private void RestoreReturnToLandTask(IEnumerable<ICopter> copters,int retnumber,int rtlalt)
|
||
{
|
||
if (copters == null || !copters.Any()) return;
|
||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
||
var lastTask = Tasks.LastOrDefault();
|
||
|
||
var RTLTask = new FlightTask(FlightTaskType.ReturnToLand);
|
||
RTLTask.RetNumAttr = retnumber;
|
||
RTLTask.RTLAlt = rtlalt;
|
||
foreach (var copter in copters)
|
||
{
|
||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForReturnToLandTask(copter, targetAlt:10);
|
||
RTLTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
}
|
||
Tasks.Add(RTLTask);
|
||
TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = RTLTask });
|
||
SelectedTask = RTLTask;
|
||
SelectedTaskIndex = Tasks.Count - 1;
|
||
}
|
||
|
||
// added by ZJF
|
||
private void RestoreLoiterTimeTask(float LoiteTimeTmp, bool flashCheck, float flashCheckPeriod,
|
||
bool oneByOneCheck, float oneByOneCheckPeriod, string flashNameArray, string flashIndexArray,
|
||
bool ChangeYaw, float HeadYaw,
|
||
dynamic singleCopterInfos)
|
||
{
|
||
var copters = _copterManager.Copters;
|
||
if (copters == null || !copters.Any()) return;
|
||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
||
var lastTask = Tasks.LastOrDefault();
|
||
|
||
var loiterTimeTask = new FlightTask(FlightTaskType.Loiter)
|
||
{
|
||
LoiterTimeAttr = LoiteTimeTmp
|
||
};
|
||
loiterTimeTask.flashAttr = flashCheck;
|
||
loiterTimeTask.flashPeriodAttr = flashCheckPeriod;
|
||
loiterTimeTask.oneByOneAttr = oneByOneCheck;
|
||
loiterTimeTask.oneByOnePeriodAttr = oneByOneCheckPeriod;
|
||
loiterTimeTask.flashCopterNameArray = flashNameArray;
|
||
loiterTimeTask.flashCopterIndexArray = flashIndexArray;
|
||
loiterTimeTask.ChangeYaw = ChangeYaw;
|
||
loiterTimeTask.HeadYaw = HeadYaw;
|
||
|
||
// foreach (var copter in copters)
|
||
for (int i = 0; i < copters.Count; i++)
|
||
{
|
||
var copter = copters[i];
|
||
var singleCopterInfoObj = singleCopterInfos[i];
|
||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForLoiterTimeTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt);
|
||
loiterTimeTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||
}
|
||
Tasks.Add(loiterTimeTask);
|
||
TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = loiterTimeTask });
|
||
SelectedTask = loiterTimeTask;
|
||
SelectedTaskIndex = Tasks.Count - 1;
|
||
}
|
||
|
||
//导出任务
|
||
public string 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 = TakeOffNumAttr
|
||
};
|
||
case FlightTaskType.FlyTo:
|
||
return new
|
||
{
|
||
type = type,
|
||
staggerRoutes = task.StaggerRoutes,
|
||
flytoTime = task.FlytoTime,
|
||
loiterTime = task.LoiterTime,
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
var offset = info.LatLngOffset;
|
||
return new
|
||
{
|
||
latOffset = offset.Lat,
|
||
lngOffset = offset.Lng,
|
||
targetAlt = info.TargetAlt,
|
||
//showLED = info.FlytoShowLED,
|
||
ledInfos = info.LEDInfos
|
||
};
|
||
})
|
||
};
|
||
case FlightTaskType.Turn:
|
||
return new
|
||
{
|
||
type = type,
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
var offset = info.LatLngOffset;
|
||
return new
|
||
{
|
||
latOffset = offset.Lat,
|
||
lngOffset = offset.Lng,
|
||
targetAlt = info.TargetAlt,
|
||
targetHeading = info.TargetHeading
|
||
};
|
||
})
|
||
};
|
||
case FlightTaskType.Loiter: // 导出悬停任务
|
||
return new
|
||
{
|
||
type = type,
|
||
loiterTimeAttr = task.LoiterTimeAttr,
|
||
flashCheck = task.flashAttr,
|
||
flashCheckPeriod = task.flashPeriodAttr,
|
||
oneByOneCheck = task.oneByOneAttr,
|
||
oneByOneCheckPeriod = task.oneByOnePeriodAttr,
|
||
flashNameArray = task.flashCopterNameArray,
|
||
flashIndexArray = task.flashCopterIndexArray,
|
||
ChangeYaw = task.ChangeYaw,
|
||
HeadYaw = task.HeadYaw,
|
||
|
||
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
var offset = info.LatLngOffset;
|
||
return new
|
||
{
|
||
latOffset = offset.Lat,
|
||
lngOffset = offset.Lng,
|
||
targetAlt = info.TargetAlt
|
||
};
|
||
})
|
||
};
|
||
case FlightTaskType.Circle:
|
||
return new
|
||
{
|
||
type = type,
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
var offset = info.LatLngOffset;
|
||
return new
|
||
{
|
||
latOffset = offset.Lat,
|
||
lngOffset = offset.Lng,
|
||
targetAlt = info.TargetAlt,
|
||
centerDirectionDeg = info.CenterDirectionDeg,
|
||
radius = info.Radius,
|
||
rate = info.Rate,
|
||
turns = info.Turns,
|
||
channel3 = info.Channel3
|
||
};
|
||
})
|
||
};
|
||
case FlightTaskType.SimpleCircle:
|
||
return new
|
||
{
|
||
type = type,
|
||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
||
{
|
||
var offset = info.LatLngOffset;
|
||
return new
|
||
{
|
||
latOffset = offset.Lat,
|
||
lngOffset = offset.Lng,
|
||
targetAlt = info.TargetAlt,
|
||
rate = info.Rate,
|
||
turns = info.Turns,
|
||
channel3 = info.Channel3
|
||
};
|
||
})
|
||
};
|
||
case FlightTaskType.ReturnToLand: // added by ZJF
|
||
return new
|
||
{
|
||
type = type,
|
||
rtlalt= task.RTLAlt,
|
||
retnumber = task.RetNumAttr
|
||
|
||
};
|
||
default:
|
||
throw new NotImplementedException(type + " task export not implemented.");
|
||
}
|
||
});
|
||
return JsonConvert.SerializeObject(tasks);
|
||
}
|
||
|
||
|
||
//导入任务,可设置导入哪些步骤
|
||
public void ImportTasksindex(string tasksText,int startindex,int endindex)
|
||
{
|
||
dynamic tasks = JsonConvert.DeserializeObject(tasksText);
|
||
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;
|
||
break;
|
||
case FlightTaskType.FlyTo:
|
||
RestoreFlyToTask((bool)task.staggerRoutes, (int)task.flytoTime, (int)task.loiterTime, task.singleCopterInfos);
|
||
break;
|
||
case FlightTaskType.Turn:
|
||
RestoreTurnTask(task.singleCopterInfos);
|
||
break;
|
||
case FlightTaskType.Circle:
|
||
RestoreCircleTask(task.singleCopterInfos);
|
||
break;
|
||
case FlightTaskType.Loiter:
|
||
RestoreLoiterTimeTask((float)task.loiterTimeAttr, (bool)task.flashCheck, (float)task.flashCheckPeriod,
|
||
(bool)task.oneByOneCheck, (float)task.oneByOneCheckPeriod, (string)task.flashNameArray,
|
||
(string)task.flashIndexArray, (bool)task.ChangeYaw, (float)task.HeadYaw, task.singleCopterInfos);
|
||
|
||
break;
|
||
case FlightTaskType.SimpleCircle:
|
||
RestoreSimpleCircleTask(task.singleCopterInfos);
|
||
break;
|
||
case FlightTaskType.ReturnToLand:
|
||
// RestoreReturnToLandTask(copters, (int)task.retnumber, (int)task.rtlalt);
|
||
break;
|
||
}
|
||
}
|
||
i++;
|
||
}
|
||
}
|
||
// 导入任务
|
||
public void ImportTasks(string tasksText)
|
||
{
|
||
dynamic tasks = JsonConvert.DeserializeObject(tasksText);
|
||
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;
|
||
break;
|
||
case FlightTaskType.FlyTo:
|
||
RestoreFlyToTask((bool)task.staggerRoutes, (int)task.flytoTime, (int)task.loiterTime, task.singleCopterInfos);
|
||
break;
|
||
case FlightTaskType.Turn:
|
||
RestoreTurnTask(task.singleCopterInfos);
|
||
break;
|
||
case FlightTaskType.Circle:
|
||
RestoreCircleTask(task.singleCopterInfos);
|
||
break;
|
||
case FlightTaskType.Loiter:
|
||
RestoreLoiterTimeTask( (float)task.loiterTimeAttr, (bool)task.flashCheck, (float)task.flashCheckPeriod,
|
||
(bool)task.oneByOneCheck, (float)task.oneByOneCheckPeriod, (string)task.flashNameArray,
|
||
(string)task.flashIndexArray, (bool)task.ChangeYaw, (float)task.HeadYaw, task.singleCopterInfos);
|
||
|
||
break;
|
||
case FlightTaskType.SimpleCircle:
|
||
RestoreSimpleCircleTask(task.singleCopterInfos);
|
||
break;
|
||
case FlightTaskType.ReturnToLand:
|
||
RestoreReturnToLandTask(copters, (int)task.retnumber, (int)task.rtlalt);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
public void Select(int taskIndex, ICopter copter)
|
||
{
|
||
this.SelectedTaskIndex = taskIndex;
|
||
this.SelectedTask = Tasks[taskIndex];
|
||
|
||
// 在悬停任务时，左键waypoint为选中
|
||
// 在悬停任务模式，且LED控制被选中时
|
||
if ((this.SelectedTask.TaskType == FlightTaskType.Loiter) && (this.SelectedTask.flashAttr || this.SelectedTask.oneByOneAttr))
|
||
{
|
||
string copterNameStr = this.SelectedTask.flashCopterNameArray;
|
||
|
||
string[] copterArray = copterNameStr.Split(',');
|
||
bool flag = false;
|
||
for (int i = 0; i < copterArray.Length; i++)
|
||
{
|
||
if (copterArray[i].Equals(""))
|
||
continue;
|
||
if (copterArray[i].Equals(copter.Name))
|
||
{
|
||
flag = true;
|
||
break;
|
||
}
|
||
}
|
||
if (!flag)
|
||
{
|
||
if (copterArray[0].Equals(""))
|
||
{
|
||
this.SelectedTask.flashCopterNameArray += copter.Name;
|
||
SelectedTask.flashCopterIndexArray += Name2Index(copter.Name);
|
||
}
|
||
else
|
||
{
|
||
this.SelectedTask.flashCopterNameArray += "," + copter.Name;
|
||
SelectedTask.flashCopterIndexArray += "," + Name2Index(copter.Name);
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
/**
|
||
* 在悬停任务时，右键waypoint为取消选中
|
||
*/
|
||
public void RightSelect(int taskIndex, ICopter copter)
|
||
{
|
||
this.SelectedTaskIndex = taskIndex;
|
||
this.SelectedTask = Tasks[taskIndex];
|
||
|
||
// 在悬停任务模式，且LED控制被选中时
|
||
if ((this.SelectedTask.TaskType == FlightTaskType.Loiter) && (this.SelectedTask.flashAttr || this.SelectedTask.oneByOneAttr))
|
||
{
|
||
this.SelectedTask.flashCopterNameArray = DeleteSelectedName(this.SelectedTask.flashCopterNameArray, copter.Name);
|
||
this.SelectedTask.flashCopterIndexArray = DeleteSelectedName(this.SelectedTask.flashCopterIndexArray, Name2Index(copter.Name).ToString());
|
||
}
|
||
}
|
||
|
||
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 = "任务运行中";
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
}
|
||
|
||
public async Task RunTaskAsync()
|
||
|
||
{
|
||
Message.Show("任务开始");
|
||
await RunAsync();
|
||
if ((IsPaused ?? false) == false)
|
||
Message.Show("任务完成");
|
||
}
|
||
|
||
public async Task RunAsync()
|
||
{
|
||
IsPaused = false;
|
||
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;
|
||
await task.RunAsync().ConfigureAwait(false);
|
||
// 1. 被暂停时，中断 RunAsync。继续运行时将把此时运行了一半的 CurrentRunningTask 重新运行一遍。
|
||
if (IsPaused == true)
|
||
{
|
||
task.Status = FlightTaskStatus.Paused;
|
||
TaskState = TasksStatus.Paused;
|
||
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.
|
||
|
||
private async void StartAvoidingCrash()
|
||
{
|
||
await Task.Factory.StartNew(LoopToAvoidCrash);
|
||
}
|
||
|
||
private async void LoopToAvoidCrash()
|
||
{
|
||
while (IsPaused == false)
|
||
{
|
||
try
|
||
{
|
||
// 在目前的起飞、降落逻辑，如果起飞或最后一个任务是返航降落，不要检查两架飞机距离过近. added by ZJF
|
||
bool RTLFlag = ( CurrentRunningTaskIndex == Tasks.Count()-1 ) && ( CurrentRunningTask.TaskType == FlightTaskType.ReturnToLand );
|
||
// bool RTLFlag = false;
|
||
if ( (CurrentRunningTaskIndex != 0) && !RTLFlag)
|
||
{
|
||
|
||
foreach (var copter in _copterManager.Copters)
|
||
{
|
||
foreach (var anotherCopter in _copterManager.Copters)
|
||
{
|
||
if (copter != anotherCopter && copter.IsTooCloseTo(anotherCopter))
|
||
{
|
||
Pause();
|
||
Message.Show($"{copter.Name} 与 {anotherCopter.Name} 距离过近，已中止任务。");
|
||
// Alert.Show($"{copter.Name} 与 {anotherCopter.Name} 距离过近，已中止任务。");
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
}
|
||
}
|
||
catch // 林俊清, 20151102, 通常是“集合已修改”异常。
|
||
{
|
||
}
|
||
finally
|
||
{
|
||
await Task.Delay(100).ConfigureAwait(false);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
public class FlightTaskAddedEventArgs : EventArgs
|
||
{
|
||
public FlightTask LastTask { get; set; }
|
||
public FlightTask AddedTask { 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; }
|
||
}
|
||
}
|