diff --git a/Plane.FormationCreator/Formation/FlightTask.cs b/Plane.FormationCreator/Formation/FlightTask.cs index 629b613..be75f76 100644 --- a/Plane.FormationCreator/Formation/FlightTask.cs +++ b/Plane.FormationCreator/Formation/FlightTask.cs @@ -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 } diff --git a/Plane.FormationCreator/Formation/FlightTaskManager.cs b/Plane.FormationCreator/Formation/FlightTaskManager.cs index f9b3f62..75f853e 100644 --- a/Plane.FormationCreator/Formation/FlightTaskManager.cs +++ b/Plane.FormationCreator/Formation/FlightTaskManager.cs @@ -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 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 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, 通常是“集合已修改”异常。 { diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Circle.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Circle.cs deleted file mode 100644 index cd3975b..0000000 --- a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Circle.cs +++ /dev/null @@ -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; - /// - /// 圆的半径，单位为厘米。 - /// - public int Radius - { - get { return _Radius; } - set { Set(nameof(Radius), ref _Radius, value); } - } - - private int _Rate = 20; - /// - /// 转圈的角速度，单位为 deg/s，范围为 -90 到 90。 - /// - 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); } - } - } -} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_LoiterTime.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_LoiterTime.cs deleted file mode 100644 index d59e131..0000000 --- a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_LoiterTime.cs +++ /dev/null @@ -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; - } - } -} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_ReturnToLand.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_ReturnToLand.cs deleted file mode 100644 index 9b8ac11..0000000 --- a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_ReturnToLand.cs +++ /dev/null @@ -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; - } - } -} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_SimpleCircle.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_SimpleCircle.cs deleted file mode 100644 index dec9406..0000000 --- a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_SimpleCircle.cs +++ /dev/null @@ -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; - } - } -} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Turn.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Turn.cs deleted file mode 100644 index e95472e..0000000 --- a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Turn.cs +++ /dev/null @@ -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); } - } - } -} diff --git a/Plane.FormationCreator/Formation/FlightTask_Circle.cs b/Plane.FormationCreator/Formation/FlightTask_Circle.cs deleted file mode 100644 index b0a352e..0000000 --- a/Plane.FormationCreator/Formation/FlightTask_Circle.cs +++ /dev/null @@ -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 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; - } - } -} diff --git a/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs b/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs deleted file mode 100644 index 5558303..0000000 --- a/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs +++ /dev/null @@ -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 - { - 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); - } - - } -} diff --git a/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs b/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs deleted file mode 100644 index 941bc74..0000000 --- a/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs +++ /dev/null @@ -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); - } - } - - // }); - } - } -} diff --git a/Plane.FormationCreator/Formation/FlightTask_SimpleCircle.cs b/Plane.FormationCreator/Formation/FlightTask_SimpleCircle.cs deleted file mode 100644 index db34e16..0000000 --- a/Plane.FormationCreator/Formation/FlightTask_SimpleCircle.cs +++ /dev/null @@ -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); - } - } -} diff --git a/Plane.FormationCreator/Formation/FlightTask_Turn.cs b/Plane.FormationCreator/Formation/FlightTask_Turn.cs deleted file mode 100644 index 6f428de..0000000 --- a/Plane.FormationCreator/Formation/FlightTask_Turn.cs +++ /dev/null @@ -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(); - } - } -} diff --git a/Plane.FormationCreator/Plane.FormationCreator.csproj b/Plane.FormationCreator/Plane.FormationCreator.csproj index 60e4950..4a8e603 100644 --- a/Plane.FormationCreator/Plane.FormationCreator.csproj +++ b/Plane.FormationCreator/Plane.FormationCreator.csproj @@ -156,13 +156,7 @@ - - - - - - @@ -170,10 +164,6 @@ - - - - diff --git a/Plane.FormationCreator/Util/rtcm3.cs b/Plane.FormationCreator/Util/rtcm3.cs index 9476f93..a4e404f 100644 --- a/Plane.FormationCreator/Util/rtcm3.cs +++ b/Plane.FormationCreator/Util/rtcm3.cs @@ -55,7 +55,7 @@ namespace Plane.Util public event EventHandler ObsMessage; public event EventHandler BasePosMessage; - public event EventHandler EphMessage; + public int length { diff --git a/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs b/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs index 21f104e..bdf5377 100644 --- a/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs @@ -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)); diff --git a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs index 1f62bb1..9d3d8fa 100644 --- a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs @@ -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;