diff --git a/Plane.FormationCreator/Formation/FlightTask.cs b/Plane.FormationCreator/Formation/FlightTask.cs index 1a3db2c..9817882 100644 --- a/Plane.FormationCreator/Formation/FlightTask.cs +++ b/Plane.FormationCreator/Formation/FlightTask.cs @@ -101,11 +101,11 @@ namespace Plane.FormationCreator.Formation break; case FlightTaskType.TakeOff: //多架同时起飞 - await MutilRunTakeOffTaskAsync(5).ConfigureAwait(false); + await MutilRunTakeOffTaskAsync().ConfigureAwait(false); break; case FlightTaskType.ReturnToLand: // Added by ZJF //多架同时返航 - await MutilRunReturnToLandTaskAsync(5).ConfigureAwait(false); + await MutilRunReturnToLandTaskAsync().ConfigureAwait(false); break; default: throw new InvalidOperationException(); diff --git a/Plane.FormationCreator/Formation/FlightTaskManager.cs b/Plane.FormationCreator/Formation/FlightTaskManager.cs index 3bad70a..81196cd 100644 --- a/Plane.FormationCreator/Formation/FlightTaskManager.cs +++ b/Plane.FormationCreator/Formation/FlightTaskManager.cs @@ -81,6 +81,27 @@ namespace Plane.FormationCreator.Formation set { Set(nameof(OriginLat), ref _OriginLat, value); } } + private int _TakeOffNumAttr = 1; + public int TakeOffNumAttr + { + get { return _TakeOffNumAttr; } + set + { + Set(nameof(TakeOffNumAttr), ref _TakeOffNumAttr, value); + + bool takeOffTaskExisted = Tasks.Count >= 1; + FlightTask takeOffTask; + if (takeOffTaskExisted) + { + takeOffTask = Tasks[0]; + takeOffTask.TakeOffNumAttr = TakeOffNumAttr; + } + + + } + } + + private double _OriginLng; public double OriginLng { @@ -101,6 +122,7 @@ namespace Plane.FormationCreator.Formation else { takeOffTask = new FlightTask(FlightTaskType.TakeOff); + takeOffTask.TakeOffNumAttr = TakeOffNumAttr; Tasks.Add(takeOffTask); TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { AddedTask = takeOffTask }); } @@ -338,14 +360,16 @@ namespace Plane.FormationCreator.Formation } // added by ZJF - private void RestoreReturnToLandTask(IEnumerable copters) + 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); - foreach (var copter in copters) + RTLTask.RetNumAttr = retnumber; + RTLTask.RTLAlt = rtlalt; + foreach (var copter in copters) { var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForReturnToLandTask(copter, targetAlt:10); RTLTask.SingleCopterInfos.Add(newSingleCopterInfo); @@ -493,7 +517,10 @@ namespace Plane.FormationCreator.Formation switch (type) { case FlightTaskType.TakeOff: - return new { type = type }; + return new { + type = type, + takeoffnumber = TakeOffNumAttr + }; case FlightTaskType.FlyTo: return new { @@ -594,7 +621,10 @@ namespace Plane.FormationCreator.Formation case FlightTaskType.ReturnToLand: // added by ZJF return new { - type = type + type = type, + rtlalt= task.RTLAlt, + retnumber = task.RetNumAttr + }; default: throw new NotImplementedException(type + " task export not implemented."); @@ -613,6 +643,7 @@ namespace Plane.FormationCreator.Formation { case FlightTaskType.TakeOff: // AddTakeOffTask(copters); // added by ZJF + TakeOffNumAttr = task.takeoffnumber; break; case FlightTaskType.FlyTo: RestoreFlyToTask((bool)task.staggerRoutes, task.singleCopterInfos); @@ -633,7 +664,7 @@ namespace Plane.FormationCreator.Formation RestoreSimpleCircleTask(task.singleCopterInfos); break; case FlightTaskType.ReturnToLand: - RestoreReturnToLandTask(copters); + RestoreReturnToLandTask(copters, (int)task.retnumber, (int)task.rtlalt); break; } } diff --git a/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs b/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs index e96da89..6d8ca06 100644 --- a/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs +++ b/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs @@ -66,6 +66,18 @@ namespace Plane.FormationCreator.Formation 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 = ""; @@ -75,7 +87,16 @@ namespace Plane.FormationCreator.Formation set { Set(nameof(flashCopterNameArray), ref _flashCopterNameArray, value); - } + + flashCopterIndexArray = ""; + + string[] copterArray = flashCopterNameArray.Split(','); + + for (int i = 0; i < copterArray.Length; i++) + { + flashCopterIndexArray += Name2Index(copterArray[i])+','; + } + } } // 闪烁的飞机序号编号 @@ -227,6 +248,7 @@ namespace Plane.FormationCreator.Formation // 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); diff --git a/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs b/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs index 091e253..35d84d5 100644 --- a/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs +++ b/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs @@ -14,7 +14,15 @@ namespace Plane.FormationCreator.Formation 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(); @@ -49,7 +57,7 @@ namespace Plane.FormationCreator.Formation } } - public async Task MutilRunReturnToLandTaskAsync(int ReturnCount) + public async Task MutilRunReturnToLandTaskAsync() { int TaskCount = _flightTaskManager.Tasks.Count(); if (TaskCount > 1) @@ -57,18 +65,18 @@ namespace Plane.FormationCreator.Formation var infos = SingleCopterInfos; int copterCount = infos.Count; - int integerPart = copterCount / ReturnCount; - int residualPart = copterCount % ReturnCount; + 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[ReturnCount]; - for (int j = ReturnCount * i; j < ReturnCount * (i + 1); j++) + var tasksRetutn = new Task[RetNumAttr]; + for (int j = RetNumAttr * i; j < RetNumAttr * (i + 1); j++) { var info = infos[j]; - int indexTmp = j - ReturnCount * i; + int indexTmp = j - RetNumAttr * i; // var copter = info.Copter; if (info.RTLStage <= 1) // 当前阶段小于等于1时进入 @@ -83,12 +91,12 @@ namespace Plane.FormationCreator.Formation await Task.WhenAll(tasksRetutn).ConfigureAwait(false); - var tasksRetutn1 = new Task[ReturnCount]; - for (int j = ReturnCount * i; j < ReturnCount * (i + 1); j++) + var tasksRetutn1 = new Task[RetNumAttr]; + for (int j = RetNumAttr * i; j < RetNumAttr * (i + 1); j++) { var info = infos[j]; - int indexTmp = j - ReturnCount * i; + int indexTmp = j - RetNumAttr * i; // var copter = info.Copter; if (info.RTLStage == 2) // 当前阶段小于等于1时进入 @@ -108,7 +116,7 @@ namespace Plane.FormationCreator.Formation if (_flightTaskManager.IsPaused == false) { - for (int j = ReturnCount * i; j < ReturnCount * (i + 1); j++) + for (int j = RetNumAttr * i; j < RetNumAttr * (i + 1); j++) { var info = infos[j]; info.RTLStage = 0; @@ -123,10 +131,10 @@ namespace Plane.FormationCreator.Formation { var tasksreturn = new Task[residualPart]; - for (int j = integerPart * ReturnCount; j < ReturnCount * integerPart + residualPart; j++) + for (int j = integerPart * RetNumAttr; j < RetNumAttr * integerPart + residualPart; j++) { var info = infos[j]; - int indexTmp = j - ReturnCount * integerPart; + int indexTmp = j - RetNumAttr * integerPart; // var copter = info.Copter; if (info.RTLStage <= 1) // 当前阶段小于等于1时进入 { @@ -141,10 +149,10 @@ namespace Plane.FormationCreator.Formation var tasksRetutn1 = new Task[residualPart]; - for (int j = integerPart * ReturnCount; j < ReturnCount * integerPart + residualPart; j++) + for (int j = integerPart * RetNumAttr; j < RetNumAttr * integerPart + residualPart; j++) { var info = infos[j]; - int indexTmp = j - ReturnCount * integerPart; + int indexTmp = j - RetNumAttr * integerPart; // var copter = info.Copter; if (info.RTLStage == 2) // 当前阶段小于等于1时进入 { @@ -160,7 +168,7 @@ namespace Plane.FormationCreator.Formation if (_flightTaskManager.IsPaused == false) { - for (int j = integerPart * ReturnCount; j < ReturnCount * integerPart + residualPart; j++) + for (int j = integerPart * RetNumAttr; j < RetNumAttr * integerPart + residualPart; j++) { var info = infos[j]; info.RTLStage = 0; @@ -177,7 +185,7 @@ namespace Plane.FormationCreator.Formation //原地下降高度到15米，然后返回起飞点 private async Task ReturnToLandTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) { - float takeOffAlt = 15.0f; + float takeOffAlt = RTLAlt; int TaskCount = _flightTaskManager.Tasks.Count(); int copterIndex = SingleCopterInfos.IndexOf(info); diff --git a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs index 1a62a41..394d2f2 100644 --- a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs +++ b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs @@ -11,6 +11,18 @@ namespace Plane.FormationCreator.Formation { public partial class FlightTask { + + + private int _TakeOffNumAttr = 1; + public int TakeOffNumAttr + { + get { return _TakeOffNumAttr; } + set + { + Set(nameof(TakeOffNumAttr), ref _TakeOffNumAttr, value); + } + } + public async Task RunTakeOffTaskAsync() { // float takeOffAlt = 15; @@ -49,14 +61,14 @@ namespace Plane.FormationCreator.Formation } // 几架飞机同时起飞，参数为takeOffCount - public async Task MutilRunTakeOffTaskAsync(int takeCount) + public async Task MutilRunTakeOffTaskAsync() { int TaskCount = _flightTaskManager.Tasks.Count(); if (TaskCount > 1) { var infos = SingleCopterInfos; - int takeOffCount = takeCount; + int takeOffCount = TakeOffNumAttr; int copterCount = infos.Count; int integerPart = copterCount / takeOffCount; int residualPart = copterCount % takeOffCount; diff --git a/Plane.FormationCreator/MainWindow.xaml.cs b/Plane.FormationCreator/MainWindow.xaml.cs index 2cbf7db..47cd977 100644 --- a/Plane.FormationCreator/MainWindow.xaml.cs +++ b/Plane.FormationCreator/MainWindow.xaml.cs @@ -122,8 +122,8 @@ namespace Plane.FormationCreator case Key.W: case Key.S: { - - if (_flightTaskManager.CurrentRunningTask==null ) + + if ((_flightTaskManager.CurrentRunningTask == null) && (_flightTaskManager.SelectedTask != null)) { var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); @@ -168,7 +168,7 @@ namespace Plane.FormationCreator { - if (_flightTaskManager.CurrentRunningTask == null) + if ((_flightTaskManager.CurrentRunningTask == null)&&(_flightTaskManager.SelectedTask!=null)) { var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); diff --git a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs index 8b629b6..5b11ca7 100644 --- a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs @@ -11,6 +11,7 @@ using System.Linq; using System.Text; using System.Threading.Tasks; using System.Windows.Input; +using Plane.Geography; namespace Plane.FormationCreator.ViewModels { @@ -142,7 +143,12 @@ namespace Plane.FormationCreator.ViewModels get { return _CanModifySingleCopterInfo; } set { Set(nameof(CanModifySingleCopterInfo), ref _CanModifySingleCopterInfo, value); } } - + private double _Distancevalue; + public double Distancevalue + { + get { return _Distancevalue; } + set { Set(nameof(Distancevalue), ref _Distancevalue, value); } + } private double _FlyToLat; public double FlyToLat { @@ -630,6 +636,9 @@ public ICommand VerticlAlignmentCommand })); } } + + + //垂直旋转 @@ -640,6 +649,192 @@ public ICommand VerticlAlignmentCommand { return _VerticlRotateCommand ?? (_VerticlRotateCommand = new RelayCommand(async RotateLine => { + + + + double lngsum = 0; + double latsum = 0; + double altsum = 0; + int selectcount = 0; + double centlng = 0; + double centlat = 0; + double centalt = 0; + var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); + bool copterisselect; + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + lngsum += _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; + latsum += _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; + altsum += _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetAlt; + selectcount++; + + } + } + //计算旋转中心 + if (selectcount > 0) + { + centlng = lngsum / selectcount; + centlat = latsum / selectcount; + centalt = altsum / selectcount; + } + else return; + + + //////////计算旋转，经测试用下面的函数组合计算比较准确 + double k = (double)RotateLine / 180 * Math.PI; + double dx = 0; + double dy = 0; + double ax = 0; + double ay = 0; + double tlng = 0; + double tlat = 0; + + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + { + tlng = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; + tlat = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; + //纬度方向距离（单位m） + ax = CalculationLogLatDistance.GetDistanceOne(tlng, centlat,tlng,tlat) *1000; + //方向角用于正负,0为正，180为负 + CalculationLogLatDistance.MyLatLng mypos1, mypos2; + mypos1 = new CalculationLogLatDistance.MyLatLng(tlng,centlat); + mypos2 = new CalculationLogLatDistance.MyLatLng(tlng,tlat); + double lpAzimuth = CalculationLogLatDistance.getAngle(mypos1, mypos2); + if (lpAzimuth > 90) + ax = -ax; + + + //高度方向距离（单位m） + ay = (_flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetAlt - centalt); + + dx = ax * Math.Cos(k) + ay * Math.Sin(k); + dy = -ax * Math.Sin(k) + ay * Math.Cos(k); + + //新高度（米） + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetAlt += (float) dy; + //计算新纬度 + double lng2 = 0; + double lat2 = 0; + if (dx < 0) + lpAzimuth = 180; + else + lpAzimuth = 0; + dx=Math.Abs(dx); + + CalculationLogLatDistance.ConvertDistanceToLogLat( + tlng, + centlat, //旋转中心纬度 + dx/1000, //新距离 + lpAzimuth, //方向垂直 + out lng2, + out lat2); + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = lat2; + + } + + + } + } + //// + + + + + await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 + + })); + } + } + + + + //缩放 + private ICommand _calDistinceCommand; + public ICommand calDistinceCommand + { + get + { + return _calDistinceCommand ?? (_calDistinceCommand = new RelayCommand(async => + { + + double lng1=0, lat1=0, alt1=0, lng2=0, lat2=0, alt2=0; + double distance = 0; + + var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); + bool copterisselect; + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + { + if (lng1 == 0) + { + lng1 = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; + lat1 = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; + alt1 = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetAlt; + } + else + if (lng2 == 0) + { + lng2 = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; + lat2 = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; + alt2 = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetAlt; + } + else break; + + } + + } + + + } + if ((lng1 != 0) && (lng2 != 0)) + { + + distance = GeographyUtils.CalcDistance( + lat1, lng1, alt1, + lat2, lng2, alt2); + distance = ((double)Math.Round(distance * 100) / 100); + + } + Distancevalue = distance; + + })); + } + } + + + + + //缩放 + private ICommand _ScaleCommand; + public ICommand ScaleCommand + { + get + { + return _ScaleCommand ?? (_ScaleCommand = new RelayCommand(async ScaleVale => + { + + double lngsum = 0; double latsum = 0; int selectcount = 0; @@ -666,20 +861,16 @@ public ICommand VerticlAlignmentCommand } } - //计算旋转中心 + //计算缩放中心 if (selectcount > 0) { centlng = lngsum / selectcount; centlat = latsum / selectcount; } else return; - double dx = 0; - double dy = 0; - double ax = 0; - double ay = 0; - double x = centlng; - double y = centlat; - double k = (double)RotateLine / 180 * Math.PI; + + + //////////计算旋转，经测试用下面的函数组合计算比较准确 for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) { @@ -688,12 +879,21 @@ public ICommand VerticlAlignmentCommand { if (capter == selectedCopter) { - ax = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; - ay = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; - dx = (ax - x) * Math.Cos(k) + (ay - y) * Math.Sin(k) + x; //此为最后的横坐标 - dy = -(ax - x) * Math.Sin(k) + (ay - y) * Math.Cos(k) + y; //此为最后的纵坐标 - _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng = dx; - _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = dy; + double lpDistance = CalculationLogLatDistance.GetDistanceOne(centlng, centlat, + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng, + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat); + + CalculationLogLatDistance.MyLatLng mypos1, mypos2; + mypos1 = new CalculationLogLatDistance.MyLatLng(centlng, centlat); + mypos2 = new CalculationLogLatDistance.MyLatLng(_flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng + , _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat); + double lpAzimuth = CalculationLogLatDistance.getAngle(mypos1, mypos2); + double lng2 = 0; + double lat2 = 0; + CalculationLogLatDistance.ConvertDistanceToLogLat(centlng, centlat, lpDistance * (double)ScaleVale/100, + lpAzimuth , out lng2, out lat2); + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng = lng2; + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = lat2; } @@ -701,17 +901,11 @@ public ICommand VerticlAlignmentCommand } } - - - await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 })); } } - - - } } diff --git a/Plane.FormationCreator/Views/ModifyTaskView.xaml b/Plane.FormationCreator/Views/ModifyTaskView.xaml index 2538571..1a424f1 100644 --- a/Plane.FormationCreator/Views/ModifyTaskView.xaml +++ b/Plane.FormationCreator/Views/ModifyTaskView.xaml @@ -7,10 +7,9 @@ xmlns:vm="clr-namespace:Plane.FormationCreator.ViewModels" xmlns:m="clr-namespace:Plane.FormationCreator.Formation" mc:Ignorable="d" - d:DesignHeight="300" - d:DesignWidth="300"> + d:DesignWidth="300" Height="400"> - + - +