group/Plane.FormationCreator
1
0
Fork 0
Code Issues 3 Pull Requests Actions Packages Projects Releases Wiki Activity

去掉不用的任务类型，只保留起飞，飞向和降落

Browse Source
This commit is contained in:
xu 2020-02-08 23:23:53 +08:00
parent d7df82f667
commit e3f4e557dd
16 changed files with 16 additions and 2022 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
Plane.FormationCreator/Formation/FlightTask.cs
View File

@ -156,27 +156,11 @@ namespace Plane.FormationCreator.Formation
case FlightTaskType.FlyTo:
await RunFlyToTaskAsync().ConfigureAwait(false);
break;
case FlightTaskType.Turn:
await RunTurnTaskAsync().ConfigureAwait(false);
break;
case FlightTaskType.Circle:
await RunCircleTaskAsync().ConfigureAwait(false);
break;
case FlightTaskType.SimpleCircle:
await RunSimpleCircleTaskAsync().ConfigureAwait(false);
break;
case FlightTaskType.Loiter:
await RunLoiterTimeTaskAsync().ConfigureAwait(false);
break;
case FlightTaskType.TakeOff:
//多架同时起飞
//await MutilRunTakeOffTaskAsync().ConfigureAwait(false);
await NewMutilRunTakeOffTaskAsync().ConfigureAwait(false);
break;
case FlightTaskType.ReturnToLand: // Added by ZJF
//多架同时返航
await MutilRunReturnToLandTaskAsync().ConfigureAwait(false);
break;
case FlightTaskType.Land:
await MutilLandTaskAsync().ConfigureAwait(false);
break;
@ -189,12 +173,6 @@ namespace Plane.FormationCreator.Formation
public enum FlightTaskType
{
FlyTo = 0,
Turn = 1,
Circle = 2,
Loiter = 3, // 加入悬停时间选项
// LEDAction = 4,
ReturnToLand = 4, // added by ZJF. 20160315
SimpleCircle = 5,
Land = 6,
TakeOff = 100
}

400
Plane.FormationCreator/Formation/FlightTaskManager.cs
View File

@ -398,8 +398,6 @@ namespace Plane.FormationCreator.Formation
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(copter, targetLatLng.Item1, targetLatLng.Item2, preSingleCopterInfo.TargetAlt);
newSingleCopterInfo.TargetHeading = lastSingleCopterInfo.TargetHeading;
newSingleCopterInfo.CenterDirectionDeg = lastSingleCopterInfo.TargetHeading;
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
}
@ -603,16 +601,7 @@ namespace Plane.FormationCreator.Formation
}
}
// 将降落的阶段标志位都置位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;
}
}
}
@ -884,89 +873,7 @@ namespace Plane.FormationCreator.Formation
RaiseTaskAdded(lastTask, newTask);
}
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);
RaiseTaskAdded(lastTask, 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);
RaiseTaskAdded(lastTask, 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);
RaiseTaskAdded(lastTask, 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);
RaiseTaskAdded(lastTask, RTLTask);
}
private void RestoreTakeOffTask(byte takeOffTime, dynamic singleCopterInfos)
{
var copters = _copterManager.Copters;
@ -1015,43 +922,7 @@ namespace Plane.FormationCreator.Formation
}
// 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);
RaiseTaskAdded(lastTask, loiterTimeTask);
SelectedTask = loiterTimeTask;
SelectedTaskIndex = Tasks.Count - 1;
}
//导出任务 (单位：米)
public IEnumerable<object> ExportTasksToMeter()
@ -1101,94 +972,7 @@ namespace Plane.FormationCreator.Formation
};
})
};
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
};
case FlightTaskType.Land:
{
@ -1351,94 +1135,7 @@ namespace Plane.FormationCreator.Formation
};
})
};
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
};
case FlightTaskType.Land:
{
@ -1487,25 +1184,7 @@ namespace Plane.FormationCreator.Formation
break;
case FlightTaskType.FlyTo:
RestoreFlyToTask((bool)task.staggerRoutes, (int)task.flytoTime, (int)task.loiterTime, (string)task.taskname, task.singleCopterInfos, isMeter);
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;
break;
case FlightTaskType.Land:
RestoreLandTask(task.singleCopterInfos);
break;
@ -1843,24 +1522,7 @@ namespace Plane.FormationCreator.Formation
case FlightTaskType.FlyTo:
RestoreFlyToTask((bool)task.staggerRoutes, (int)task.flytoTime, (int)task.loiterTime, (string)task.taskname, task.singleCopterInfos, isMeter);
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;
case FlightTaskType.Land:
RestoreLandTask(task.singleCopterInfos);
break;
@ -1883,39 +1545,7 @@ namespace Plane.FormationCreator.Formation
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);
}
}
}
}
@ -1927,12 +1557,6 @@ namespace Plane.FormationCreator.Formation
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名字对应的序号
@ -2132,11 +1756,7 @@ namespace Plane.FormationCreator.Formation
{
try
{
// 在目前的起飞、降落逻辑，如果起飞或最后一个任务是返航降落，不要检查两架飞机距离过近. added by ZJF
bool RTLFlag = ( CurrentRunningTaskIndex == Tasks.Count()-1 ) && ( CurrentRunningTask.TaskType == FlightTaskType.ReturnToLand );
// bool RTLFlag = false;
//if ( (CurrentRunningTaskIndex != 0) && !RTLFlag)
{
double distance;
foreach (var copter in _copterManager.Copters)
{
@ -2150,7 +1770,7 @@ namespace Plane.FormationCreator.Formation
}
}
}
}
catch // 林俊清, 20151102, 通常是“集合已修改”异常。
{

68
Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Circle.cs
View File

@ -1,68 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Plane.Copters;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTaskSingleCopterInfo
{
public static FlightTaskSingleCopterInfo CreateForCircleTask(ICopter copter, LatLng latLngOffset, float targetAlt, short centerDirectionDeg, int radius, int rate, int turns, ushort channel3)
{
var info = new FlightTaskSingleCopterInfo(copter)
{
LatLngOffset = latLngOffset,
TargetAlt = targetAlt,
CenterDirectionDeg = centerDirectionDeg,
Radius = radius,
Rate = rate,
Turns = turns,
Channel3 = channel3
};
return info;
}
private short _CenterDirectionDeg;
public short CenterDirectionDeg
{
get { return _CenterDirectionDeg; }
set { Set(nameof(CenterDirectionDeg), ref _CenterDirectionDeg, value); }
}
private int _Radius = 1000;
/// <summary>
/// 圆的半径，单位为厘米。
/// </summary>
public int Radius
{
get { return _Radius; }
set { Set(nameof(Radius), ref _Radius, value); }
}
private int _Rate = 20;
/// <summary>
/// 转圈的角速度，单位为 deg/s，范围为 -90 到 90。
/// </summary>
public int Rate
{
get { return _Rate; }
set { Set(nameof(Rate), ref _Rate, value); }
}
private int _Turns = 1;
public int Turns
{
get { return _Turns; }
set { Set(nameof(Turns), ref _Turns, value); }
}
private ushort _Channel3 = 1500;
public ushort Channel3
{
get { return _Channel3; }
set { Set(nameof(Channel3), ref _Channel3, value); }
}
}
}

22
Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_LoiterTime.cs
View File

@ -1,22 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Plane.Copters;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTaskSingleCopterInfo
{
public static FlightTaskSingleCopterInfo CreateForLoiterTimeTask(ICopter copter, LatLng latLngOffset, float targetAlt)
{
var info = new FlightTaskSingleCopterInfo(copter)
{
LatLngOffset = latLngOffset,
TargetAlt = targetAlt
};
return info;
}
}
}

22
Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_ReturnToLand.cs
View File

@ -1,22 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Plane.Copters;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTaskSingleCopterInfo
{
// This file is added by ZJF
public static FlightTaskSingleCopterInfo CreateForReturnToLandTask(ICopter copter, float targetAlt)
{
var info = new FlightTaskSingleCopterInfo(copter)
{
TargetAlt = targetAlt
};
return info;
}
}
}

25
Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_SimpleCircle.cs
View File

@ -1,25 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Plane.Copters;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTaskSingleCopterInfo
{
public static FlightTaskSingleCopterInfo CreateForSimpleCircleTask(ICopter copter, LatLng latLngOffset, float targetAlt, int rate, int turns, ushort channel3)
{
var info = new FlightTaskSingleCopterInfo(copter)
{
LatLngOffset = latLngOffset,
TargetAlt = targetAlt,
Rate = rate,
Turns = turns,
Channel3 = channel3
};
return info;
}
}
}

30
Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Turn.cs
View File

@ -1,30 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Plane.Copters;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTaskSingleCopterInfo
{
public static FlightTaskSingleCopterInfo CreateForTurnTask(ICopter copter, LatLng latLngOffset, float targetAlt, short targetHeading)
{
var info = new FlightTaskSingleCopterInfo(copter)
{
LatLngOffset = latLngOffset,
TargetAlt = targetAlt,
TargetHeading = targetHeading
};
return info;
}
private short _TargetHeading;
public short TargetHeading
{
get { return _TargetHeading; }
set { Set(nameof(TargetHeading), ref _TargetHeading, value); }
}
}
}

137
Plane.FormationCreator/Formation/FlightTask_Circle.cs
View File

@ -1,137 +0,0 @@
using Plane.Copters;
using Plane.Geography;
using Plane.Logging;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTask
{
public async Task RunCircleTaskAsync()
{
// 林俊清, 20150922, 首先一起调好方向。
await Task.WhenAll(
SingleCopterInfos.Select(info =>
{
int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
return Task.Run(()=> { });
info.TargetHeading = info.CenterDirectionDeg;
return TurnTaskFlySingleCopterAsync(info);
})
).ConfigureAwait(false);
await Task.WhenAll(
SingleCopterInfos.Select(info => CircleTaskFlySingleCopterAsync(info))
).ConfigureAwait(false);
}
private async Task CircleTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
{
// 林俊清, 20150907, 目前不对 Circle 进行仿真。
if (info.Copter is FakeCopter)
{
await Task.Delay(1000);
return;
}
int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
return;
await info.Copter.SetParamAsync("CIRCLE_RADIUS", info.Radius);
await info.Copter.SetParamAsync("CIRCLE_RATE", info.Rate);
var center = GeographyUtils.CalcLatLngSomeMetersAway2D(info.Copter.Latitude, info.Copter.Longitude, info.CenterDirectionDeg, 10);
await info.Copter.CircleAsync();
while (info.Copter.State != Copters.CopterState.Circling)
{
if (_flightTaskManager.IsPaused == true)
{
await info.Copter.HoverAsync();
return;
}
await Task.Delay(25).ConfigureAwait(false);
}
await info.Copter.SetChannelsAsync(ch3: info.Channel3);
for (int i = info.Turns; i > 0; i--)
{
//// 等待一段时间，让飞行器飞出一段距离。
//for (int i = 0; i < 3000 / 25; i++)
//{
// if (_flightTaskManager.IsPaused == true)
// {
// info.Copter.Hover();
// return;
// }
// await Task.Delay(25).ConfigureAwait(false);
//}
//var log = new StringBuilder(1024);
//var lastLogTime = DateTime.Now;
bool step1 = false;
while (true)
{
if (_flightTaskManager.IsPaused == true)
{
await info.Copter.HoverAsync();
return;
}
var angle = info.Radius == 0 ? NormalizeAngleDeg(info.Copter.Heading - info.CenterDirectionDeg) : GetNormalizedAngleDeg(info, center);
//log.AppendLine($"{angle} {step1}");
if (!step1)
{
if (angle > 100 && angle < 330)
{
step1 = true;
//log.AppendLine($"step1 is true now.");
}
}
else if (angle > 340 || angle < 5)
{
//log.AppendLine("break");
break;
}
//if (lastLogTime.AddSeconds(10) < DateTime.Now)
//{
// Logger.Instance.Log(log.ToString());
// log.Clear();
// lastLogTime = DateTime.Now;
//}
await Task.Delay(25).ConfigureAwait(false);
}
//Logger.Instance.Log(log.ToString());
}
await info.Copter.HoverAsync();
}
private static double GetNormalizedAngleDeg(FlightTaskSingleCopterInfo info, Tuple<double, double> center)
{
var angle = info.Copter.InPlaneDirectionToDeg(center.Item1, center.Item2) - info.CenterDirectionDeg;
return NormalizeAngleDeg(angle);
}
private static double NormalizeAngleDeg(double angle)
{
while (true)
{
if (angle < 0) angle += 360;
else if (angle >= 360) angle -= 360;
else break;
}
return angle;
}
}
}

723
Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs
View File

@ -1,723 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTask
{
private float _LoiterTimeAttr = 0.0f;
public float LoiterTimeAttr
{
get { return _LoiterTimeAttr; }
set
{
Set(nameof(LoiterTimeAttr), ref _LoiterTimeAttr, value);
}
}
// 改变机头
private bool _ChangeYaw = false;
public bool ChangeYaw
{
get { return _ChangeYaw; }
set
{
Set(nameof(ChangeYaw), ref _ChangeYaw, value);
}
}
// 机头方向0为北，顺时针360
private float _HeadYaw = 1.0f;
public float HeadYaw
{
get { return _HeadYaw; }
set
{
Set(nameof(HeadYaw), ref _HeadYaw, value);
}
}
// LED灯闪烁
private bool _flashAttr = false;
public bool flashAttr
{
get { return _flashAttr; }
set
{
Set(nameof(flashAttr), ref _flashAttr, value);
}
}
// LED闪烁次数
private float _flashPeriodAttr = 1.0f;
public float flashPeriodAttr
{
get { return _flashPeriodAttr; }
set
{
Set(nameof(flashPeriodAttr), ref _flashPeriodAttr, value);
}
}
private string Name2Index(string name) // 获取指定copter名字对应的序号
{
int index = -1;
for (int i = 0; i < SingleCopterInfos.Count; i++)
{
if (name.Equals(SingleCopterInfos[i].Copter.Name))
{
return i.ToString();
}
}
return index.ToString();
}
// 闪烁的飞机名字编号
private string _flashCopterNameArray = "";
public string flashCopterNameArray
{
get { return _flashCopterNameArray; }
set
{
Set(nameof(flashCopterNameArray), ref _flashCopterNameArray, value);
flashCopterIndexArray = "";
string[] copterArray = flashCopterNameArray.Split(',');
for (int i = 0; i < copterArray.Length; i++)
{
flashCopterIndexArray += Name2Index(copterArray[i])+',';
}
}
}
// 闪烁的飞机序号编号
private string _flashCopterIndexArray = "";
public string flashCopterIndexArray
{
get { return _flashCopterIndexArray; }
set
{
Set(nameof(flashCopterIndexArray), ref _flashCopterIndexArray, value);
}
}
// LED走马灯
private bool _oneByOneAttr = false;
public bool oneByOneAttr
{
get { return _oneByOneAttr; }
set
{
Set(nameof(oneByOneAttr), ref _oneByOneAttr, value);
}
}
// LED走马灯次数
private float _oneByOnePeriodAttr = 1.0f;
public float oneByOnePeriodAttr
{
get { return _oneByOnePeriodAttr; }
set
{
Set(nameof(oneByOnePeriodAttr), ref _oneByOnePeriodAttr, value);
}
}
private int[] columnFirst = {0, 1, 2, 3, 4 };
private int[] columnSecond = { 5, 6, 7, 8, 9 };
private int[] columnThird = { 10, 11, 12, 13, 14 };
private int[] columnFourth = { 15, 16, 17, 18, 19 };
private int[] columnFive = { 20, 21, 22, 23, 24 };
private int[] columnSix = { 25, 26, 27, 28, 29 };
private int[] rowFirst = { 0, 5, 10, 15, 20, 25 };
private int[] rowSecond = { 1, 6, 11, 16, 21, 26 };
private int[] rowThird = { 2, 7, 12, 17, 22, 27 };
private int[] rowFourth = { 3, 8, 13, 18, 23, 28 };
private int[] rowFive = { 4, 9, 14, 19, 24, 29 };
private int[] rowFirstForEight = { 0, 1, 2, 3 };
private int[] rowSecondForEight = { 4, 5, 6, 7 };
private int[] doubleColumnFirst = { 0, 1, 2, 3, 4, 25, 26, 27, 28, 29 };
private int[] doubleColumnSecond = { 5, 6, 7, 8, 9, 20, 21, 22, 23, 24 };
private int[] doubleColumnThird = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19 };
private int[] circleFirst = { 0, 1, 2, 3, 4, 9, 14, 19, 24, 29, 28, 27, 26, 25, 20, 15, 10, 5 };
private int[] circleSecond = { 6, 7, 8, 13, 18, 23, 22, 21, 16, 11 };
private int[] circleThird = { 12, 17 };
private int[] lineFirst = { 4 };
private int[] lineSecond = { 3, 9 };
private int[] lineThird = { 2, 8, 14 };
private int[] lineFourth = { 1, 7, 13, 19 };
private int[] lineFive = { 0, 6, 12, 18, 24 };
private int[] lineSix = { 5, 11, 17, 23, 29 };
private int[] lineSeven = { 10, 16, 22, 28 };
private int[] lineEight = { 15, 21, 27 };
private int[] lineNine = { 20, 26 };
private int[] lineTen = { 25 };
public async Task RunLoiterTimeTaskAsync()
{
if (ChangeYaw)
{
await Task.WhenAll(SingleCopterInfos.Select(info => SetCopterYawAsync(info,HeadYaw)));
}
// 判断flashPeriodAttr, oneByOnePeriodAttr的值，执行不同的闪烁模式
// 当flashPeriodAttr小于10，正常闪烁，即没有预制闪烁模式
// 当flashPeriodAttr大于等于10，小于20，执行预制闪烁模式
// 当flashPeriodAttr大于等于20， 改变飞控闪烁模式，以oneByOnePeriodAttr作为周期值
//LEDFlashPlanAsync需要30架飞机
if ((flashPeriodAttr >= 10.0f) && (flashPeriodAttr < 20.0f))
{
await LEDFlashPlanAsync();
return;
}
//需要固件支持
if (flashPeriodAttr >= 20.0f)
{
await LEDFlashParaModifyPlanAsync();
return;
}
var infos = SingleCopterInfos;
if (flashAttr) // LED闪烁显示效果
{
if (flashCopterIndexArray.Equals(""))
return;
string[] copterArray = flashCopterIndexArray.Split(',');
var tasks_selected = new Task[copterArray.Length];
// LED灯全灭
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(1000).ConfigureAwait(false);
// 选中的飞机LED灯全亮
for (int i = 0; i < copterArray.Length; i++)
{
int index = int.Parse(copterArray[i]);
var info = infos[index];
tasks_selected[i] = await Task.Factory.StartNew(async () =>
{
var internalInfo = info;
await LEDFlashTaskFlySingleCopterAsync(internalInfo, true);
});
}
await Task.WhenAll(tasks_selected).ConfigureAwait(false);
await Task.Delay(3000).ConfigureAwait(false);
// LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
//延时等待
//判断是否下一步
DateTime start = DateTime.Now;
while (true)
{
if (_flightTaskManager.IsPaused == true)
{
await Task.WhenAll(SingleCopterInfos.Select(info => (info.Copter.HoverAsync())));
return;
}
DateTime end = DateTime.Now;
TimeSpan ts = end - start;
//等待指定时间
if (ts.TotalMilliseconds > LoiterTimeAttr * 1000)
break;
await Task.Delay(20).ConfigureAwait(false);
}
}
else if (oneByOneAttr) // LED走马灯显示效果
{
if (flashCopterNameArray.Equals(""))
return;
string[] copterArray = flashCopterIndexArray.Split(',');
//LED全灭
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(1000).ConfigureAwait(false);
// LED灯一个接一个全亮
for (int i = 0; i < copterArray.Length; i++)
{
if (copterArray[i] == "") break;
int index = int.Parse(copterArray[i]);
var info = infos[index];
await LEDFlashTaskFlySingleCopterAsync(info, true);
await Task.Delay(200).ConfigureAwait(false);
}
await Task.Delay(1000).ConfigureAwait(false);
//////////////////全部闪烁一次
// LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
await Task.Delay(200).ConfigureAwait(false);
// LED灯全灭
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(200).ConfigureAwait(false);
/////////////////////////////////////
//////////////////全部闪烁一次
// LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
await Task.Delay(200).ConfigureAwait(false);
// LED灯全灭
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(200).ConfigureAwait(false);
/////////////////////////////////////
//////////////////全部闪烁一次
// LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
await Task.Delay(200).ConfigureAwait(false);
// LED灯全灭
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(200).ConfigureAwait(false);
/////////////////////////////////////
// LED灯全亮
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
//延时等待
//判断是否下一步
DateTime start = DateTime.Now;
while (true)
{
if (_flightTaskManager.IsPaused == true)
{
await Task.WhenAll(SingleCopterInfos.Select(info => (info.Copter.HoverAsync())));
return;
}
DateTime end = DateTime.Now;
TimeSpan ts = end - start;
//等待指定时间
if (ts.TotalMilliseconds > LoiterTimeAttr * 1000)
break;
await Task.Delay(20).ConfigureAwait(false);
}
}
else // 没有LED显示效果只等待
{
//判断是否下一步
DateTime start = DateTime.Now;
while (true)
{
if (_flightTaskManager.IsPaused == true)
{
await Task.WhenAll(SingleCopterInfos.Select(info => (info.Copter.HoverAsync())));
return;
}
DateTime end = DateTime.Now;
TimeSpan ts = end - start;
//等待指定时间
if (ts.TotalMilliseconds > LoiterTimeAttr * 1000)
break;
await Task.Delay(20).ConfigureAwait(false);
}
}
}
private async Task LEDFlashParaModifyPlanAsync()
{
/*
await LEDFlashParaModifyAsync(rowFirstForEight);
await Task.Delay(500).ConfigureAwait(false);
await LEDFlashParaModifyAsync(rowSecondForEight);
await Task.Delay(500).ConfigureAwait(false);
*/
int copterCount = SingleCopterInfos.Count();
int[] intArray = new int[copterCount];
for (int ii=0; ii<copterCount; ii++)
{
intArray[ii] = ii;
}
await LEDFlashParaModifyAsync(intArray);
/*
await LEDFlashParaModifyAsync(rowFirst);
await Task.Delay(500).ConfigureAwait(false);
await LEDFlashParaModifyAsync(rowSecond);
await Task.Delay(500).ConfigureAwait(false);
await LEDFlashParaModifyAsync(rowThird);
await Task.Delay(500).ConfigureAwait(false);
await LEDFlashParaModifyAsync(rowFourth);
await Task.Delay(500).ConfigureAwait(false);
await LEDFlashParaModifyAsync(rowFive);
await Task.Delay(500).ConfigureAwait(false);
*/
}
private async Task LEDFlashParaModifyAsync(int[] LEDArray)
{
var infos = SingleCopterInfos;
var tasks_selected = new Task[LEDArray.Length];
for (int i = 0; i < LEDArray.Length; i++)
{
var info = infos[LEDArray[i]];
tasks_selected[i] = await Task.Factory.StartNew(async () =>
{
var internalInfo = info;
await LEDParaModifySingleCopterAsync(internalInfo, oneByOnePeriodAttr);
});
}
await Task.WhenAll(tasks_selected).ConfigureAwait(false);
}
private async Task SetCopterYawAsync(FlightTaskSingleCopterInfo info, float headyaw)
{
var copter = info.Copter;
// await Task.Delay(200).ConfigureAwait(false);
int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
var tasks = new Task[1];
tasks[0] = Task.Run(async () =>
{
try
{
await copter.SetMobileControlAsync(yaw: headyaw);
while (!copter.ArrivedHeading((short)headyaw))
{
if (_flightTaskManager.IsPaused == true)
{
await info.Copter.HoverAsync();
return;
}
await Task.Delay(25).ConfigureAwait(false);
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
}
}
catch (TimeoutException ex)
{
_logger.Log($"SetYAWtimeout, {ex.Message}, CopterId: {copter.Id}。");
}
});
await Task.WhenAll(tasks).ConfigureAwait(false);
}
private async Task LEDParaModifySingleCopterAsync(FlightTaskSingleCopterInfo info, float count)
{
var copter = info.Copter;
int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
var tasks = new Task[1];
tasks[0] = Task.Run(async () =>
{
try
{
if (count == 1.0)
await copter.SetShowLEDAsync(true);
else
await copter.SetShowLEDAsync(false);
//await copter.SetParamAsync("NTF_SHOWLED", count, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"SetShowLEDAsync 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
/*
var tasks = new Task[2];
tasks[0] = Task.Run(async () =>
{
try
{
await copter.SetParamAsync("NOTI_GPSLED", count, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"NOTI_GPSLED 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
tasks[1] = Task.Run(async () =>
{
try
{
await copter.SetParamAsync("NOTI_ARMLED", count, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"NOTI_ARMLED 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
*/
await Task.WhenAll(tasks).ConfigureAwait(false);
}
/**
* isOn为true，打开灯；isOn为false，关闭灯
*/
private async Task LEDFlashTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info, bool isOn)
{
var copter = info.Copter;
int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
// float gpsLed = await c.GetParamAsync("NOTI_GPSLED");
/*
float ledControl = 0.0f; //关灯
if (isOn)
{
ledControl = 1.0f; //长亮
}
*/
var tasks = new Task[1];
tasks[0] = Task.Run(async () =>
{
try
{
await copter.SetShowLEDAsync(isOn);
// await copter.SetParamAsync("NTF_SHOWLED", ledControl, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"SetShowLED 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
/*
var tasks = new Task[2];
tasks[0] = Task.Run(async () =>
{
try
{
await copter.SetParamAsync("NOTI_GPSLED", ledControl, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"NOTI_GPSLED 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
tasks[1] = Task.Run(async () =>
{
try
{
await copter.SetParamAsync("NOTI_ARMLED", ledControl, 5000);
}
catch (TimeoutException ex)
{
_logger.Log($"NOTI_ARMLED 超时, {ex.Message}, CopterId: {copter.Id}。");
}
});
*/
await Task.WhenAll(tasks).ConfigureAwait(false);
}
private async Task LEDFlashPlanAsync()
{
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(500).ConfigureAwait(false);
await LEDColumnFlashAsync();
await Task.Delay(500).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(500).ConfigureAwait(false);
await LEDDoubleColumnFlashAsync();
await Task.Delay(500).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(500).ConfigureAwait(false);
await LEDLineFlashAsync();
await Task.Delay(500).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(500).ConfigureAwait(false);
await LEDCircleFlashAsync();
await Task.Delay(500).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(200).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
await Task.Delay(200).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
await Task.Delay(200).ConfigureAwait(false);
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
await Task.Delay(200).ConfigureAwait(false);
}
// 一排一排闪烁 需要30架飞机，6排，一排5架，一排排闪灯
private async Task LEDColumnFlashAsync()
{
await LEDArrayFlashAsync(columnFirst);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(columnSecond);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(columnThird);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(columnFourth);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(columnFive);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(columnSix);
await Task.Delay(200).ConfigureAwait(false);
}
// 以圆圈收缩形式显示
private async Task LEDCircleFlashAsync()
{
await LEDArrayFlashAsync(circleFirst);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(circleSecond);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(circleThird);
await Task.Delay(200).ConfigureAwait(false);
}
// 两列同时闪烁
private async Task LEDDoubleColumnFlashAsync()
{
await LEDArrayFlashAsync(doubleColumnFirst);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(doubleColumnSecond);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(doubleColumnThird);
await Task.Delay(200).ConfigureAwait(false);
}
// 斜线方式闪烁
private async Task LEDLineFlashAsync()
{
await LEDArrayFlashAsync(lineFirst);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineSecond);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineThird);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineFourth);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineFive);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineSix);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineSeven);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineEight);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineNine);
await Task.Delay(200).ConfigureAwait(false);
await LEDArrayFlashAsync(lineTen);
await Task.Delay(200).ConfigureAwait(false);
}
private async Task LEDArrayFlashAsync(int[] LEDArray)
{
var infos = SingleCopterInfos;
var tasks_selected = new Task[LEDArray.Length];
for (int i = 0; i < LEDArray.Length; i++)
{
var info = infos[LEDArray[i]];
tasks_selected[i] = await Task.Factory.StartNew(async () =>
{
var internalInfo = info;
await LEDFlashTaskFlySingleCopterAsync(internalInfo, true);
});
}
await Task.WhenAll(tasks_selected).ConfigureAwait(false);
}
}
}

489
Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs
View File

@ -1,489 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTask
{
private float _RTLAlt = 20.0f;
public float RTLAlt
{
get { return _RTLAlt; }
set { Set(nameof(RTLAlt), ref _RTLAlt, value); }
}
private int _RetNumAttr = 1;
public int RetNumAttr
{
get { return _RetNumAttr; }
set
{
Set(nameof(RetNumAttr), ref _RetNumAttr, value);
}
}
public async Task RunReturnToLandTaskAsync()
{
int TaskCount = _flightTaskManager.Tasks.Count();
if (TaskCount > 1)
{
var infos = SingleCopterInfos;
// var infos = SingleCopterInfos.OrderByDescending(i => i.TargetAlt).ToList();
var tasks = new Task[infos.Count];
var tasksTmp = new Task[infos.Count];
for (int i = 0; i < infos.Count; i++)
{
var info = infos[i];
// var copter = info.Copter;
if (info.RTLStage <= 1) // 当前阶段小于等于1时进入
{
await ReturnToLandTaskFlySingleCopterAsync(info);
}
tasksTmp[i] = ReturnToLandSecondTaskAsync(info);
}
await Task.WhenAll(tasksTmp).ConfigureAwait(false);
if (_flightTaskManager.IsPaused == false)
{
for (int i = 0; i < infos.Count; i++)
{
var info = infos[i];
info.RTLStage = 0;
}
}
}
}
public async Task MutilRunReturnToLandTaskAsync()
{
int TaskCount = _flightTaskManager.Tasks.Count();
if (TaskCount > 1)
{
var infos = SingleCopterInfos;
int copterCount = infos.Count;
int integerPart = copterCount / RetNumAttr;
int residualPart = copterCount % RetNumAttr;
var tasksTmp = new Task[infos.Count];
//整数部分执行返航降落
for (int i = 0; i < integerPart; i++)
{
var tasksRetutn = new Task[RetNumAttr*2];
//设置返航任务
for (int j = RetNumAttr * i; j < RetNumAttr * (i + 1); j++)
{
var info = infos[j];
int indexTmp = j - RetNumAttr * i;
// var copter = info.Copter;
if (info.RTLStage <= 1) // 当前阶段小于等于1时进入
{
tasksRetutn[indexTmp] = ReturnToLandTaskFlySingleCopterAsync(info);
}
else
{
tasksRetutn[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
}
}
//设置降落任务
if (i > 0)
{
for (int j = RetNumAttr * (i - 1); j < RetNumAttr * (i); j++)
{
var info = infos[j];
int indexTmp = RetNumAttr+ j - RetNumAttr * (i-1);
// var copter = info.Copter;
if (info.RTLStage == 2) // 当前阶段小于等于1时进入
{
tasksRetutn[indexTmp] = ReturnToLandSecondTaskAsync(info);
}
else
{
tasksRetutn[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
}
}
}
else {
//第一波返航，降落任务为空
for (int j = RetNumAttr * (i +1); j < RetNumAttr * (i+2); j++)
{
int indexTmp = j - RetNumAttr * i;
tasksRetutn[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
}
}
//开始执行这波返航和上波降落
await Task.WhenAll(tasksRetutn).ConfigureAwait(false);
//上波降落后设置为0
if (_flightTaskManager.IsPaused == false)
{
if (i > 0)
{
for (int j = RetNumAttr * (i - 1); j < RetNumAttr * (i); j++)
{
var info = infos[j];
info.RTLStage = 0;
}
}
}
}
if (residualPart > 0)
{
var tasksreturn = new Task[residualPart+ RetNumAttr];
//余数部分开始返航
for (int j = integerPart * RetNumAttr; j < RetNumAttr * integerPart + residualPart; j++)
{
var info = infos[j];
int indexTmp = j - RetNumAttr * integerPart;
// var copter = info.Copter;
if (info.RTLStage <= 1) // 当前阶段小于等于1时进入
{
tasksreturn[indexTmp] = ReturnToLandTaskFlySingleCopterAsync(info);
}
else
{
tasksreturn[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
}
}
//无余数的整数部分最后一波开始降落
// var tasksLastland = new Task[RetNumAttr];
for (int j = RetNumAttr * (integerPart - 1); j < RetNumAttr * (integerPart); j++)
{
var info = infos[j];
int indexTmp = RetNumAttr + j - RetNumAttr * integerPart;
// var copter = info.Copter;
if (info.RTLStage == 2) // 当前阶段小于等于1时进入
{
tasksreturn[residualPart + indexTmp] = ReturnToLandSecondTaskAsync(info);
}
else
{
tasksreturn[residualPart+indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
}
}
//开始执行降落和返航
await Task.WhenAll(tasksreturn).ConfigureAwait(false);
//降落后设置为0
if (_flightTaskManager.IsPaused == false)
{
for (int j = RetNumAttr * (integerPart - 1); j < RetNumAttr * (integerPart); j++)
{
var info = infos[j];
info.RTLStage = 0;
}
}
//最后余数部分降落
var tasksRetutn1 = new Task[residualPart];
for (int j = integerPart * RetNumAttr; j < RetNumAttr * integerPart + residualPart; j++)
{
var info = infos[j];
int indexTmp = j - RetNumAttr * integerPart;
// var copter = info.Copter;
if (info.RTLStage == 2) // 当前阶段小于等于1时进入
{
tasksRetutn1[indexTmp] = ReturnToLandSecondTaskAsync(info);
}
else
{
tasksRetutn1[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
}
}
await Task.WhenAll(tasksRetutn1).ConfigureAwait(false);
if (_flightTaskManager.IsPaused == false)
{
for (int j = integerPart * RetNumAttr; j < RetNumAttr * integerPart + residualPart; j++)
{
var info = infos[j];
info.RTLStage = 0;
}
}
}
else
{
//无余数的整数部分最后一波开始降落
var tasksLastland = new Task[RetNumAttr];
for (int j = RetNumAttr * (integerPart - 1); j < RetNumAttr * (integerPart); j++)
{
var info = infos[j];
int indexTmp = RetNumAttr + j - RetNumAttr * integerPart;
// var copter = info.Copter;
if (info.RTLStage == 2) // 当前阶段小于等于1时进入
{
tasksLastland[indexTmp] = ReturnToLandSecondTaskAsync(info);
}
else
{
tasksLastland[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
}
}
await Task.WhenAll(tasksLastland).ConfigureAwait(false);
//降落后设置为0
if (_flightTaskManager.IsPaused == false)
{
for (int j = RetNumAttr * (integerPart - 1); j < RetNumAttr * (integerPart); j++)
{
var info = infos[j];
info.RTLStage = 0;
}
}
}
}
}
//原地下降高度到15米，然后返回起飞点
private async Task ReturnToLandTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
{
float takeOffAlt = RTLAlt;
int TaskCount = _flightTaskManager.Tasks.Count();
int copterIndex = SingleCopterInfos.IndexOf(info);
var copter = info.Copter;
var copterPreviousTask = _flightTaskManager.Tasks[TaskCount-2].SingleCopterInfos[copterIndex]; // 倒数第二步的目标位置
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
DateTime dtNow = DateTime.Now;
DateTime dtLastTime = DateTime.Now;
TimeSpan ts = dtNow - dtLastTime;
// 第一阶段：垂直飞行
if (info.RTLStage == 0)
{
for (int j = 0; j < 3; j++)
{
await info.Copter.FlyToAsync(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt);
}
while (!info.Copter.ArrivedTarget(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt))
{
if (_flightTaskManager.IsPaused == true)
{
await info.Copter.HoverAsync();
return;
}
await Task.Delay(25).ConfigureAwait(false);
dtNow = DateTime.Now;
ts = dtNow - dtLastTime;
if (ts.TotalMilliseconds > 1000)
{
for (int j = 0; j < 2; j++)
{
await info.Copter.FlyToAsync(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt);
}
dtLastTime = dtNow;
}
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
}
info.RTLStage++;
}
dtNow = DateTime.Now;
dtLastTime = DateTime.Now;
ts = dtNow - dtLastTime;
var copterFirstTask = _flightTaskManager.Tasks[0].SingleCopterInfos[copterIndex]; // 第一步记录的家位置
// 第二阶段：水平飞行
if (info.RTLStage == 1)
{
for (int j = 0; j < 3; j++)
{
await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt);
}
while (!info.Copter.ArrivedTarget(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt))
{
if (_flightTaskManager.IsPaused == true)
{
await info.Copter.HoverAsync();
return;
}
await Task.Delay(25).ConfigureAwait(false);
dtNow = DateTime.Now;
ts = dtNow - dtLastTime;
if (ts.TotalMilliseconds > 1000)
{
for (int j = 0; j < 2; j++)
{
await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt);
}
dtLastTime = dtNow;
}
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
}
info.RTLStage++;
}
}
//在起飞点上空从15米降落到4米，然后切land模式
private async Task ReturnToLandSecondTaskAsync(FlightTaskSingleCopterInfo info)
{
// await Task.Run(async () =>
// {
int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
DateTime dtNow = DateTime.Now;
DateTime dtLastTime = DateTime.Now;
TimeSpan ts = dtNow - dtLastTime;
float landAlt = 4.0f; // 飞机降落到4m再切land模式
var copterFirstTask = _flightTaskManager.Tasks[0].SingleCopterInfos[copterIndex]; // 第一步记录的家位置
// 第三阶段
if (info.RTLStage == 2)
{
for (int j = 0; j < 3; j++)
{
await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, landAlt);
}
while (!info.Copter.ArrivedTarget(copterFirstTask.TargetLat, copterFirstTask.TargetLng, landAlt))
{
if (_flightTaskManager.IsPaused == true)
{
await info.Copter.HoverAsync();
return;
}
await Task.Delay(25).ConfigureAwait(false);
dtNow = DateTime.Now;
ts = dtNow - dtLastTime;
if (ts.TotalMilliseconds > 1000)
{
for (int j = 0; j < 2; j++)
{
await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, landAlt);
}
dtLastTime = dtNow;
}
// 当该飞机被标记时，悬停并跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
{
await info.Copter.HoverAsync();
return;
}
}
info.RTLStage++;
}
// 切到land模式
if (_flightTaskManager.IsPaused == true)
{
await info.Copter.HoverAsync();
return;
}
var copter = info.Copter;
if (info.RTLStage == 3) // 下降过程
{
for (int j = 0; j < 5; j++) // added by ZJF
{
// await copter.ReturnToLaunchAsync();
await copter.LandAsync(); // 修改为降落模式
await Task.Delay(10);
}
}
// });
}
}
}

24
Plane.FormationCreator/Formation/FlightTask_SimpleCircle.cs
View File

@ -1,24 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTask
{
public async Task RunSimpleCircleTaskAsync()
{
var center = SingleCopterInfos.Select(info => info.Copter).GetCenter().Value;
// 林俊清, 20150921, 自动计算圆心方向和半径，然后直接调用普通画圈的方法。
foreach (var info in SingleCopterInfos)
{
info.CenterDirectionDeg = (short)info.Copter.InPlaneDirectionToDeg(center.Lat, center.Lng);
info.Radius = (int)(info.Copter.InPlaneDistanceTo(center.Lat, center.Lng) * 100);
}
await RunCircleTaskAsync().ConfigureAwait(false);
}
}
}

41
Plane.FormationCreator/Formation/FlightTask_Turn.cs
View File

@ -1,41 +0,0 @@
using Plane.Copters;
using Plane.Logging;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Plane.FormationCreator.Formation
{
public partial class FlightTask
{
public async Task RunTurnTaskAsync()
{
await Task.WhenAll(
SingleCopterInfos.Select(info => TurnTaskFlySingleCopterAsync(info))
).ConfigureAwait(false);
}
private async Task TurnTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
{
int copterIndex = SingleCopterInfos.IndexOf(info);
// 当该飞机被标记时，跳过飞行任务
if ((bool)_copterManager.CopterStatus[copterIndex])
return;
await info.Copter.SetMobileControlAsync(yaw: info.TargetHeading);
while (!info.Copter.ArrivedHeading(info.TargetHeading))
{
if (_flightTaskManager.IsPaused == true)
{
await info.Copter.HoverAsync();
return;
}
await Task.Delay(25).ConfigureAwait(false);
}
await info.Copter.HoverAsync();
}
}
}

10
Plane.FormationCreator/Plane.FormationCreator.csproj
View File

@ -156,13 +156,7 @@
<Compile Include="Formation\Copter.cs" />
<Compile Include="Formation\FlightTaskSingleCopterInfo_Land.cs" />
<Compile Include="Formation\FlightTaskSingleCopterInfo_LED.cs" />
<Compile Include="Formation\FlightTaskSingleCopterInfo_LoiterTime.cs" />
<Compile Include="Formation\FlightTaskSingleCopterInfo_ReturnToLand.cs" />
<Compile Include="Formation\FlightTaskSingleCopterInfo_SimpleCircle.cs" />
<Compile Include="Formation\FlightTaskSingleCopterInfo_Turn.cs" />
<Compile Include="Formation\FlightTaskSingleCopterInfo_TakeOff.cs" />
<Compile Include="Formation\FlightTask_Circle.cs" />
<Compile Include="Formation\FlightTaskSingleCopterInfo_Circle.cs" />
<Compile Include="Formation\Extensions.cs" />
<Compile Include="Formation\FlightTask.cs" />
<Compile Include="Formation\FlightTaskSingleCopterInfo.cs" />
@ -170,10 +164,6 @@
<Compile Include="Formation\FlightTask_FlyTo.cs" />
<Compile Include="Formation\FlightTaskSingleCopterInfo_FlyTo.cs" />
<Compile Include="Formation\FlightTask_Land.cs" />
<Compile Include="Formation\FlightTask_LoiterTime.cs" />
<Compile Include="Formation\FlightTask_ReturnToLand.cs" />
<Compile Include="Formation\FlightTask_SimpleCircle.cs" />
<Compile Include="Formation\FlightTask_Turn.cs" />
<Compile Include="Formation\FormationController.cs" />
<Compile Include="Formation\CopterManager.cs" />
<Compile Include="Formation\GroupManager.cs" />

2
Plane.FormationCreator/Util/rtcm3.cs
View File

@ -55,7 +55,7 @@ namespace Plane.Util
public event EventHandler ObsMessage;
public event EventHandler BasePosMessage;
public event EventHandler EphMessage;
public int length
{

9
Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs
View File

@ -28,7 +28,7 @@ namespace Plane.FormationCreator.ViewModels
{
// serialport
internal static IConnection Rtkport ;
private static System.Threading.Thread thrtk;
private static bool trkthreadrun = false;
private static bool rtcmthreadrun = false;
@ -1371,12 +1371,7 @@ namespace Plane.FormationCreator.ViewModels
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt - groudAlt)
);
break;
case FlightTaskType.Loiter:
break;
case FlightTaskType.ReturnToLand:
missions.Add(Mission.CreateReturnToLaunchMission());
break;
case FlightTaskType.Land:
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));

14
Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs
View File

@ -248,8 +248,7 @@ namespace Plane.FormationCreator.ViewModels
for (int i = _startindex; i <= _endindex; i++)
{
if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.TakeOff) ||
(_flightTaskManager.Tasks[i].TaskType == FlightTaskType.ReturnToLand))
if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.TakeOff))
continue;
@ -1124,13 +1123,7 @@ public ICommand VerticlAlignmentCommand
{
if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.Loiter) && (_flightTaskManager.Tasks[i].ChangeYaw))
{
_flightTaskManager.Tasks[i].HeadYaw += RotateLine;
_flightTaskManager.Tasks[i].HeadYaw = _flightTaskManager.Tasks[i].HeadYaw % 360;
}
for (int j = 0; j < _flightTaskManager.Tasks[i].SingleCopterInfos.Count; j++)
for (int j = 0; j < _flightTaskManager.Tasks[i].SingleCopterInfos.Count; j++)
{
copterlng=_flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLng;
copterlat = _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLat;
@ -1430,8 +1423,7 @@ public ICommand VerticlAlignmentCommand
for (int i = _startindex; i <= _endindex; i++)
{
if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.TakeOff) ||
(_flightTaskManager.Tasks[i].TaskType == FlightTaskType.ReturnToLand))
if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.TakeOff))
continue;