From c85a17a324de534f6124f5685197bad3888c6c38 Mon Sep 17 00:00:00 2001 From: pxzleo Date: Sun, 13 Aug 2017 02:17:11 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5=E4=BB=BB=E5=8A=A1=E6=95=B4?= =?UTF-8?q?=E4=BD=93=E6=97=8B=E8=BD=AC=20=E5=8A=A0=E5=85=A5=E8=87=AA?= =?UTF-8?q?=E5=8A=A8=E8=AF=86=E5=88=ABRTK=E8=88=AA=E7=82=B9=E5=88=B0?= =?UTF-8?q?=E8=BE=BE=E7=B2=BE=E5=BA=A6=E4=B8=BA0.6=E7=B1=B3=EF=BC=8C?= =?UTF-8?q?=E7=A2=B0=E6=92=9E=E7=B2=BE=E5=BA=A6=E4=B8=BA0.5=E7=B1=B3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CalculationLogLatDistance.cs | 49 +++++++---- .../Formation/Extensions.cs | 29 +++++-- .../Formation/FlightTask_TakeOff.cs | 47 ++++++++--- .../ViewModels/ModifyTaskViewModel.cs | 81 ++++++++++++++++++- .../Views/ModifyTaskView.xaml | 6 +- 5 files changed, 180 insertions(+), 32 deletions(-) diff --git a/Plane.FormationCreator/CalculationLogLatDistance.cs b/Plane.FormationCreator/CalculationLogLatDistance.cs index 199b13d..5f94edc 100644 --- a/Plane.FormationCreator/CalculationLogLatDistance.cs +++ b/Plane.FormationCreator/CalculationLogLatDistance.cs @@ -56,20 +56,41 @@ namespace Plane.FormationCreator return s; } - /** - * 求两经纬度距离(google maps源码中) - * - * @param lon1 - * 第一点的经度 - * @param lat1 - * 第一点的纬度 - * @param lon2 - * 第二点的经度 - * @param lat2 - * 第二点的纬度 - * @return 两点距离，单位km - * */ - public static double GetDistanceTwo(double lon1, double lat1, double lon2, + public static void GetRotatePos(double Centlon, double Centlat,double Angle, double lon_in, + double lat_in, out double lon_out, out double lat_out) + { + + double lpDistance = GetDistanceOne(Centlon, Centlat, + lon_in, + lat_in); + + MyLatLng mypos1, mypos2; + mypos1 = new CalculationLogLatDistance.MyLatLng(Centlon, Centlat); + mypos2 = new CalculationLogLatDistance.MyLatLng(lon_in , lat_in); + double lpAzimuth = CalculationLogLatDistance.getAngle(mypos1, mypos2); + ConvertDistanceToLogLat(Centlon, Centlat, lpDistance, + lpAzimuth + Angle, out lon_out, out lat_out); + return ; + } + + + + + + /** + * 求两经纬度距离(google maps源码中) + * + * @param lon1 + * 第一点的经度 + * @param lat1 + * 第一点的纬度 + * @param lon2 + * 第二点的经度 + * @param lat2 + * 第二点的纬度 + * @return 两点距离，单位km + * */ + public static double GetDistanceTwo(double lon1, double lat1, double lon2, double lat2) { double radLat1 = rad(lat1); diff --git a/Plane.FormationCreator/Formation/Extensions.cs b/Plane.FormationCreator/Formation/Extensions.cs index cda1a89..f8e5ddc 100644 --- a/Plane.FormationCreator/Formation/Extensions.cs +++ b/Plane.FormationCreator/Formation/Extensions.cs @@ -6,10 +6,18 @@ using System.Linq; using System.Text; using System.Threading.Tasks; + + namespace Plane.FormationCreator.Formation { static class Extensions { + public const float GpsArrivedDis = 2.0f; //GPS模式下航点到达精度 + public const float RTKArrivedDis = 0.6f; //RTK模式航点达到精度 + public const float GpsCloseDis = 2.0f; //GPS模式下碰撞检测最近距离 + public const float RTKClosedDis = 0.5f; //RTK模式下碰撞检测最近距离 + + public static LatLng? GetCenter(this IEnumerable copters) { int count = 0; @@ -54,10 +62,14 @@ namespace Plane.FormationCreator.Formation public static bool ArrivedTarget(this ICopter copter, double targetLat, double targetLng, float targetAlt) { - // return copter.DistanceTo(targetLat, targetLng, targetAlt) < 2; - return copter.DistanceTo(targetLat, targetLng, targetAlt) < 0.6; //到达航点精度 - // return copter.DistanceTo(targetLat, targetLng, targetAlt) < 2; // added by ZJF - } + // return copter.DistanceTo(targetLat, targetLng, targetAlt) < 0.6; //到达航点精度 + + if (copter.GpsFixType == GpsFixType.RTKFIXED) + return copter.DistanceTo(targetLat, targetLng, targetAlt) < RTKArrivedDis; //RTK到达航点精度0.6米 + else + return copter.DistanceTo(targetLat, targetLng, targetAlt) < GpsArrivedDis; //GPS到达航点精度2.0米 + + } public static bool ArrivedTargetInPlane(this ICopter copter, double targetLat, double targetLng) { @@ -71,7 +83,14 @@ namespace Plane.FormationCreator.Formation public static bool IsTooCloseTo(this ICopter copter, ICopter copter2) { - return copter.DistanceTo(copter2) < 0.5; //最近距离2米 + if (copter.GpsFixType == GpsFixType.RTKFIXED) + return copter.DistanceTo(copter2) < RTKClosedDis; //最近距离0.5米 + else + return copter.DistanceTo(copter2) < GpsCloseDis; //最近距离2米 + + + + } } } diff --git a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs index b088deb..08c1218 100644 --- a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs +++ b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs @@ -23,6 +23,7 @@ namespace Plane.FormationCreator.Formation } } + //一架架起飞--未使用 public async Task RunTakeOffTaskAsync() { // float takeOffAlt = 15; @@ -60,7 +61,12 @@ namespace Plane.FormationCreator.Formation } } - // 几架飞机同时起飞，参数为takeOffCount + // 几架飞机同时起飞，参数为takeOffCount-----------------使用中 + //起飞分三个阶段：1阶段分批起飞到15米(目前15米高度是飞控起飞航点决定)，上一批起飞超过5米下一批开始起飞， + //等待全部起飞完成后执行第二阶段， + //2阶段等待高度超过9米(可能已到达15米)然后平飞到第一个航点位置,高度为15米的位置， + //3阶段垂直上升到第一个航点指定高度 + // public async Task MutilRunTakeOffTaskAsync() { int TaskCount = _flightTaskManager.Tasks.Count(); @@ -78,12 +84,13 @@ namespace Plane.FormationCreator.Formation for (int i = 0; i < integerPart; i++) { var tasksTakeOff = new Task[takeOffCount]; + //执行n架同时起飞 for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++) { var info = infos[j]; int indexTmp = j - takeOffCount * i; - if (info.takeOffStage == 0) // 第一阶段：起飞到10m + if (info.takeOffStage == 0) // 第一阶段：起飞到15m { tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info); } @@ -92,8 +99,9 @@ namespace Plane.FormationCreator.Formation tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); }); } } + //等待多架起飞任务-起飞高度超过5米完成 await Task.WhenAll(tasksTakeOff).ConfigureAwait(false); - + //加入已起飞飞机的第二第三阶段起飞任务--并不执行 for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++) { var info = infos[j]; @@ -110,7 +118,7 @@ namespace Plane.FormationCreator.Formation var info = infos[j]; int indexTmp = j - takeOffCount * integerPart; - if (info.takeOffStage == 0) // 第一阶段：起飞到10m + if (info.takeOffStage == 0) // 第一阶段：起飞到15m { tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info); } @@ -119,14 +127,16 @@ namespace Plane.FormationCreator.Formation tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); }); } } + //等待起飞超过5米 await Task.WhenAll(tasksTakeOff).ConfigureAwait(false); for (int j = integerPart * takeOffCount; j < takeOffCount * integerPart + residualPart; j++) { var info = infos[j]; - tasksTmp[j] = TakeOffSecondTaskAsync(info); // 第二和第三阶段 + tasksTmp[j] = TakeOffSecondTaskAsync(info); // 加入第二和第三阶段 } } + //执行并等待所有的第二第三阶段起飞任务完成 await Task.WhenAll(tasksTmp).ConfigureAwait(false); if (_flightTaskManager.IsPaused == false) { @@ -142,6 +152,7 @@ namespace Plane.FormationCreator.Formation private CopterManager _copterManager = ServiceLocator.Current.GetInstance(); + //执行第一阶段解锁起飞任务---使用中 private async Task TakeOffTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) { int copterIndex = SingleCopterInfos.IndexOf(info); @@ -155,7 +166,11 @@ namespace Plane.FormationCreator.Formation { return; } + + //设置灯光 await copter.SetShowLEDAsync(true); + + //开始解锁 await copter.UnlockAsync(); for (int i = 0; !copter.IsUnlocked; i++) { @@ -175,11 +190,13 @@ namespace Plane.FormationCreator.Formation await Task.Delay(25).ConfigureAwait(false); } + //////解锁完成 // 为了返航，记录家的位置, 应该放在起飞命令 info.TargetLat = info.Copter.Latitude; info.TargetLng = info.Copter.Longitude; + //开始起飞 for (int i = 0; i < 5; i++) // added by ZJF { await copter.TakeOffAsync(); @@ -187,6 +204,7 @@ namespace Plane.FormationCreator.Formation } // while (copter.Altitude < 4 || copter.State == Copters.CopterState.TakingOff) + //低于5米任务一直等待 while (copter.Altitude < 5) // 修改起飞逻辑，当高度达到5米时，下一架开始起飞 { if (_flightTaskManager.IsPaused == true) @@ -206,6 +224,7 @@ namespace Plane.FormationCreator.Formation */ } + //执行第二第三阶段起飞任务-------------使用中 private async Task TakeOffSecondTaskAsync(FlightTaskSingleCopterInfo info) { float takeOffAlt = 15; @@ -217,7 +236,7 @@ namespace Plane.FormationCreator.Formation return; // await Task.Run(async () => // { - // 修改起飞逻辑，当高度达到5米时，下一架开始起飞 + // 小于9米时等待----按起飞高度算此时可能已到达15米起飞高度 if (info.takeOffStage == 0) { while (info.Copter.Altitude < 9) @@ -239,17 +258,20 @@ namespace Plane.FormationCreator.Formation DateTime dtLastTime = DateTime.Now; TimeSpan ts = dtNow - dtLastTime; - // 第二阶段：水平飞行 + // 第二阶段：超过9米开始水平飞行 if (info.takeOffStage == 1) { + //切换到guided模式 await info.Copter.GuidAsync(); + //设置灯光为航点要求的灯光 await info.Copter.SetShowLEDAsync(copterNextTask.FlytoShowLED ); + //异步执行飞到第一个航点高度固定为15米的任务 for (int j = 0; j < 3; j++) { await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt); await Task.Delay(10).ConfigureAwait(false); } - + //直到到达第一个航点并高度15米 while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt)) { if (_flightTaskManager.IsPaused == true) @@ -261,6 +283,7 @@ namespace Plane.FormationCreator.Formation dtNow = DateTime.Now; ts = dtNow - dtLastTime; + //每2秒再异步执行一次到航点高度15米的任务 if (ts.TotalMilliseconds > 2000) { for (int j = 0; j < 2; j++) @@ -273,22 +296,23 @@ namespace Plane.FormationCreator.Formation } if (_flightTaskManager.IsPaused == false) { - info.takeOffStage++; + info.takeOffStage++; //当前变为2 } } dtNow = DateTime.Now; dtLastTime = DateTime.Now; ts = dtNow - dtLastTime; - // 第三阶段：垂直飞行 + // 第三阶段：从第一个航点位置15米垂直飞行 if (info.takeOffStage == 2) { + //执行飞到第一个航点高度为设定目标高度 for (int j = 0; j < 3; j++) { await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt); await Task.Delay(10).ConfigureAwait(false); } - + //直到到达第一个航点 while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt)) { if (_flightTaskManager.IsPaused == true) @@ -300,6 +324,7 @@ namespace Plane.FormationCreator.Formation dtNow = DateTime.Now; ts = dtNow - dtLastTime; + //每2秒发一次第一个航点飞行任务 if (ts.TotalMilliseconds > 2000) { for (int j = 0; j < 2; j++) diff --git a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs index e3c49b2..dd811cf 100644 --- a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs @@ -680,8 +680,87 @@ public ICommand VerticlAlignmentCommand })); } } - + //任务整体旋转 + private ICommand _TaskRotateCommand; + public ICommand TaskRotateCommand + { + get + { + return _TaskRotateCommand ?? (_TaskRotateCommand = new RelayCommand(async RotateLine => + { + double centlng = 0; + double centlat = 0; + + if (_copterManager.Copters.Count > 0) + { + centlng = _copterManager.Copters[0].Longitude; + centlat = _copterManager.Copters[0].Latitude; + + //虚拟飞机一起旋转 + if ("FakeCopter" == _copterManager.Copters[0].GetType().Name) + { + for (int i = 0; i < _copterManager.Copters.Count; i++) + { + FakeCopter tempcopter = (FakeCopter)_copterManager.Copters[i]; + double lng_out = 0; + double lat_out = 0; + CalculationLogLatDistance.GetRotatePos(centlng, centlat, (double)RotateLine, + tempcopter.Longitude, + tempcopter.Latitude, + out lng_out, out lat_out + ); + tempcopter.SetProperties( + latitude: lat_out, + longitude: lng_out + ); + + } + } + + + + + + double copterlng = 0; + double copterlat = 0; + double lng_out1 = 0; + double lat_out1 = 0; + for (int i = 0; i < _flightTaskManager.Tasks.Count; i++) + { + + + 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++) + { + copterlng=_flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLng; + copterlat = _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLat; + + CalculationLogLatDistance.GetRotatePos(centlng, centlat, (double)RotateLine, + copterlng, + copterlat, + out lng_out1, out lat_out1 + ); + + _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLng = lng_out1; + _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLat = lat_out1; + } + + } + + + } + + await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 + + })); + } + } diff --git a/Plane.FormationCreator/Views/ModifyTaskView.xaml b/Plane.FormationCreator/Views/ModifyTaskView.xaml index 4d6b31f..afa8fac 100644 --- a/Plane.FormationCreator/Views/ModifyTaskView.xaml +++ b/Plane.FormationCreator/Views/ModifyTaskView.xaml @@ -77,7 +77,11 @@ Text="0" VerticalContentAlignment="Center" /> - +