diff --git a/Plane.FormationCreator/App.xaml.cs b/Plane.FormationCreator/App.xaml.cs index 0e5bc3a..ea74d0e 100644 --- a/Plane.FormationCreator/App.xaml.cs +++ b/Plane.FormationCreator/App.xaml.cs @@ -114,18 +114,31 @@ namespace Plane.FormationCreator MainWindow = new MainWindow(); MainWindow.Show(); - TcpServerConnectionManager.Instance.ConnectionEstablished += ConnectionManager_ConnectionEstablished; - if (!TcpServerConnectionManager.Instance.StartListening()) + try { - Alert.Show("网络连接不正常，无法启动监听。"); + + TcpServerConnectionManager.Instance.ConnectionEstablished += ConnectionManager_ConnectionEstablished; + if (!TcpServerConnectionManager.Instance.StartListening()) + { + Alert.Show("网络连接不正常，无法连接飞机。"); + return; + } + UdpServerConnectionManager.Instance.ExceptionThrown += (sender, e1) => + { + _logger.Log(e1.Exception); + }; + UdpServerConnectionManager.Instance.ConnectionEstablished += ConnectionManager_ConnectionEstablished; + UdpServerConnectionManager.Instance.StartReceiving(); + } + catch (Exception ex) + { + Alert.Show("网络连接不正常，无法连接飞机。"); return; } - UdpServerConnectionManager.Instance.ExceptionThrown += (sender, e1) => - { - _logger.Log(e1.Exception); - }; - UdpServerConnectionManager.Instance.ConnectionEstablished += ConnectionManager_ConnectionEstablished; - UdpServerConnectionManager.Instance.StartReceiving(); + + + + } private void Copter_TextReceived(object sender, MessageCreatedEventArgs e) { diff --git a/Plane.FormationCreator/Formation/Extensions.cs b/Plane.FormationCreator/Formation/Extensions.cs index ea702f6..c6fdd42 100644 --- a/Plane.FormationCreator/Formation/Extensions.cs +++ b/Plane.FormationCreator/Formation/Extensions.cs @@ -59,38 +59,30 @@ namespace Plane.FormationCreator.Formation { return GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, targetLat, targetLng); } - + //是否达到目标航点 public static bool ArrivedTarget(this ICopter copter, double targetLat, double targetLng, float targetAlt) { - // 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) - { - return copter.InPlaneDistanceTo(targetLat, targetLng) < 1; - } - + } + //是否已达到目标朝向 public static bool ArrivedHeading(this ICopter copter, short targetHeading) { return Math.Abs(copter.Heading - targetHeading) < 2; } + //是否距离太近 public static bool IsTooCloseTo(this ICopter copter, ICopter copter2) { - if (copter.GpsFixType == GpsFixType.RTKFIXED) - return copter.DistanceTo(copter2) < RTKClosedDis; //最近距离0.5米 + if ((copter.GpsFixType == GpsFixType.RTKFIXED)&&(copter2.GpsFixType == GpsFixType.RTKFIXED)) + return copter.DistanceTo(copter2) < RTKClosedDis; //都是RTKFIXED则最近距离0.5米 else - return copter.DistanceTo(copter2) < GpsCloseDis; //最近距离2米 - - - - + return copter.DistanceTo(copter2) < GpsCloseDis; //其他任何情况最近距离2米 } + + + } } diff --git a/Plane.FormationCreator/Formation/FlightTaskManager.cs b/Plane.FormationCreator/Formation/FlightTaskManager.cs index 117ee50..1c707d7 100644 --- a/Plane.FormationCreator/Formation/FlightTaskManager.cs +++ b/Plane.FormationCreator/Formation/FlightTaskManager.cs @@ -924,32 +924,88 @@ namespace Plane.FormationCreator.Formation public bool? IsPaused { get { return _IsPaused; } - private set { + private set + { if (Set(nameof(IsPaused), ref _IsPaused, value)) { - if (_IsPaused??false) + if (_IsPaused ?? false) { MessageText = "任务暂停！"; - }else + } + else { MessageText = "任务运行中"; - } + } } - + } } + private bool _CanRunTask=false; + public bool CanRunTask + { + get { return _CanRunTask; } + private set + { + if (Set(nameof(CanRunTask), ref _CanRunTask, value)) + { + + } + + + } + + } + public async Task RunTaskAsync() { + if (IsPaused != true) + { + await RunCheckAsync(); + if (!CanRunTask) + return; + } Message.Show("任务开始"); await RunAsync(); if ((IsPaused ?? false) == false) Message.Show("任务完成"); } + + public async Task RunCheckAsync() + { + CanRunTask = false; + + foreach (var copter in _copterManager.Copters) + { + float gpstype = await copter.GetParamAsync("GPS_TYPE"); + //RTK + if (gpstype == 15) + { + if (copter.GpsFixType != GpsFixType.RTKFIXED) + { + Message.Show(copter.Id+ ",是RTK定位，但未进入RTKFIX模式"); + return; + } + } + //auto--普通GPS + if (gpstype == 0) + { + if (!copter.IsGpsAccurate) + { + Message.Show(copter.Id + ",GPS未定位"); + return; + } + } + + + + } + CanRunTask = true; + } public async Task RunAsync() { IsPaused = false; diff --git a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs index d1a3485..59f0a5b 100644 --- a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs +++ b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs @@ -45,389 +45,10 @@ namespace Plane.FormationCreator.Formation } } - public async Task RunFollowFourLeadersTaskAsync() // 跟随前面飞机 - { - int[] followHeadsIndex = { 0, 0, 0, 0}; - int followHeadsNum = 0; - int startTaskIndex = 0; - int endTaskIndex = 0; - - int taskIndex = _flightTaskManager.CurrentRunningTaskIndex; - if (taskIndex >= 2 && taskIndex <= 4) - { - followHeadsNum = 1; - followHeadsIndex[0] = 0; - followHeadsIndex[1] = 4; - followHeadsIndex[2] = 8; - // followHeadsIndex[3] = 6; - - startTaskIndex = 2; - endTaskIndex = 4; - } - - if (taskIndex >= 18 && taskIndex <= 20) - { - followHeadsNum = 3; - followHeadsIndex[0] = 3; - followHeadsIndex[1] = 7; - followHeadsIndex[2] = 11; - // followHeadsIndex[3] = 6; - - startTaskIndex = 18; - endTaskIndex = 20; - } - - /* - if (taskIndex >= 6 && taskIndex <= 6) - { - followHeadsNum = 2; - followHeadsIndex[0] = 0; - followHeadsIndex[1] = 5; - - startTaskIndex = 6; - endTaskIndex = 6; - } - */ - - if (StaggerRoutes) - { - var infos = SingleCopterInfos; - if (_flightTaskManager.Tasks.IndexOf(this) == startTaskIndex) - { - var tasks = new Task[infos.Count]; - for (int i = 0; i < infos.Count; i++) - { - var info = infos[i]; - var copter = info.Copter; - var i1 = i; - tasks[i1] = await Task.Factory.StartNew(async () => - { - await FollowFourLeadersTaskFlySingleCopterAsync(info); - }); - - } - - if (startTaskIndex == endTaskIndex) - { - await Task.WhenAll(tasks).ConfigureAwait(false); - } - else - { - for (int i = 0; i < followHeadsNum; i++) - { - await tasks[followHeadsIndex[i]]; - } - } - - // await Task.WhenAll(tasks[0]).ConfigureAwait(false); - } - else if ((_flightTaskManager.Tasks.IndexOf(this) > startTaskIndex) && (_flightTaskManager.Tasks.IndexOf(this) < endTaskIndex)) - { - var tasks = new Task[followHeadsNum]; - for (int i = 0; i < followHeadsNum; i++) - { - var info = infos[followHeadsIndex[i]]; - var copter = info.Copter; - var i1 = i; - tasks[i1] = await Task.Factory.StartNew(async () => - { - await FollowFourLeadersTaskFlySingleCopterAsync(info); - }); - - } - await Task.WhenAll(tasks).ConfigureAwait(false); - } - else - //(_flightTaskManager.Tasks.IndexOf(this) == endTaskIndex) - { - var tasks = new Task[followHeadsNum]; - for (int i = 0; i < followHeadsNum; i++) - { - var info = infos[followHeadsIndex[i]]; - var copter = info.Copter; - var i1 = i; - tasks[i1] = await Task.Factory.StartNew(async () => - { - await FollowFourLeadersTaskFlySingleCopterAsync(info); - }); - - } - - await Task.WhenAll(tasks).ConfigureAwait(false); - while (true) - { - bool isFinishedFlag = true; - for (int i=0; i FollowFourLeadersTaskFlySingleCopterAsync(info)) - ).ConfigureAwait(false); - } - } - - private async Task FollowFourLeadersTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) - { - var infos = SingleCopterInfos; // 本次任务所有飞机编号 - int index = infos.FindIndex(s => s == info); // 本次任务当前飞机编号 - - int followHead = 0; - int indexHead = 0; - - int startTaskIndex = 0; - int endTaskIndex = _flightTaskManager.Tasks.Count(); - float directionAngle = 0; - - int taskIndex = _flightTaskManager.CurrentRunningTaskIndex; - // 如果当前任务编号属于跟随过程的任务 - if (taskIndex >= 2 && taskIndex <= 4) - { - if (index == 0 ) // 当前飞机编号属于领航的飞机 - { - followHead = 1; - } - else // 当前飞机编号属于跟随的飞机 - { - indexHead = index - 1; // 计算跟随的前方飞机编号 - startTaskIndex = 2; // 本次跟随过程开始任务编号 - endTaskIndex = 4; // 本次跟随过程结束任务编号 - info.isFinished = false;// 赋值结束本次task的初始标志位 - directionAngle = 0; // 与前方跟随飞机的角度，0为正北方向 - } - } - - if (taskIndex >= 18 && taskIndex <= 20) - { - if (index == 3 || index == 7 || index == 11) - { - followHead = 1; - } - else - { - indexHead = index + 1; - startTaskIndex = 18; - endTaskIndex = 20; - info.isFinished = false; - directionAngle = 180; - } - } - - /* - if (taskIndex >= 6 && taskIndex <= 6) - { - if (index == 0 || index == 5) - { - followHead = 1; - } - else - { - if (index % 2 == 0) - { - indexHead = index - 2; - directionAngle = 90; - } - else - { - indexHead = index + 2; - directionAngle = 270; - } - - startTaskIndex = 6; - endTaskIndex = 6; - info.isFinished = false; - } - } - - if (taskIndex >= 18 && taskIndex <= 20) - { - if (index == 1 || index == 3 || index == 5 || index == 7) - { - followHead = 1; - } - else - { - indexHead = index + 1; - } - } - - if (taskIndex >= 7 && taskIndex <= 9) - { - if (index == 0 || index == 7 ) - { - followHead = 1; - } - else - { - indexHead = index - 2; - } - } - - if (taskIndex >= 10 && taskIndex <= 12) - { - if (index == 1 || index == 6) - { - followHead = 1; - } - else - { - indexHead = index + 2; - } - } - */ - - // 如果不是被跟随的飞机 - if (followHead != 1) - { - var infoBefore = infos[indexHead]; // 跟随的飞机编号 - DateTime dtNow = DateTime.Now; // 记录当前时间 - DateTime dtLastTime = DateTime.Now; // 记录gps上次更新时间 - TimeSpan ts = dtNow - dtLastTime; // 记录gps是否更新时间段 - int flagRefresh = 1; // gps更新标志 - int flagStopTask = 0; // 如果飞机超过5s没有更新gps坐标，则认为本次跟随任务结束, 即flagStopTask>=10 - double GPSRate = 500; // 500 ms, gps更新时间 - - Tuple targetLatLng = null; // 更新的gps坐标 - - // int taskIndex = 0; - int stopFlag = 10; // 结束任务标志, 10即10*500ms = 5s - - // while (flagStopTask < stopFlag) - while (true) - { - taskIndex = _flightTaskManager.CurrentRunningTaskIndex; // 获取当前任务编号 - - if (flagRefresh == 1) // 需要更新gps坐标 - { - flagRefresh = 0; // 更新标志清零 - flagStopTask++; // 结束任务标志加1 - - // var direction = GeographyUtils.CalcDirection(info.Copter.Latitude, info.Copter.Longitude, infoBefore.Copter.Latitude, infoBefore.Copter.Longitude); - // targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway(infoBefore.Copter.Latitude, infoBefore.Copter.Longitude, (float)( direction), 5); - - // 计算目标经纬度 - targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(infoBefore.Copter.Latitude, infoBefore.Copter.Longitude, directionAngle, 8); - - // 判断飞机当前是否在目标范围内. 球半径1m - if (!info.Copter.ArrivedTarget(targetLatLng.Item1, targetLatLng.Item2, infoBefore.Copter.Altitude)) - { - flagStopTask = 0; - for (int j = 0; j < 3; j++) - { - // 发送目标坐标给飞控 - await info.Copter.FlyToAsync(targetLatLng.Item1, targetLatLng.Item2, infoBefore.Copter.Altitude); - } - } - - } - - // 如果没有在目标范围内，则一直在循环内。当更新gps标志置位，退出循环 - // 修改到达目标半径为1m，与飞控的判断条件相同, by ZJF - while (!info.Copter.ArrivedTarget(targetLatLng.Item1, targetLatLng.Item2, infoBefore.Copter.Altitude)) - { - if (_flightTaskManager.IsPaused == true) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(25).ConfigureAwait(false); - - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - if (ts.TotalMilliseconds > GPSRate) - { - break; - } - } - - // 如果1、已经是最后一步任务；2、结束任务标志位置位；3、跟随的飞机已经结束任务，则本飞机结束当前任务，且切换到悬停模式 - if ((taskIndex == _flightTaskManager.Tasks.Count() - 1) && (flagStopTask >= stopFlag) && infoBefore.isFinished) - // if ((taskIndex == _flightTaskManager.Tasks.Count() - 1) && (flagStopTask >= stopFlag) && _flightTaskManager.Tasks[taskIndex].SingleCopterInfos[indexHead].isFinished) - { - info.isFinished = true; - await info.Copter.HoverAsync(); - // return; - } - - // 如果1、已经是本次跟随过程的最后一步任务；2、结束任务标志位置位；3、跟随的飞机已经结束任务，则本飞机结束当前任务，退出循环 - if ((taskIndex == endTaskIndex) && (flagStopTask >= stopFlag) && infoBefore.isFinished) - // if ((taskIndex == endTaskIndex) && (flagStopTask >= stopFlag) && _flightTaskManager.Tasks[taskIndex].SingleCopterInfos[indexHead].isFinished) - { - info.isFinished = true; - break; - // return; - } - - // 如果任务暂停，则切换到悬停模式 - if (_flightTaskManager.IsPaused == true) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(20).ConfigureAwait(false); - - // 计算是否更新gps标志位 - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - if (ts.TotalMilliseconds > GPSRate) - { - flagRefresh = 1; - dtLastTime = dtNow; - } - } - } - else // 如果是被跟随的飞机 - { - DateTime dtNow = DateTime.Now; - DateTime dtLastTime = DateTime.Now; - TimeSpan ts = dtNow - dtLastTime; - - taskIndex = _flightTaskManager.CurrentRunningTaskIndex; - for (int j = 0; j < 3; j++) - await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt); - - while (!info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, info.TargetAlt)) - { - if (_flightTaskManager.IsPaused == true) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(25).ConfigureAwait(false); - - // 为了防止3次发送更新gps坐标失败，此处添加每隔1s重新发送目标gps坐标 - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - if (ts.TotalMilliseconds > 1000) - { - for (int j = 0; j < 3; j++) - await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt); - dtLastTime = dtNow; - } - } - - info.isFinished = true; - if (taskIndex == _flightTaskManager.Tasks.Count() - 1) - { - await info.Copter.HoverAsync(); - } - // await info.Copter.HoverAsync(); - } - - } - - public async Task RunFlyToOnlyAltTaskAsync() // 仅改变高度 + + public async Task RunFlyToTaskAsync() // 全部飞到指定航点 { + //是否有交错 if (StaggerRoutes) { var infos = SingleCopterInfos; @@ -442,103 +63,15 @@ namespace Plane.FormationCreator.Formation if (i1 > 0) { var prevCopter = infos[i1 - 1].Copter; + //检测平面是否有相交的线 while (CheckCrossing(infos, i1) && - prevCopter.Altitude - copter.Altitude < 2) + Math.Abs(prevCopter.Altitude - copter.Altitude) < 2) { await Task.Delay(25).ConfigureAwait(false); } } - await FlyToOnlyAltTaskFlySingleCopterAsync(info); - }); - } - await Task.WhenAll(tasks).ConfigureAwait(false); - } - else - { - await Task.WhenAll( - SingleCopterInfos.Select(info => FlyToOnlyAltTaskFlySingleCopterAsync(info)) - ).ConfigureAwait(false); - } - } - - private async Task FlyToOnlyAltTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) // 仅高度变化 - { - int taskIndex = _flightTaskManager.CurrentRunningTaskIndex; - int copterIndex = SingleCopterInfos.IndexOf(info); - double infoTargetLat = 0.0; - double infoTargetLng = 0.0; - - //if (taskIndex > 0 && taskIndex <= 2) - //{ - // infoTargetLat = _flightTaskManager.Tasks[0].SingleCopterInfos[copterIndex].TargetLat; - // infoTargetLng = _flightTaskManager.Tasks[0].SingleCopterInfos[copterIndex].TargetLng; - //} - - DateTime dtNow = DateTime.Now; - DateTime dtLastTime = DateTime.Now; - TimeSpan ts = dtNow - dtLastTime; - - infoTargetLat = info.Copter.Latitude; - infoTargetLng = info.Copter.Longitude; - - for (int i = 0; i < 3; i++) - { - await info.Copter.FlyToAsync(infoTargetLat, infoTargetLng, info.TargetAlt); - } - - while (!info.Copter.ArrivedTarget(infoTargetLat, infoTargetLng, info.TargetAlt)) - { - if (_flightTaskManager.IsPaused == true) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(25).ConfigureAwait(false); - - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - if (ts.TotalMilliseconds > 2000) - { - for (int i = 0; i < 3; i++) - { - await info.Copter.FlyToAsync(infoTargetLat, infoTargetLng, info.TargetAlt); - } - dtLastTime = dtNow; - } - } - // await info.Copter.HoverAsync(); - if (taskIndex == _flightTaskManager.Tasks.Count() - 1) - { - await info.Copter.HoverAsync(); - } - } - - public async Task RunFlyToTaskAsync() // 全部飞到指定航点 - { - //是否有交错 - if (StaggerRoutes) - { - var infos = SingleCopterInfos; - var tasks = new Task[infos.Count]; - for (int i = 0; i < infos.Count; i++) - { - var info = infos[i]; - - tasks[i] = await Task.Factory.StartNew(async () => - { - var internalInfo = info; - //if (i1 > 0) - //{ - // var prevCopter = infos[i1 - 1].Copter; - // while (CheckCrossing(infos, i1) && - // prevCopter.Altitude - copter.Altitude < 2) - // { - // await Task.Delay(25).ConfigureAwait(false); - // } - //} - - await FlyToTaskFlySingleCopterAsync(internalInfo); - }); + await FlyToTaskFlySingleCopterAsync(info); + }); } await Task.WhenAll(tasks).ConfigureAwait(false); @@ -551,92 +84,6 @@ namespace Plane.FormationCreator.Formation } } - /* - private async Task FlyToTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) - { - int taskIndex = _flightTaskManager.CurrentRunningTaskIndex; - int copterIndex = SingleCopterInfos.IndexOf(info); - - - DateTime dtNow = DateTime.Now; - DateTime dtLastTime = DateTime.Now; - TimeSpan ts = dtNow - dtLastTime; - - int pointNum = 10; - EHLocation[] targetLanLngArray = new EHLocation[pointNum]; - - if (taskIndex >= 2) - // if (false) - { - var copterPreviousTaskInfo = _flightTaskManager.Tasks[taskIndex - 1].SingleCopterInfos[copterIndex]; - var direction = GeographyUtils.CalcDirection2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, info.TargetLat, info.TargetLng); - var distance = GeographyUtils.CalcDistance(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, copterPreviousTaskInfo.TargetAlt, info.TargetLat, info.TargetLng, info.TargetAlt); - var altDiff = info.TargetAlt - copterPreviousTaskInfo.TargetAlt; - var horizontalDiff = GeographyUtils.CalcDistance2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, info.TargetLat, info.TargetLng); - - for (int ii = 0; ii < pointNum; ii++) - { - // var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, (float)(GeographyUtils.RadToDeg(direction)), (float)distance * (ii + 1) / pointNum); - var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, (float)(GeographyUtils.RadToDeg(direction)), (float)horizontalDiff * (ii + 1) / pointNum); - var targetAltTmp = copterPreviousTaskInfo.TargetAlt + altDiff * (ii + 1) / pointNum; - targetLanLngArray[ii] = new EHLocation(targetLatLng.Item1, targetLatLng.Item2, targetAltTmp); - } - - } - else - { - for (int ii = 0; ii < pointNum; ii++) - { - targetLanLngArray[ii] = new EHLocation(info.TargetLat,info.TargetLng, info.TargetAlt); - } - } - - int pointStage = 0; - while (pointStage < 10) - { - for (int i = 0; i < 3; i++) - { - // await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt); - await info.Copter.FlyToAsync(targetLanLngArray[pointStage].Latitude, targetLanLngArray[pointStage].Longitude, targetLanLngArray[pointStage].Altitude); - await Task.Delay(10).ConfigureAwait(false); - } - - while (!info.Copter.ArrivedTarget(targetLanLngArray[pointStage].Latitude, targetLanLngArray[pointStage].Longitude, targetLanLngArray[pointStage].Altitude)) - { - if (_flightTaskManager.IsPaused == true) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(25).ConfigureAwait(false); - - - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - if (ts.TotalMilliseconds > 2000) - { - for (int i = 0; i < 2; i++) - { - // await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt); - await info.Copter.FlyToAsync(targetLanLngArray[pointStage].Latitude, targetLanLngArray[pointStage].Longitude, targetLanLngArray[pointStage].Altitude); - await Task.Delay(10).ConfigureAwait(false); - } - dtLastTime = dtNow; - } - - } - - pointStage++; - } - - - // await info.Copter.HoverAsync(); - if (taskIndex == _flightTaskManager.Tasks.Count() - 1) - { - await info.Copter.HoverAsync(); - } - } - */ private async Task FlyToTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) { @@ -647,6 +94,7 @@ namespace Plane.FormationCreator.Formation if ((bool)_copterManager.CopterStatus[copterIndex]) return; + await info.Copter.SetShowLEDAsync(info.FlytoShowLED); if (info.Copter.State != Plane.Copters.CopterState.CommandMode) @@ -655,79 +103,38 @@ namespace Plane.FormationCreator.Formation DateTime dtNow = DateTime.Now; DateTime dtLastTime = DateTime.Now; TimeSpan ts = dtNow - dtLastTime; - - int pointNum = 10; - PLLocation[] targetLanLngArray = new PLLocation[pointNum]; - - // if (taskIndex >= 2) // 将地图上两个目标点分成10份 - if (false) // 只发送最终目标点 + //发送2次目标航点 + for (int i = 0; i < 2; i++) { - var copterPreviousTaskInfo = _flightTaskManager.Tasks[taskIndex - 1].SingleCopterInfos[copterIndex]; - var direction = GeographyUtils.CalcDirection2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, info.TargetLat, info.TargetLng); - var distance = GeographyUtils.CalcDistance(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, copterPreviousTaskInfo.TargetAlt, info.TargetLat, info.TargetLng, info.TargetAlt); - var altDiff = info.TargetAlt - copterPreviousTaskInfo.TargetAlt; - var horizontalDiff = GeographyUtils.CalcDistance2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, info.TargetLat, info.TargetLng); - - for (int ii = 0; ii < pointNum; ii++) - { - // var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, (float)(GeographyUtils.RadToDeg(direction)), (float)distance * (ii + 1) / pointNum); - var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, (float)(GeographyUtils.RadToDeg(direction)), (float)horizontalDiff * (ii + 1) / pointNum); - var targetAltTmp = copterPreviousTaskInfo.TargetAlt + altDiff * (ii + 1) / pointNum; - targetLanLngArray[ii] = new PLLocation(targetLatLng.Item1, targetLatLng.Item2, targetAltTmp); - } - + await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt); + await Task.Delay(20).ConfigureAwait(false); } - else - { - for (int ii = 0; ii < pointNum; ii++) - { - targetLanLngArray[ii] = new PLLocation(info.TargetLat,info.TargetLng, info.TargetAlt); - } - } - - int pointStage = 0; - for (int i = 0; i < 5; i++) - { - // await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt); - await info.Copter.FlyToAsync(targetLanLngArray[pointStage].Latitude, targetLanLngArray[pointStage].Longitude, targetLanLngArray[pointStage].Altitude); - await Task.Delay(10).ConfigureAwait(false); - } - pointStage++; - + info.Copter.MissionStatus = 1; + //判断是否到达 while (!info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, info.TargetAlt)) { if (_flightTaskManager.IsPaused == true) { + info.Copter.MissionStatus = 0; await info.Copter.HoverAsync(); return; } - await Task.Delay(25).ConfigureAwait(false); //判断是否到达位置20hz - - dtNow = DateTime.Now; ts = dtNow - dtLastTime; - if (ts.TotalMilliseconds > 500) // 每500ms发送一遍指点坐标 + if (ts.TotalMilliseconds > 1000) // 每1000ms发送一遍指点坐标 { - for (int i = 0; i < 2; i++) - { - // await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt); - await info.Copter.FlyToAsync(targetLanLngArray[pointStage].Latitude, targetLanLngArray[pointStage].Longitude, targetLanLngArray[pointStage].Altitude); - await Task.Delay(10).ConfigureAwait(false); - } - pointStage++; - if (pointStage == pointNum) - { - pointStage = pointNum - 1; - } + await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt); dtLastTime = dtNow; } - + await Task.Delay(25).ConfigureAwait(false); //判断是否到达位置20hz } - // await info.Copter.HoverAsync(); + info.Copter.MissionStatus = 0; + if (taskIndex == _flightTaskManager.Tasks.Count() - 1) { await info.Copter.HoverAsync(); } + } diff --git a/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs b/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs index 5b52156..767b860 100644 --- a/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs +++ b/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs @@ -325,11 +325,12 @@ namespace Plane.FormationCreator.Formation { await info.Copter.FlyToAsync(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt); } - + info.Copter.MissionStatus = 1; while (!info.Copter.ArrivedTarget(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt)) { if (_flightTaskManager.IsPaused == true) { + info.Copter.MissionStatus = 0; await info.Copter.HoverAsync(); return; } @@ -346,6 +347,8 @@ namespace Plane.FormationCreator.Formation dtLastTime = dtNow; } } + info.Copter.MissionStatus = 0; + info.RTLStage++; } @@ -362,11 +365,13 @@ namespace Plane.FormationCreator.Formation { await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt); } + info.Copter.MissionStatus = 1; while (!info.Copter.ArrivedTarget(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt)) { if (_flightTaskManager.IsPaused == true) { + info.Copter.MissionStatus = 0; await info.Copter.HoverAsync(); return; } @@ -383,6 +388,7 @@ namespace Plane.FormationCreator.Formation dtLastTime = dtNow; } } + info.Copter.MissionStatus = 0; info.RTLStage++; } diff --git a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs index 08c1218..52cfe28 100644 --- a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs +++ b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs @@ -271,11 +271,13 @@ namespace Plane.FormationCreator.Formation await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt); await Task.Delay(10).ConfigureAwait(false); } + info.Copter.MissionStatus = 1; //直到到达第一个航点并高度15米 while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt)) { if (_flightTaskManager.IsPaused == true) { + info.Copter.MissionStatus = 0; await info.Copter.HoverAsync(); return; } @@ -294,10 +296,14 @@ namespace Plane.FormationCreator.Formation dtLastTime = dtNow; } } + info.Copter.MissionStatus = 0; + if (_flightTaskManager.IsPaused == false) { info.takeOffStage++; //当前变为2 } + info.Copter.MissionStatus = 0; + } dtNow = DateTime.Now; @@ -312,11 +318,15 @@ namespace Plane.FormationCreator.Formation await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt); await Task.Delay(10).ConfigureAwait(false); } + info.Copter.MissionStatus = 1; + //直到到达第一个航点 while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt)) { if (_flightTaskManager.IsPaused == true) { + info.Copter.MissionStatus = 0; + await info.Copter.HoverAsync(); return; } @@ -335,8 +345,10 @@ namespace Plane.FormationCreator.Formation dtLastTime = dtNow; } } + info.Copter.MissionStatus = 0; + } - + // }); } diff --git a/Plane.FormationCreator/Plane.FormationCreator.csproj b/Plane.FormationCreator/Plane.FormationCreator.csproj index 4f2d561..07905a4 100644 --- a/Plane.FormationCreator/Plane.FormationCreator.csproj +++ b/Plane.FormationCreator/Plane.FormationCreator.csproj @@ -139,6 +139,9 @@ + + ModifyParam.xaml + @@ -211,6 +214,10 @@ Designer MSBuild:Compile + + Designer + MSBuild:Compile + Designer MSBuild:Compile diff --git a/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs b/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs index 645fc34..a42bb5e 100644 --- a/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs @@ -140,6 +140,51 @@ namespace Plane.FormationCreator.ViewModels } } + + private ICommand _ParamModify; + public ICommand ParamModify + { + get + { + return _ParamModify ?? (_ParamModify = new RelayCommand(async () => + { + + var ModifyParamWindow = new Plane.FormationCreator.ModifyParam(); + if (ModifyParamWindow.ShowDialog() == true) + { + + string paramstr = ModifyParamWindow.textParamName.Text; + float paramvalue = Convert.ToSingle(ModifyParamWindow.textParamValue.Text); + + await Task.WhenAll(_copterManager.AcceptingControlCopters.Select + (copter => copter.SetParamAsync(paramstr, paramvalue) )); + + bool ifallok = true; + await Task.WhenAll( + + _copterManager.AcceptingControlCopters.Select(async copter => + { + float getvaule = await copter.GetParamAsync(paramstr); + if (getvaule != paramvalue) + { + ifallok = false; + Alert.Show(copter.Id+ "设置失败,读取的参数是"+ getvaule, "提示", MessageBoxButton.OK, MessageBoxImage.Information); + } + } + + )); + if (ifallok) + Alert.Show("所有已选的飞机参数设置完成", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + else + Alert.Show("有部分飞机参数设置失败，请检查并再次设置", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + + } + })); + } + } + + + private ICommand _ReturnToLaunchCommand; public ICommand ReturnToLaunchCommand { @@ -255,12 +300,14 @@ namespace Plane.FormationCreator.ViewModels //分发到每个飞机 foreach (var copter in _copterManager.Copters) { - await copter.InjectGpsDataAsync(packet, (ushort)packet.Length); + //RTK定位才发送 + if (copter.LocationType == CopterLocationType.RTK) + { + await copter.InjectGpsDataAsync(packet, (ushort)packet.Length); + await Task.Delay(10).ConfigureAwait(false); //为了在时间上均摊RTK数据包 + } } - - await Task.Delay(200).ConfigureAwait(false); - } }).ConfigureAwait(false); Alert.Show("RTK数据停止发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information); diff --git a/Plane.FormationCreator/Views/ControlPanelView.xaml b/Plane.FormationCreator/Views/ControlPanelView.xaml index cf615ef..8d4d056 100644 --- a/Plane.FormationCreator/Views/ControlPanelView.xaml +++ b/Plane.FormationCreator/Views/ControlPanelView.xaml @@ -40,6 +40,8 @@ Command="{Binding HoverCommand}" />