diff --git a/Plane.FormationCreator/Formation/FlightTask.cs b/Plane.FormationCreator/Formation/FlightTask.cs index 5e90ca2..de65925 100644 --- a/Plane.FormationCreator/Formation/FlightTask.cs +++ b/Plane.FormationCreator/Formation/FlightTask.cs @@ -163,7 +163,11 @@ namespace Plane.FormationCreator.Formation get { return _ModifyingSingleCopterInfo; } set { Set(nameof(ModifyingSingleCopterInfo), ref _ModifyingSingleCopterInfo, value); } } - + /// + /// 实际执行某一个任务--包含等待 + /// + /// + /// public async Task RunAsync() { switch (TaskType) @@ -172,8 +176,6 @@ namespace Plane.FormationCreator.Formation await RunFlyToTaskAsync().ConfigureAwait(false); break; case FlightTaskType.TakeOff: - //多架同时起飞 - //await MutilRunTakeOffTaskAsync().ConfigureAwait(false); await NewMutilRunTakeOffTaskAsync().ConfigureAwait(false); break; case FlightTaskType.Land: diff --git a/Plane.FormationCreator/Formation/FlightTaskManager.cs b/Plane.FormationCreator/Formation/FlightTaskManager.cs index f039dd8..820f907 100644 --- a/Plane.FormationCreator/Formation/FlightTaskManager.cs +++ b/Plane.FormationCreator/Formation/FlightTaskManager.cs @@ -22,6 +22,10 @@ using Plane.CommunicationManagement; using Microsoft.Practices.ServiceLocation; using Plane.FormationCreator.ViewModels; using MahApps.Metro.Controls; +using Newtonsoft.Json.Linq; +using GMap.NET.MapProviders; +using GMap.NET; +using static Plane.FormationCreator.CalculationLogLatDistance; namespace Plane.FormationCreator.Formation { @@ -648,7 +652,21 @@ namespace Plane.FormationCreator.Formation // SelectedTaskIndex = 0; } - + //模拟紧急返航 + public async Task sim_DoMissionEmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance,int rettime, bool descending) + { + var copters = _copterManager.Copters; + if (TaskState == TasksStatus.Stop) + return; + IsEmergencyRet = true; + //设置所有模拟飞机的位置 + for (int j = 0; j < copters.Count; j++) + { + var copter = copters[j]; + var fc = copter as FakeCopter; + await fc.EmergencyRetAsync(takeoffcentloc, taskcentloc, mindistance, rettime, descending); + } + } public async Task ForceNextTasks() { @@ -2199,6 +2217,15 @@ namespace Plane.FormationCreator.Formation } return ret; } + CancellationTokenSource cts ; + + //绕行取消，用于3D绕行计算 + public void cancel_ABypassBAsync() + { + if (cts!=null) + cts.Cancel(); //取消掉 异步执行的 绕行函数 + } + //新开线程异步调用ABypassB并等待返回 public async Task<(List>,bool)> ABypassBAsync(FlightRouteV2.Vector3[] aVecs, FlightRouteV2.Vector3[] bVecs) { @@ -2207,7 +2234,8 @@ namespace Plane.FormationCreator.Formation var task = Task.Run(() => { - ret = FlyVecFun.ABypassB(aVecs, bVecs, Routecallback, Cronograma, out isPasstmp); + cts = new CancellationTokenSource(); + ret = FlyVecFun.ABypassB(aVecs, bVecs, Routecallback, Cronograma, cts.Token, out isPasstmp); }); try { @@ -2852,8 +2880,24 @@ namespace Plane.FormationCreator.Formation Message.Show($"经纬方式计算用时：{stopWatch.Elapsed.TotalMilliseconds}ms,总飞行距离{sumlength}米,有{crosscount}个交叉"); } - - //计算中心点 (三维的) + //计算一个图案的重心点 (三维的) + private PLLocation CenterGravityLatLng(List points) + { + double aLatitude = 0; + double aLongitude = 0; + double aAlt = 0; + foreach (var item in points) + { + aLatitude += item.Latitude; + aLongitude += item.Longitude; + aAlt += item.Altitude; + } + aLatitude = aLatitude / points.Count; + aLongitude = aLongitude / points.Count; + aAlt = aAlt / points.Count; + return new PLLocation(aLatitude, aLongitude, (float)aAlt); + } + //计算两个图案的共同几何中心点 (三维的) private PLLocation CenterLatLng(List point1, List point2) { PLLocation centerLatLng = new PLLocation(0,0,0); @@ -3195,8 +3239,30 @@ namespace Plane.FormationCreator.Formation } } + private bool _IsEmergencyRet=false; + public bool IsEmergencyRet + { + get { return _IsEmergencyRet; } + private set + { + if (Set(nameof(IsEmergencyRet), ref _IsEmergencyRet, value)) + { + if (_IsEmergencyRet) + { + MessageText = "任务紧急返航中！"; + } + else + { + MessageText = "任务运行中"; + } + } + + + } + } + TimeSpan timeSpan; - DateTime taskStartTime; + public DateTime taskStartTime; public async Task RunTaskAsync() { if (Tasks.Count == 0) return; @@ -3249,10 +3315,14 @@ namespace Plane.FormationCreator.Formation } } - + /// + /// 开始运行模拟任务 + /// + /// public async Task RunAsync() { IsPaused = false; + IsEmergencyRet = false; for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++) { var task = Tasks[i]; @@ -3261,6 +3331,10 @@ namespace Plane.FormationCreator.Formation } AppEx.Current.AppMode = AppMode.RunningTasks; StartAvoidingCrash(); //开始碰撞检测 + + //告诉所有飞机开始任务--模拟飞机不计算起飞延迟，直接传0 + foreach (var copter in _copterManager.Copters) + await copter.MissionStartAsync(0, 0, 0, 0, 0); TaskState = TasksStatus.Running; for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++) { @@ -3269,16 +3343,13 @@ namespace Plane.FormationCreator.Formation task.Status = FlightTaskStatus.Running; CurrentRunningTask = task; CurrentRunningTaskIndex = i; - - //////////////////显示提示信息 + //显示提示信息 int starttime = 0; for (int j = 0; j < task.TaskIndex; j++) starttime += GetTaskTime(j); TimeSpan ts = new TimeSpan(0, 0, Convert.ToInt32(starttime)); Message.Show($"{ts}:任务{i+1} {task.TaskCnName } 开始执行，需{ GetTaskTime(task.TaskIndex)}秒"); - ///////////////////////// - - + //开始执行任务 await task.RunAsync().ConfigureAwait(false); // 1. 被暂停时，中断 RunAsync。继续运行时将把此时运行了一半的 CurrentRunningTask 重新运行一遍。 if (IsPaused == true) @@ -3288,10 +3359,17 @@ namespace Plane.FormationCreator.Formation Message.Show($"任务{i + 1} {task.TaskCnName } 暂停执行"); return; } + //紧急返航中-跳出任务循环 + if (IsEmergencyRet == true) break; task.Status = FlightTaskStatus.Stop; } - // 2. 正常结束时，重置 CurrentRunningTask、CurrentRunningTaskIndex 和 IsPaused。 + //紧急返航中-等待返航结束 + while (IsEmergencyRet) { + //延时10ms等待 + await Task.Delay(10); + } + // 2. 正常结束时，重置 CurrentRunningTask、CurrentRunningTaskIndex 和 IsPaused。 TaskState = TasksStatus.Stop; CurrentRunningTask = null; CurrentRunningTaskIndex = 0; @@ -3306,11 +3384,179 @@ namespace Plane.FormationCreator.Formation #endregion Run and pause. + //计算飞行一段时间后的位置 + private PLLocation TaskFlyLoc(PLLocation startloc, PLLocation tarloc,int currflytime,int flytime ) + { + PLLocation flyLatLng; + if (currflytime >= flytime) + { + flyLatLng=new PLLocation(tarloc.Latitude, tarloc.Longitude, tarloc.Altitude); + return flyLatLng; + } + double _direction = startloc.CalcDirection2D(tarloc.Latitude, tarloc.Longitude); + double _Lng_delta = (float)(Math.Sin(_direction) * GeographyUtils.CalcMetersToLngSpan(tarloc.Latitude)); + double _Lat_delta = (float)(Math.Cos(_direction) * GeographyUtils.METERS_TO_LAT_SPAN); + + //计算xy和z方向距离 + float _distance_xy = (float)startloc.CalcDistance2D(tarloc); + float _distance_z = tarloc.Altitude - startloc.Altitude; + + float currdis_xy= _distance_xy/ flytime* currflytime; + float currdis_z = _distance_z / flytime * currflytime; + // 更新位置 + float Altitude = startloc.Altitude + currdis_z; + double Longitude = startloc.Longitude + currdis_xy * _Lng_delta; + double Latitude = startloc.Latitude + currdis_xy * _Lat_delta; + flyLatLng = new PLLocation(Latitude, Longitude, Altitude); + return flyLatLng; + } + + + + + + + //计算紧急返航数据 + //rettime :返航时间点--从起飞开始计算的秒数 + //takeoffcentloc:起飞图案重心点 + //taskcentloc:返航图案重心点 + //mindistance:离起飞重心点最近飞机距离 + //overlapping:两个图案是否有重叠--用来决定飞机是先降低高度还是先横着飞 + //返回值：是否可以计算返航--起飞不能用这个函数，直接降落 + public bool EmergencyRet(int rettime, out PLLocation takeoffcentloc, out PLLocation taskcentloc, out double mindistance, out bool overlapping) + { + mindistance = 0; + int starttime = 0; + int flyedtasktime = 0; + int rettaskindex = -1; + int taskflytime = 0; + taskcentloc = null; + overlapping = false; + + double takeoffmaxlat=0; + double takeoffmaxlng = 0; + double takeoffminlat = double.MaxValue; + double takeoffminlng = double.MaxValue; + double taskmaxlat = 0; + double taskmaxlng = 0; + double taskminlat = double.MaxValue; + double taskminlng = double.MaxValue; + + + + + Dictionary curTaskPoint = new Dictionary(); + + //计算地面起飞矩阵的中心点 + for (int i = 0; i < _copterManager.Copters.Count; i++) + { + //起飞任务 + var curinfo = Tasks[0].SingleCopterInfos[i]; + PLLocation curLoc = new PLLocation(curinfo.TargetLat, curinfo.TargetLng,0); + if (curinfo.TargetLat > takeoffmaxlat) + takeoffmaxlat = curinfo.TargetLat; + if (curinfo.TargetLng > takeoffmaxlng) + takeoffmaxlng = curinfo.TargetLng; + if (curinfo.TargetLat < takeoffminlat) + takeoffminlat = curinfo.TargetLat; + if (curinfo.TargetLng < takeoffminlng) + takeoffminlng = curinfo.TargetLng; + curTaskPoint.Add(i, curLoc); + } + takeoffcentloc = CenterGravityLatLng(curTaskPoint.Values.ToList()); + curTaskPoint.Clear(); + + //计算返航时间正在飞行哪个航点，飞了多久 + for (int i = 0; i < Tasks.Count; i++) + { + flyedtasktime= GetTaskTime(i); + if ((starttime+ flyedtasktime) > rettime) + { + rettaskindex = i; + taskflytime = rettime - starttime; + } + starttime += flyedtasktime; + } + //起飞不能用这个函数，直接降落 + if (rettaskindex<1) return false; + mindistance = double.MaxValue; + + //计算悬停图案的重心点 + for (int i = 0; i < _copterManager.Copters.Count; i++) + { + var curinfo = Tasks[rettaskindex].SingleCopterInfos[i]; + var prvcurinfo = Tasks[rettaskindex - 1].SingleCopterInfos[i]; + PLLocation curLoc; + if (taskflytime < Tasks[rettaskindex].FlytoTime) + { + PLLocation coptercurLoc = new PLLocation(curinfo.TargetLat, curinfo.TargetLng, curinfo.TargetAlt); + PLLocation copterprvLoc = new PLLocation(prvcurinfo.TargetLat, prvcurinfo.TargetLng, prvcurinfo.TargetAlt); + //计算飞行一段时间后的位置 + curLoc = TaskFlyLoc(copterprvLoc, coptercurLoc, taskflytime, Tasks[rettaskindex].FlytoTime); + } + else //正在悬停 + curLoc = new PLLocation(curinfo.TargetLat, curinfo.TargetLng, 0); + + curTaskPoint.Add(i, curLoc); + if (curinfo.TargetLat > taskmaxlat) + taskmaxlat = curinfo.TargetLat; + if (curinfo.TargetLng > taskmaxlng) + taskmaxlng = curinfo.TargetLng; + if (curinfo.TargetLat < taskminlat) + taskminlat = curinfo.TargetLat; + if (curinfo.TargetLng < taskminlng) + taskminlng = curinfo.TargetLng; + double taskdistance = takeoffcentloc.CalcDistance(curLoc); + if (taskdistance < mindistance) + mindistance = taskdistance; + } + taskcentloc = CenterGravityLatLng(curTaskPoint.Values.ToList()); + curTaskPoint.Clear(); + + //判断两个图案是否有重叠 + overlapping = DoRectanglesOverlap(takeoffminlat, takeoffminlng, takeoffmaxlat, takeoffmaxlng, + taskminlat, taskminlng, taskmaxlat, taskmaxlng); + + + + return true; + } + + /// + /// 判断两个矩形是否有重叠 + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + static bool DoRectanglesOverlap(double minLat1, double minLng1, double maxLat1, double maxLng1, + double minLat2, double minLng2, double maxLat2, double maxLng2) + { + // 判断第一个矩形在第二个矩形的左边或右边 + if (minLng1 > maxLng2 || minLng2 > maxLng1) + { + return false; + } + + // 判断第一个矩形在第二个矩形的上边或下边 + if (minLat1 > maxLat2 || minLat2 > maxLat1) + { + return false; + } + + return true; + } + + Dictionary AvoidCrashLog = new Dictionary(); - - + private async void StartAvoidingCrash() { diff --git a/Plane.FormationCreator/Formation/FlightTaskStatus.cs b/Plane.FormationCreator/Formation/FlightTaskStatus.cs index 169c4fb..5c4cbb1 100644 --- a/Plane.FormationCreator/Formation/FlightTaskStatus.cs +++ b/Plane.FormationCreator/Formation/FlightTaskStatus.cs @@ -1,13 +1,13 @@ -public enum FlightTaskStatus -{ - Stop, - Running, - Paused, - Selected -} -public enum TasksStatus -{ - Stop, - Running, - Paused +public enum FlightTaskStatus +{ + Stop, + Running, + Paused, + Selected +} +public enum TasksStatus +{ + Stop, + Running, + Paused } \ No newline at end of file diff --git a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs index 48c2770..057a0db 100644 --- a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs +++ b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs @@ -1,262 +1,212 @@ -using Plane.Copters; -using Plane.Geography; -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; -using Plane.Windows.Messages; - -namespace Plane.FormationCreator.Formation -{ - public partial class FlightTask - { - private bool _StaggerRoutes = true; - - //是否有交错 - public bool StaggerRoutes - { - get { return _StaggerRoutes; } - set { Set(nameof(StaggerRoutes), ref _StaggerRoutes, value); } - } - - //同一个任务每一架飞机的FlytoTime和LoiterTime保持统一 - private int _FlytoTime = 10; - public int FlytoTime - { - get { return _FlytoTime; } - set - { - if (value > 4095) value = 4095; - if (value < 0) value = 0; - Set(nameof(FlytoTime), ref _FlytoTime, value); - } - } - private int _LoiterTime = 1; - public int LoiterTime - { - get { return _LoiterTime; } - set - { - if (value > 4095) value = 4095; - if (value < 0) value = 0; - Set(nameof(LoiterTime), ref _LoiterTime, value); - } - - } - - - private bool _VerticalLift = false; - public bool VerticalLift // 垂直升降标志位，后面需要加入即使拖动地图上的飞机，也不会变化经纬度. added by ZJF - { - get { return _VerticalLift; } - set - { - if (value) - { - int currentIndex = _flightTaskManager.SelectedTaskIndex; - var currentCopterInfos = _flightTaskManager.SelectedTask.SingleCopterInfos; - var previousCopterInfos = _flightTaskManager.Tasks[currentIndex - 1].SingleCopterInfos; - for (int i = 0; i < currentCopterInfos.Count; i++) - { - currentCopterInfos[i].TargetLat = previousCopterInfos[i].TargetLat; - currentCopterInfos[i].TargetLng = previousCopterInfos[i].TargetLng; - currentCopterInfos[i].TargetAlt = previousCopterInfos[i].TargetAlt; - } - } - - Set(nameof(VerticalLift), ref _VerticalLift, value); - } - } - - - - - - - - - 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 Task.WhenAll(tasks).ConfigureAwait(false); - - } - else - { - await Task.WhenAll( - SingleCopterInfos.Select(info => FlyToTaskFlySingleCopterAsync(info)) - ).ConfigureAwait(false); - } - } - - - // private int RuningTaskRemaining = 0; - - private async Task FlyToTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) - { - DateTime dtNow = DateTime.Now; - DateTime dtLastTime = DateTime.Now; - TimeSpan ts = dtNow - dtLastTime; - - - FlightTask task = _flightTaskManager.CurrentRunningTask; - int flyToTime = task.FlytoTime * 1000; - int loiterTime = task.LoiterTime * 1000; - int taskIndex = _flightTaskManager.CurrentRunningTaskIndex; - int copterIndex = SingleCopterInfos.IndexOf(info); - - - // 当该飞机被标记时，悬停并跳过飞行任务 - if ((bool)_copterManager.CopterStatus[copterIndex]) - { - await info.Copter.HoverAsync(); - return; - } - - await info.Copter.SetShowLEDAsync(info.FlytoShowLED); - - if (info.Copter.State != Plane.Copters.CopterState.CommandMode) - await info.Copter.GuidAsync(); - - - double targetLat = info.TargetLat; - double targetLng = info.TargetLng; - if (info.IsLandWaypoint) - { - targetLat = info.Copter.TakeOffPoint.Latitude; - targetLng = info.Copter.TakeOffPoint.Longitude; - } - int flytype = _flightTaskManager.getflytype(taskIndex); - await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt, task.FlytoTime, flytype); - // await Task.Delay(10).ConfigureAwait(false); - - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - - // int sendFlyToTimes = 0; - - /* - //第0个任务为takeoff - if (taskIndex > 0) - { - FlightTask prevTask = _flightTaskManager.Tasks[taskIndex - 1]; - if (prevTask.TaskType == FlightTaskType.FlyTo && prevTask.LoiterTime == 0) - flyToTime += prevTask.RuningTaskRemaining; - } - */ - - //while (!info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, info.TargetAlt)) //按航点飞 ：所有Copter到达目标点开始飞下个航点 - while (ts.TotalMilliseconds < (flyToTime + loiterTime)) //按时间轴飞：当前任务时间到达后自动飞往下个航点 - { - /* - //悬停时间等于0为快速航点 到达之后立即出发下个航点 切时间累积 - if (loiterTime == 0 && - info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, info.TargetAlt)) - { - task.RuningTaskRemaining = flyToTime - (int)ts.TotalMilliseconds; - break; - } - */ - if (_flightTaskManager.IsPaused == true) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(100).ConfigureAwait(false); //判断是否到达位置10hz - - - - // if (info.LEDInfos.Count > 0) - // { - // string LEDRGB = ""; - // List LedControl = info.LEDInfos.OrderBy(i=>i.Delay).ToList(); - // for (int i = 0; i < LedControl.Count; i++) - // { - // var led = LedControl[i]; - // if (ts.TotalMilliseconds >= led.Delay * 1000) - // LEDRGB = info.LEDInfos[i].LEDRGB; - // else - // break; - // } - // info.Copter.LEDColor = LEDRGB; - // - // } - - if (info.LEDInfos.Count > 0) - { - string LEDRGB = ""; - List LedControl = info.LEDInfos.ToList(); - double time = 0; - for (int i = 0; i < LedControl.Count; i++) - { - - var led = LedControl[i]; - time += led.Delay * 1000; - if (ts.TotalMilliseconds >= time) - LEDRGB = info.LEDInfos[i].LEDRGB; - else - break; - } - info.Copter.LEDColor = LEDRGB; - - } - - - // 当该飞机被标记时，悬停并跳过飞行任务 - if ((bool)_copterManager.CopterStatus[copterIndex]) - { - await info.Copter.HoverAsync(); - return; - } - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - } - // await info.Copter.HoverAsync(); - if (taskIndex == _flightTaskManager.Tasks.Count() - 1) - { - await info.Copter.HoverAsync(); - } - } - - - private static bool CheckCrossing(List infos, int currentIndex) - { - var info = infos[currentIndex]; - for (int i = 0; i < currentIndex; i++) - { - var nextInfo = infos[i]; - if (GeographyUtils.CheckCrossing2D(info.Copter.Latitude, info.Copter.Longitude, info.TargetLat, info.TargetLng, - nextInfo.Copter.Latitude, nextInfo.Copter.Longitude, nextInfo.TargetLat, nextInfo.TargetLng)) - { - return true; - } - } - return false; - } - } -} +using Plane.Copters; +using Plane.Geography; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Windows.Messages; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTask + { + private bool _StaggerRoutes = true; + + //是否有交错 + public bool StaggerRoutes + { + get { return _StaggerRoutes; } + set { Set(nameof(StaggerRoutes), ref _StaggerRoutes, value); } + } + + //同一个任务每一架飞机的FlytoTime和LoiterTime保持统一 + private int _FlytoTime = 10; + public int FlytoTime + { + get { return _FlytoTime; } + set + { + if (value > 4095) value = 4095; + if (value < 0) value = 0; + Set(nameof(FlytoTime), ref _FlytoTime, value); + } + } + private int _LoiterTime = 1; + public int LoiterTime + { + get { return _LoiterTime; } + set + { + if (value > 4095) value = 4095; + if (value < 0) value = 0; + Set(nameof(LoiterTime), ref _LoiterTime, value); + } + + } + + + private bool _VerticalLift = false; + public bool VerticalLift // 垂直升降标志位，后面需要加入即使拖动地图上的飞机，也不会变化经纬度. added by ZJF + { + get { return _VerticalLift; } + set + { + if (value) + { + int currentIndex = _flightTaskManager.SelectedTaskIndex; + var currentCopterInfos = _flightTaskManager.SelectedTask.SingleCopterInfos; + var previousCopterInfos = _flightTaskManager.Tasks[currentIndex - 1].SingleCopterInfos; + for (int i = 0; i < currentCopterInfos.Count; i++) + { + currentCopterInfos[i].TargetLat = previousCopterInfos[i].TargetLat; + currentCopterInfos[i].TargetLng = previousCopterInfos[i].TargetLng; + currentCopterInfos[i].TargetAlt = previousCopterInfos[i].TargetAlt; + } + } + + Set(nameof(VerticalLift), ref _VerticalLift, value); + } + } + + + + + + + + + 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; + await FlyToTaskFlySingleCopterAsync(internalInfo); + }); + } + await Task.WhenAll(tasks).ConfigureAwait(false); + + } + else + { + await Task.WhenAll( + SingleCopterInfos.Select(info => FlyToTaskFlySingleCopterAsync(info)) + ).ConfigureAwait(false); + } + } + + + // private int RuningTaskRemaining = 0; + /// + /// 单独飞机执行飞行任务 + /// + /// + /// + private async Task FlyToTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) + { + DateTime dtNow = DateTime.Now; + DateTime dtLastTime = DateTime.Now; + TimeSpan ts = dtNow - dtLastTime; + + + FlightTask task = _flightTaskManager.CurrentRunningTask; + int flyToTime = task.FlytoTime * 1000; + int loiterTime = task.LoiterTime * 1000; + int taskIndex = _flightTaskManager.CurrentRunningTaskIndex; + int copterIndex = SingleCopterInfos.IndexOf(info); + + + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } + + await info.Copter.SetShowLEDAsync(info.FlytoShowLED); + + if (info.Copter.State != Plane.Copters.CopterState.CommandMode) + await info.Copter.GuidAsync(); + + + double targetLat = info.TargetLat; + double targetLng = info.TargetLng; + if (info.IsLandWaypoint) + { + targetLat = info.Copter.TakeOffPoint.Latitude; + targetLng = info.Copter.TakeOffPoint.Longitude; + } + int flytype = _flightTaskManager.getflytype(taskIndex); + //指定目标位置 + await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt, task.FlytoTime, flytype); + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + //等待时间到达，并执行灯光模拟--不移动飞机，移动飞机是飞机自己计算的 + while (ts.TotalMilliseconds < (flyToTime + loiterTime)) //按时间轴飞：当前任务时间到达后自动飞往下个航点 + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(100).ConfigureAwait(false); //判断是否到达位置10hz + //执行灯光模拟 + if (info.LEDInfos.Count > 0) + { + string LEDRGB = ""; + List LedControl = info.LEDInfos.ToList(); + double time = 0; + for (int i = 0; i < LedControl.Count; i++) + { + + var led = LedControl[i]; + time += led.Delay * 1000; + if (ts.TotalMilliseconds >= time) + LEDRGB = info.LEDInfos[i].LEDRGB; + else + break; + } + info.Copter.LEDColor = LEDRGB; + } + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + } + // await info.Copter.HoverAsync(); + if (taskIndex == _flightTaskManager.Tasks.Count() - 1) + { + await info.Copter.HoverAsync(); + } + } + + + private static bool CheckCrossing(List infos, int currentIndex) + { + var info = infos[currentIndex]; + for (int i = 0; i < currentIndex; i++) + { + var nextInfo = infos[i]; + if (GeographyUtils.CheckCrossing2D(info.Copter.Latitude, info.Copter.Longitude, info.TargetLat, info.TargetLng, + nextInfo.Copter.Latitude, nextInfo.Copter.Longitude, nextInfo.TargetLat, nextInfo.TargetLng)) + { + return true; + } + } + return false; + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs index a536192..9648002 100644 --- a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs +++ b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs @@ -1,487 +1,486 @@ -using Plane.Copters; -using Microsoft.Practices.ServiceLocation; -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; -using Plane.Geography; - -namespace Plane.FormationCreator.Formation -{ - public partial class FlightTask - { - - - //起飞到第一个航点高度的飞行时间 - private byte _TakeOffTime = 10; - public byte TakeOffTime - { - get { return _TakeOffTime; } - set { Set(nameof(TakeOffTime), ref _TakeOffTime, value); } - } - - //一架架起飞--未使用 - public async Task RunTakeOffTaskAsync() - { - // float takeOffAlt = 15; - int TaskCount = _flightTaskManager.Tasks.Count(); - if (TaskCount > 1) - { - var infos = SingleCopterInfos; - // var tasks = new Task[infos.Count]; - var tasksTmp = new Task[infos.Count]; - for (int i = 0; i < infos.Count; i++) - { - var info = infos[i]; - - if (info.takeOffStage == 0) // 第一阶段：起飞到10m - { - await TakeOffTaskFlySingleCopterAsync(info); - //if (_flightTaskManager.IsPaused == false) - //{ - // info.takeOffStage++; - //} - } - - tasksTmp[i] = TakeOffSecondTaskAsync(info); // 第二和第三阶段 - } - await Task.WhenAll(tasksTmp).ConfigureAwait(false); - if (_flightTaskManager.IsPaused == false) - { - for (int i = 0; i < infos.Count; i++) - { - var info = infos[i]; - info.takeOffStage = 0; - } - } - - } - } - - //新版的起飞方案 - public async Task NewMutilRunTakeOffTaskAsync() - { - var infos = SingleCopterInfos; - var tasksTakeOff = new Task[infos.Count]; - for (int i = 0; i < infos.Count; i++) - { - //tasksTakeOff[i] = NewSingleRunTaskOffTaskAsunc(infos[i]); - - - tasksTakeOff[i] = await Task.Factory.StartNew(async () => - { - var internalInfo = infos[i]; - - await NewSingleRunTaskOffTaskAsunc(internalInfo); - }); - } - await Task.WhenAll(tasksTakeOff).ConfigureAwait(false); - //await Task.Delay(100); - } - - private async Task NewSingleRunTaskOffTaskAsunc(FlightTaskSingleCopterInfo info) - { - DateTime dtNow = DateTime.Now; - DateTime dtLastTime = DateTime.Now; - TimeSpan ts = dtNow - dtLastTime; - - int copterIndex = SingleCopterInfos.IndexOf(info); - var copter = info.Copter; - await copter.UnlockAsync(); - //等待起飞时间 - while ((int)ts.TotalMilliseconds < (int)info.TakeOffWaitTime * 1000) - { - if (_flightTaskManager.IsPaused == true) - { - await copter.UnlockAsync(); - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(100); - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - } - - //虚拟飞机5秒后不起飞会自动上锁 - await copter.UnlockAsync(); - await copter.TakeOffAsync(); - var copterNextTask = _flightTaskManager.Tasks[TaskIndex + 1].SingleCopterInfos[copterIndex]; - float takeOffAlt = copterNextTask.TargetAlt; - info.TargetLat = info.Copter.Latitude; - info.TargetLng = info.Copter.Longitude; - //info.Copter.takeOffTargetAltitude = takeOffAlt; - FlightTask task = _flightTaskManager.CurrentRunningTask; - float takeflytime = task.TakeOffTime - info.TakeOffWaitTime; - //开始往上飞 - await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt, takeflytime, 1); //秒 - //解锁起飞用暗紫色 - info.Copter.LEDColor = CopterManager.CopterTakeoffColor; - - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - - - while (ts.TotalMilliseconds < task.TakeOffTime * 1000) - { - if (_flightTaskManager.IsPaused == true) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(100).ConfigureAwait(false); - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - //灯光控制 - if (info.LEDInfos.Count > 0) - { - string LEDRGB = ""; - List LedControl = info.LEDInfos.ToList(); - double time = 0; - for (int i = 0; i < LedControl.Count; i++) - { - - var led = LedControl[i]; - time += led.Delay * 1000; - if (ts.TotalMilliseconds >= time) - LEDRGB = info.LEDInfos[i].LEDRGB; - else - break; - } - info.Copter.LEDColor = LEDRGB; - ///灯光控制结束 - } - } - //起飞完成用默认颜色 - info.Copter.LEDColor = CopterManager.CopterFlyingColor; - - } - - - //老方案 ----- 按起飞数量起飞 - // 几架飞机同时起飞，参数为takeOffCount-----------------使用中 - //起飞分三个阶段： - //1阶段：分批起飞到15米(目前15米高度是飞控起飞航点决定)，上一批起飞超过5米下一批开始起飞， - // 等待全部起飞完成后执行第二阶段， - //2阶段：等待高度超过9米(可能已到达15米)然后平飞到第一个航点位置,高度为15米的位置， - //3阶段：垂直上升到第一个航点指定高度 - // - //修改方案：针对高度 - //1.上升到起飞高度(目前没有地面站设置高度，高度在飞控中) - //2.直接飞往第一个航点的高度 - public async Task MutilRunTakeOffTaskAsync() - { - - int TaskCount = _flightTaskManager.Tasks.Count(); - if (TaskCount > 1) - { - var infos = SingleCopterInfos; - - //不再使用起飞数量 强制设置起飞总数等于所有飞机 - int takeOffCount = _copterManager.Copters.Count(); - int copterCount = infos.Count; - int integerPart = copterCount / takeOffCount; - int residualPart = copterCount % takeOffCount; - - // var tasks = new Task[infos.Count]; - var tasksTmp = new Task[infos.Count]; - 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) // 第一阶段：起飞到15m - { - tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info); - } - else - { - 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]; - tasksTmp[j] = TakeOffSecondTaskAsync(info); // 第二和第三阶段 - } - } - - // 余数架飞机同时起飞 - if (residualPart > 0) - { - var tasksTakeOff = new Task[residualPart]; - for (int j = integerPart * takeOffCount; j < takeOffCount * integerPart + residualPart; j++) - { - var info = infos[j]; - - int indexTmp = j - takeOffCount * integerPart; - if (info.takeOffStage == 0) // 第一阶段：起飞到15m - { - tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info); - } - else - { - 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); // 加入第二和第三阶段 - } - } - //执行并等待所有的第二第三阶段起飞任务完成 - await Task.WhenAll(tasksTmp).ConfigureAwait(false); - if (_flightTaskManager.IsPaused == false) - { - for (int i = 0; i < infos.Count; i++) - { - var info = infos[i]; - info.takeOffStage = 0; - } - } - - } - } - - private CopterManager _copterManager = ServiceLocator.Current.GetInstance(); - - //执行第一阶段解锁起飞任务---使用中 - private async Task TakeOffTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) - { - DateTime dtNow = DateTime.Now; - DateTime dtLastTime = DateTime.Now; - TimeSpan ts = dtNow - dtLastTime; - - int copterIndex = SingleCopterInfos.IndexOf(info); - var copter = info.Copter; - var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex]; - - if ((bool)_copterManager.CopterStatus[copterIndex]) - return; - - if (_flightTaskManager.IsPaused == true) - { - return; - } - - //设置灯光 - await copter.SetShowLEDAsync(true); - - //开始解锁 - await copter.UnlockAsync(); - for (int i = 0; !copter.IsUnlocked; i++) - { - //if (_flightTaskManager.IsPaused == true) - //{ - // return; - //} - //8秒内每1000毫秒尝试解锁一次 - //解锁间隔一定要超过1s否则导致飞控以后无法解锁 - - if (i > 320) - return; //无法解锁后面不用执行了 - if (i % (1000 / 25) == 1000 / 25 - 1) - { - await copter.UnlockAsync(); // 每 1000 毫秒重试一次。 - } - - await Task.Delay(25).ConfigureAwait(false); - } - //////解锁完成 - - // 为了返航，记录家的位置, 应该放在起飞命令 - info.TargetLat = info.Copter.Latitude; - info.TargetLng = info.Copter.Longitude; - - //等待起飞时间 - while (ts.TotalMilliseconds < info.TakeOffWaitTime * 1000 ) - { - await Task.Delay(100); - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - } - - //开始起飞 - for (int i = 0; i < 5; i++) // added by ZJF - { - await copter.TakeOffAsync(); - await Task.Delay(50).ConfigureAwait(false); - } - - // while (copter.Altitude < 4 || copter.State == Copters.CopterState.TakingOff) - //低于5米任务一直等待 - while (copter.Altitude < 5) // 修改起飞逻辑，当高度达到5米时，下一架开始起飞 - { - if (_flightTaskManager.IsPaused == true) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(100).ConfigureAwait(false); - } - -/* //先不要这个控制看能否正常工作 - if (copter.Altitude > 8) - { - await info.Copter.GuidAsync(); - return; - } -*/ - } - - //执行第二第三阶段起飞任务-------------使用中 - private async Task TakeOffSecondTaskAsync(FlightTaskSingleCopterInfo info) - { - - int copterIndex = SingleCopterInfos.IndexOf(info); - var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex]; - float takeOffAlt = copterNextTask.TargetAlt;// 15; 起飞高度到下个任务的高度 - // 当该飞机被标记时，悬停并跳过飞行任务 - if ((bool)_copterManager.CopterStatus[copterIndex]) - { - await info.Copter.HoverAsync(); - return; - } // await Task.Run(async () => - // { - // 小于9米时等待----按起飞高度算此时可能已到达15米起飞高度 - if (info.takeOffStage == 0) - { - while (info.Copter.Altitude < 9) - { - if (_flightTaskManager.IsPaused == true) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(100).ConfigureAwait(false); - // 当该飞机被标记时，悬停并跳过飞行任务 - if ((bool)_copterManager.CopterStatus[copterIndex]) - { - await info.Copter.HoverAsync(); - return; - } - } - if (_flightTaskManager.IsPaused == false) - { - info.takeOffStage++; - } - } - - DateTime dtNow = DateTime.Now; - DateTime dtLastTime = DateTime.Now; - TimeSpan ts = dtNow - dtLastTime; - - #region 屏蔽掉第二个阶段直接进入第三个阶 到达起飞高度直接飞往第一个航点 - - // 第二阶段：超过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(info.TargetLat, info.TargetLng, takeOffAlt); - await Task.Delay(10).ConfigureAwait(false); - } - //直到到达第一个航点并高度15米 - while (!info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, takeOffAlt)) - { - if (_flightTaskManager.IsPaused == true) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(25).ConfigureAwait(false); - - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - //每2秒再异步执行一次到航点高度15米的任务 - if (ts.TotalMilliseconds > 2000) - { - for (int j = 0; j < 2; j++) - { - await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt); - await Task.Delay(10).ConfigureAwait(false); - } - dtLastTime = dtNow; - } - // 当该飞机被标记时，悬停并跳过飞行任务 - if ((bool)_copterManager.CopterStatus[copterIndex]) - { - await info.Copter.HoverAsync(); - return; - } - } - if (_flightTaskManager.IsPaused == false) - { - info.takeOffStage++; //当前变为2 - } - } - - dtNow = DateTime.Now; - dtLastTime = DateTime.Now; - ts = dtNow - dtLastTime; - - #endregion - - info.takeOffStage++; - - // 第三阶段：从第一个航点位置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) - { - await info.Copter.HoverAsync(); - return; - } - await Task.Delay(25).ConfigureAwait(false); - - dtNow = DateTime.Now; - ts = dtNow - dtLastTime; - //每2秒发一次第一个航点飞行任务 - if (ts.TotalMilliseconds > 2000) - { - for (int j = 0; j < 2; j++) - { - await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt); - await Task.Delay(10).ConfigureAwait(false); - } - dtLastTime = dtNow; - } - // 当该飞机被标记时，悬停并跳过飞行任务 - if ((bool)_copterManager.CopterStatus[copterIndex]) - { - await info.Copter.HoverAsync(); - return; - } - } - } - - // }); - } - - } -} +using Plane.Copters; +using Microsoft.Practices.ServiceLocation; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Geography; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTask + { + + + //起飞到第一个航点高度的飞行时间 + private byte _TakeOffTime = 10; + public byte TakeOffTime + { + get { return _TakeOffTime; } + set { Set(nameof(TakeOffTime), ref _TakeOffTime, value); } + } + + //一架架起飞--未使用 + public async Task RunTakeOffTaskAsync() + { + // float takeOffAlt = 15; + int TaskCount = _flightTaskManager.Tasks.Count(); + if (TaskCount > 1) + { + var infos = SingleCopterInfos; + // var tasks = new Task[infos.Count]; + var tasksTmp = new Task[infos.Count]; + for (int i = 0; i < infos.Count; i++) + { + var info = infos[i]; + + if (info.takeOffStage == 0) // 第一阶段：起飞到10m + { + await TakeOffTaskFlySingleCopterAsync(info); + //if (_flightTaskManager.IsPaused == false) + //{ + // info.takeOffStage++; + //} + } + + tasksTmp[i] = TakeOffSecondTaskAsync(info); // 第二和第三阶段 + } + await Task.WhenAll(tasksTmp).ConfigureAwait(false); + if (_flightTaskManager.IsPaused == false) + { + for (int i = 0; i < infos.Count; i++) + { + var info = infos[i]; + info.takeOffStage = 0; + } + } + + } + } + + //新版的起飞方案 + public async Task NewMutilRunTakeOffTaskAsync() + { + var infos = SingleCopterInfos; + var tasksTakeOff = new Task[infos.Count]; + for (int i = 0; i < infos.Count; i++) + { + tasksTakeOff[i] = await Task.Factory.StartNew(async () => + { + var internalInfo = infos[i]; + await NewSingleRunTaskOffTaskAsunc(internalInfo); + }); + } + await Task.WhenAll(tasksTakeOff).ConfigureAwait(false); + } + + /// + /// 单独飞机 执行起飞任务 + /// + /// + /// + private async Task NewSingleRunTaskOffTaskAsunc(FlightTaskSingleCopterInfo info) + { + DateTime dtNow = DateTime.Now; + DateTime dtLastTime = DateTime.Now; + TimeSpan ts = dtNow - dtLastTime; + + int copterIndex = SingleCopterInfos.IndexOf(info); + var copter = info.Copter; + await copter.UnlockAsync(); + //等待起飞时间 + while ((int)ts.TotalMilliseconds < (int)info.TakeOffWaitTime * 1000) + { + if (_flightTaskManager.IsPaused == true) + { + await copter.UnlockAsync(); + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(100); + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + } + + //虚拟飞机5秒后不起飞会自动上锁 + await copter.UnlockAsync(); + await copter.TakeOffAsync(); + var copterNextTask = _flightTaskManager.Tasks[TaskIndex + 1].SingleCopterInfos[copterIndex]; + float takeOffAlt = copterNextTask.TargetAlt; + info.TargetLat = info.Copter.Latitude; + info.TargetLng = info.Copter.Longitude; + //info.Copter.takeOffTargetAltitude = takeOffAlt; + FlightTask task = _flightTaskManager.CurrentRunningTask; + float takeflytime = task.TakeOffTime - info.TakeOffWaitTime; + //飞行目标，开始往上飞 + await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt, takeflytime, 1); //秒 + //解锁起飞用暗紫色 + info.Copter.LEDColor = CopterManager.CopterTakeoffColor; + + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + + //等待起飞时间完成,并模拟灯光--不移动飞机-飞机自己计算并移动位置 + while (ts.TotalMilliseconds < task.TakeOffTime * 1000) + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(100).ConfigureAwait(false); + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + //灯光控制 + if (info.LEDInfos.Count > 0) + { + string LEDRGB = ""; + List LedControl = info.LEDInfos.ToList(); + double time = 0; + for (int i = 0; i < LedControl.Count; i++) + { + + var led = LedControl[i]; + time += led.Delay * 1000; + if (ts.TotalMilliseconds >= time) + LEDRGB = info.LEDInfos[i].LEDRGB; + else + break; + } + info.Copter.LEDColor = LEDRGB; + ///灯光控制结束 + } + } + //起飞完成用默认颜色 + info.Copter.LEDColor = CopterManager.CopterFlyingColor; + } + + + //老方案 ----- 按起飞数量起飞 + // 几架飞机同时起飞，参数为takeOffCount-----------------使用中 + //起飞分三个阶段： + //1阶段：分批起飞到15米(目前15米高度是飞控起飞航点决定)，上一批起飞超过5米下一批开始起飞， + // 等待全部起飞完成后执行第二阶段， + //2阶段：等待高度超过9米(可能已到达15米)然后平飞到第一个航点位置,高度为15米的位置， + //3阶段：垂直上升到第一个航点指定高度 + // + //修改方案：针对高度 + //1.上升到起飞高度(目前没有地面站设置高度，高度在飞控中) + //2.直接飞往第一个航点的高度 + public async Task MutilRunTakeOffTaskAsync() + { + + int TaskCount = _flightTaskManager.Tasks.Count(); + if (TaskCount > 1) + { + var infos = SingleCopterInfos; + + //不再使用起飞数量 强制设置起飞总数等于所有飞机 + int takeOffCount = _copterManager.Copters.Count(); + int copterCount = infos.Count; + int integerPart = copterCount / takeOffCount; + int residualPart = copterCount % takeOffCount; + + // var tasks = new Task[infos.Count]; + var tasksTmp = new Task[infos.Count]; + 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) // 第一阶段：起飞到15m + { + tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info); + } + else + { + 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]; + tasksTmp[j] = TakeOffSecondTaskAsync(info); // 第二和第三阶段 + } + } + + // 余数架飞机同时起飞 + if (residualPart > 0) + { + var tasksTakeOff = new Task[residualPart]; + for (int j = integerPart * takeOffCount; j < takeOffCount * integerPart + residualPart; j++) + { + var info = infos[j]; + + int indexTmp = j - takeOffCount * integerPart; + if (info.takeOffStage == 0) // 第一阶段：起飞到15m + { + tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info); + } + else + { + 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); // 加入第二和第三阶段 + } + } + //执行并等待所有的第二第三阶段起飞任务完成 + await Task.WhenAll(tasksTmp).ConfigureAwait(false); + if (_flightTaskManager.IsPaused == false) + { + for (int i = 0; i < infos.Count; i++) + { + var info = infos[i]; + info.takeOffStage = 0; + } + } + + } + } + + private CopterManager _copterManager = ServiceLocator.Current.GetInstance(); + + //执行第一阶段解锁起飞任务---使用中 + private async Task TakeOffTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) + { + DateTime dtNow = DateTime.Now; + DateTime dtLastTime = DateTime.Now; + TimeSpan ts = dtNow - dtLastTime; + + int copterIndex = SingleCopterInfos.IndexOf(info); + var copter = info.Copter; + var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex]; + + if ((bool)_copterManager.CopterStatus[copterIndex]) + return; + + if (_flightTaskManager.IsPaused == true) + { + return; + } + + //设置灯光 + await copter.SetShowLEDAsync(true); + + //开始解锁 + await copter.UnlockAsync(); + for (int i = 0; !copter.IsUnlocked; i++) + { + //if (_flightTaskManager.IsPaused == true) + //{ + // return; + //} + //8秒内每1000毫秒尝试解锁一次 + //解锁间隔一定要超过1s否则导致飞控以后无法解锁 + + if (i > 320) + return; //无法解锁后面不用执行了 + if (i % (1000 / 25) == 1000 / 25 - 1) + { + await copter.UnlockAsync(); // 每 1000 毫秒重试一次。 + } + + await Task.Delay(25).ConfigureAwait(false); + } + //////解锁完成 + + // 为了返航，记录家的位置, 应该放在起飞命令 + info.TargetLat = info.Copter.Latitude; + info.TargetLng = info.Copter.Longitude; + + //等待起飞时间 + while (ts.TotalMilliseconds < info.TakeOffWaitTime * 1000 ) + { + await Task.Delay(100); + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + } + + //开始起飞 + for (int i = 0; i < 5; i++) // added by ZJF + { + await copter.TakeOffAsync(); + await Task.Delay(50).ConfigureAwait(false); + } + + // while (copter.Altitude < 4 || copter.State == Copters.CopterState.TakingOff) + //低于5米任务一直等待 + while (copter.Altitude < 5) // 修改起飞逻辑，当高度达到5米时，下一架开始起飞 + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(100).ConfigureAwait(false); + } + +/* //先不要这个控制看能否正常工作 + if (copter.Altitude > 8) + { + await info.Copter.GuidAsync(); + return; + } +*/ + } + + //执行第二第三阶段起飞任务-------------使用中 + private async Task TakeOffSecondTaskAsync(FlightTaskSingleCopterInfo info) + { + + int copterIndex = SingleCopterInfos.IndexOf(info); + var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex]; + float takeOffAlt = copterNextTask.TargetAlt;// 15; 起飞高度到下个任务的高度 + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } // await Task.Run(async () => + // { + // 小于9米时等待----按起飞高度算此时可能已到达15米起飞高度 + if (info.takeOffStage == 0) + { + while (info.Copter.Altitude < 9) + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(100).ConfigureAwait(false); + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } + } + if (_flightTaskManager.IsPaused == false) + { + info.takeOffStage++; + } + } + + DateTime dtNow = DateTime.Now; + DateTime dtLastTime = DateTime.Now; + TimeSpan ts = dtNow - dtLastTime; + + #region 屏蔽掉第二个阶段直接进入第三个阶 到达起飞高度直接飞往第一个航点 + + // 第二阶段：超过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(info.TargetLat, info.TargetLng, takeOffAlt); + await Task.Delay(10).ConfigureAwait(false); + } + //直到到达第一个航点并高度15米 + while (!info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, takeOffAlt)) + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(25).ConfigureAwait(false); + + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + //每2秒再异步执行一次到航点高度15米的任务 + if (ts.TotalMilliseconds > 2000) + { + for (int j = 0; j < 2; j++) + { + await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt); + await Task.Delay(10).ConfigureAwait(false); + } + dtLastTime = dtNow; + } + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } + } + if (_flightTaskManager.IsPaused == false) + { + info.takeOffStage++; //当前变为2 + } + } + + dtNow = DateTime.Now; + dtLastTime = DateTime.Now; + ts = dtNow - dtLastTime; + + #endregion + + info.takeOffStage++; + + // 第三阶段：从第一个航点位置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) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(25).ConfigureAwait(false); + + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + //每2秒发一次第一个航点飞行任务 + if (ts.TotalMilliseconds > 2000) + { + for (int j = 0; j < 2; j++) + { + await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt); + await Task.Delay(10).ConfigureAwait(false); + } + dtLastTime = dtNow; + } + // 当该飞机被标记时，悬停并跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + { + await info.Copter.HoverAsync(); + return; + } + } + } + + // }); + } + + } +} diff --git a/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs b/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs index e58ce44..3dd44d9 100644 --- a/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs @@ -1,2127 +1,2163 @@ -using Plane.Copters; -using Plane.FormationCreator.Formation; -using Plane.Geography; -using Plane.Logging; -using Plane.Windows.Messages; -using GalaSoft.MvvmLight; -using GalaSoft.MvvmLight.CommandWpf; -using Microsoft.Practices.ServiceLocation; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Linq; -using System.Text; -using System.Threading.Tasks; -using System.Windows; -using System.Windows.Input; -using Plane.Communication; -using Microsoft.Win32; -using Plane.Util; -using System.Windows.Media; -using Plane.CommunicationManagement; -using Plane.FormationCreator.Views; -using Plane.Windows.IniHelper; - -namespace Plane.FormationCreator.ViewModels -{ - public class ControlPanelViewModel : ViewModelBase - { - // serialport - internal static IConnection Rtkport ; - - private static bool trkthreadrun = false; - - private static bool rtcmthreadrun = false; - - - - public ControlPanelViewModel(FormationController formationController, CopterManager copterManager) - { - _formationController = formationController; - _copterManager = copterManager; - LoadRTKcomvalue(); - } - - private FormationController _formationController; - private CopterManager _copterManager; - private CommModuleManager _commModuleManager = CommModuleManager.Instance; - - private ILogger _logger = ServiceLocator.Current.GetInstance(); - - private float _AltP = 0.5f; - public float AltP - { - get { return _AltP; } - set { Set(nameof(AltP), ref _AltP, value); } - } - - private float _TaskOffAlt = 2; - public float TaskOffAlt - { - get { return _TaskOffAlt; } - set { Set(nameof(TaskOffAlt), ref _TaskOffAlt, value); } - } - - private string _RTKState = "RTK未发送"; - public string RTKState - { - get { return _RTKState; } - set { Set(nameof(RTKState), ref _RTKState, value); } - } - - private string _RTKbtntxt = "发送RTK"; - public string RTKbtntxt - { - get { return _RTKbtntxt; } - set { Set(nameof(RTKbtntxt), ref _RTKbtntxt, value); } - } - - private string _NTRIPbtntxt = "发送RTCM"; - public string NTRIPbtntxt - { - get { return _NTRIPbtntxt; } - set { Set(nameof(NTRIPbtntxt), ref _NTRIPbtntxt, value); } - } - - private ICommand _LockAllCommand; - public ICommand LockAllCommand - { - get - { - return _LockAllCommand ?? (_LockAllCommand = new RelayCommand(async () => - { - // await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync())); - if (Alert.Show("您确定要上锁吗？所有飞行器将无视转速，立即强制停止运转！！！！", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning) - == MessageBoxResult.OK) - { - if (Alert.Show("再次确认上锁？请确认所有飞行器都在地面，否则将自由落体", "再次警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning) - == MessageBoxResult.OK) - { - await _commModuleManager.LockAsync(); - /* - await Task.WhenAll(_copterManager.Copters.Select(async c => - { - await c.LockAsync(); - })); - */ - } - } - })); - } - } - - - private static readonly object locker = new object(); - private ICommand _DetectionVoltage; - public ICommand DetectionVoltage - { - get - { - return _DetectionVoltage ?? (_DetectionVoltage = new RelayCommand(async () => - { - - float minVol; - InputDialog inputDialog = new InputDialog("最低检测电压( 0 恢复灯光):", "15.6"); - if (inputDialog.ShowDialog() == true) - minVol = float.Parse(inputDialog.Answer); - else return; - - //测试电池 - await _commModuleManager.TestBattery((short)0, minVol); - if (minVol == 0.0) return; - Message.Show("---请检查飞机灯光---"); - Message.Show(string.Format("---绿 色 电压高于 {0:F2}v (单节高于 {1:F2}v)--- ", minVol, minVol / 4.0f)); - Message.Show(string.Format("---黄 色 电压低于 {0:F2}v (单节低于 {1:F2}v)--- ", minVol, minVol / 4.0f)); - Message.Show(string.Format("---棕 色 电压低于 {0:F2}v (单节低于 {1:F2}v)--- ", minVol - 0.2, (minVol - 0.2) / 4.0f)); - Message.Show(string.Format("---洋红色 电压低于 {0:F2}v (单节低于 {1:F2}v)--- ", minVol - 0.4, (minVol - 0.4) / 4.0f)); - Message.Show(string.Format("---亮红色 电压低于 {0:F2}v (单节低于 {1:F2}v)--- ", minVol - 0.6, (minVol - 0.6) / 4.0f)); - Message.Show("---输入0恢复灯光---"); - - Message.Show("--------------开始检测单机电压--------------"); - Dictionary dic_voltage = new Dictionary(); - await Task.WhenAll(_copterManager.Copters.Select(async c => - { - float voltageSum = 0.0f; - string name = c.Name; - - for (int i = 0; i < 5; i++) - { - - voltageSum += c.Voltage; - await Task.Delay(1000).ConfigureAwait(false); - } - float voltageAverage = voltageSum / 5; - if (name != null && name != "") - { - lock (locker) - { - dic_voltage.Add(name, voltageAverage); - } - } - })).ConfigureAwait(false); - - - Dictionary dic_voltage_Order = dic_voltage.OrderByDescending(o => o.Value).ToDictionary(o => o.Key, o => o.Value); - bool showch = false; - ICopter vcopter = null; - _copterManager.shiftkeydown = true; - //清除选择 - _copterManager.Select(null); - foreach (KeyValuePair kv in dic_voltage_Order) - { - if ((!showch) && (minVol > 0.00) && (kv.Value <= minVol)) - { - Message.Show(string.Format("以下飞机电压低于[{0}V，单节{1:F2}]", minVol, minVol / 4)); - showch = true; - } - Message.Show(string.Format("{0} --> 5秒平均电压：{1:F2},单节{2:F2}", kv.Key, kv.Value, kv.Value / 4)); - - - //选中低电压飞机 - if ((minVol > 0.00) && (kv.Value <= minVol)) - { - vcopter = _copterManager.Copters.FirstOrDefault(o => o.Name == kv.Key); - if (vcopter != null) - _copterManager.Select(vcopter); //需要访问UI线程 - } - await Task.Delay(5).ConfigureAwait(false); - } - await Task.Delay(500).ConfigureAwait(false); - _copterManager.shiftkeydown = false; - Message.Show(string.Format("----检测电压完成,检测总数：{0}----", dic_voltage.Count)); - //todo:增加电压阈值变成不同颜色灯 - })); - } - } - - private ICommand _DetectionMissionData; - public ICommand DetectionMissionData - { - get - { - return _DetectionMissionData ?? (_DetectionMissionData = new RelayCommand(async () => - { - var missionState = _commModuleManager.MissionWriteState.Where(i => i.Value.IsFailed); - if (missionState.Count() == 0) - Message.Show("航点全部写入成功"); - else - { - Message.Show("----统计失败航点写入失败----"); - foreach (KeyValuePair item in missionState) - { - CommWriteMissinState state = item.Value; - string msg = $"{item.Key}:飞机状态:{state.StateReturn}, 错误码:{state.ErrorCode}, 传输:{state.SendAchieved}, 写入:{state.WriteSucceed} "; - Message.Show(msg); - await Task.Delay(10); - } - } - })); - } - } - - private ICommand _DetectionReturnData; - public ICommand DetectionReturnData - { - get - { - return _DetectionReturnData ?? (_DetectionReturnData = new RelayCommand(async () => - { - Dictionary> dic_returnData = new Dictionary>(); - await Task.WhenAll(_copterManager.Copters.Select(async c => - { - - string name = c.Name; - int return_data = BitConverter.ToInt32(c.Retain, 0); - - lock (locker) - { - List copterList; - if (!dic_returnData.ContainsKey(return_data)) - { - copterList = new List(); - dic_returnData.Add(return_data, copterList); - } - else - copterList = dic_returnData[return_data]; - - copterList.Add(name); - } - await Task.Delay(10).ConfigureAwait(false); - })).ConfigureAwait(false); - - await Task.Run(async () => - { - foreach (KeyValuePair> kv in dic_returnData) - { - byte[] test = BitConverter.GetBytes(kv.Key); - Array.Reverse(test); - string data = string.Join(".", test); - string copters = string.Join(",", kv.Value.ToArray()); - Message.Show(string.Format("返回数据{0}({1})：{2}", data, kv.Key,copters)); - await Task.Delay(5).ConfigureAwait(false); - } - Message.Show("----统计完成----"); - }).ConfigureAwait(false); - - })); - } - } - - private ICommand _ReportGPSTypeCommand; - public ICommand ReportGPSTypeCommand - { - get - { - return _ReportGPSTypeCommand ?? (_ReportGPSTypeCommand = new RelayCommand(async()=> - { - if (_copterManager.Copters.Count > 0) - { - Dictionary> GPSTypes = new Dictionary>(); - - Dictionary> Heading = new Dictionary>(); - - foreach (var copter in _copterManager.Copters) - { - string name = copter.Name; - if (!GPSTypes.Keys.Contains(copter.GpsFixType.ToString())) - { - List copterNames = new List(); - copterNames.Add(name); - GPSTypes.Add(copter.GpsFixType.ToString(), copterNames); - } - else - GPSTypes[copter.GpsFixType.ToString()].Add(name); - - - - - if (!Heading.Keys.Contains(copter.Heading.ToString())) - { - List copterNames = new List(); - copterNames.Add(name); - Heading.Add(copter.Heading.ToString(), copterNames); - } - else - Heading[copter.Heading.ToString()].Add(name); - - - } - Message.Show($"==定位状态=="); - - foreach (var item in GPSTypes) - { - Message.Show($"{item.Key}:{string.Join(",", item.Value)}"); - Message.Show($"------------"); - } - Message.Show($"==机头方向=="); - //从小到大排个序 - Dictionary> SortedHeading = Heading.OrderBy(p => p.Key).ToDictionary(p => p.Key, o => o.Value); - - foreach (var item in SortedHeading) - { - Message.Show($"{item.Key} 度:{string.Join(",", item.Value)}"); - Message.Show($"------------"); - - } - - } - await Task.Delay(10); - })); - } - } - - - - private ICommand _DetectionCommModuleVersion; - public ICommand DetectionCommModuleVersion - { - get - { - return _DetectionCommModuleVersion ?? (_DetectionCommModuleVersion = new RelayCommand(async () => - { - Dictionary> dic_version = new Dictionary>(); - await Task.WhenAll(_copterManager.Copters.Select(async c => - { - string name = c.Name; - int version = c.CommModuleVersion; - - lock (locker) - { - List copterList; - if (!dic_version.ContainsKey(version)) - { - copterList = new List(); - dic_version.Add(version, copterList); - } - else - copterList = dic_version[version]; - - copterList.Add(name); - } - await Task.Delay(10).ConfigureAwait(false); - })).ConfigureAwait(false); - - await Task.Run(async () => - { - foreach (KeyValuePair> kv in dic_version) - { - string copters = string.Join(",", kv.Value.ToArray()); - Message.Show(string.Format("通信模块版本{0}：{1}", kv.Key, copters)); - await Task.Delay(5).ConfigureAwait(false); - } - Message.Show("----统计完成----"); - }).ConfigureAwait(false); - - })); - } - } - - private ICommand _AllLandCommand; - public ICommand AllLandCommand - { - get - { - return _AllLandCommand ?? (_AllLandCommand = new RelayCommand(async () => - { - _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); - await _commModuleManager.LandAsync(); - /* - await Task.WhenAll(_copterManager.Copters.Select(async c => - { - await c.LandAsync(); - })); - */ - })); - } - } - - private ICommand _LEDFlickerCommand; - public ICommand LEDFlickerCommand - { - get - { - return _LEDFlickerCommand ?? (_LEDFlickerCommand = new RelayCommand(() => - { - _commModuleManager.LED_FlickerAsync(_copterManager.AcceptingControlCopters); - })); - } - } - - - private ICommand _DelCommand; - public ICommand DelCommand - { - get - { - return _DelCommand ?? (_DelCommand = new RelayCommand(async () => - { - FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - { - if (_flightTaskManager.Tasks.Count > 0) - { - Alert.Show("有航点任务无法删除飞机,请先清除航点！", "警告", MessageBoxButton.OK, MessageBoxImage.Warning); - return; - } - - await _copterManager.DelSelCopters(); - } - })); - } - } - - - private ICommand _MotorTestCommand; - public ICommand MotorTestCommand - { - get - { - return _MotorTestCommand ?? (_MotorTestCommand = new RelayCommand(async () => - { - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - { - await _commModuleManager.MotorTestAsync(_copterManager.AcceptingControlCopters); - } - - /* - - await Task.WhenAll(_copterManager.Copters.Select(async c => - { - for (int i = 1; i <= 4; i++) - { - await c.MotorTestAsync(i).ConfigureAwait(false); - await Task.Delay(10); - } - - })).ConfigureAwait(false); - */ - })); - } - } - - private ICommand _GuidAsyncCommand; - public ICommand GuidAsyncCommand - { - get - { - return _GuidAsyncCommand ?? (_GuidAsyncCommand = new RelayCommand(async () => - { - if (TaskOffAlt <= 0) return; - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - await _commModuleManager.TakeOffAsync(TaskOffAlt, _copterManager.AcceptingControlCopters); - /* - await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async c => - { - await c.GuidAsync(); - await Task.Delay(100).ConfigureAwait(false); - await c.TakeOffAsync(1); - })); - */ - })); - } - } - - /// - /// 是否允许开始任务，全部解锁会在3秒内循环发送3次，该过程中不允许开始任务 - /// - private bool _AllowMissionStart = true; - public bool AllowMissionStart - { - get { return _AllowMissionStart; } - set { Set(nameof(AllowMissionStart), ref _AllowMissionStart, value); } - } - - - private ICommand _UnlockAllCommand; - public ICommand UnlockAllCommand - { - get - { - return _UnlockAllCommand ?? (_UnlockAllCommand = new RelayCommand(async () => - { - AllowMissionStart = false; - // await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync())); - for (int i = 0; i < 3; i++) - { - _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); - await _commModuleManager.UnlockAsync(); - await Task.Delay(1000).ConfigureAwait(false); - } - await Task.Delay(100); - AllowMissionStart = true; - - /* - await Task.WhenAll(_copterManager.Copters.Select(async c => { - await c.UnlockAsync(); - //解锁间隔一定要超过1s否则导致飞控以后无法解锁 - for (int i = 0; !c.IsUnlocked; i++) - { - if (i > 200) - break; - if (i % (1000 / 25) == 1000 / 25 - 1) - { - await c.UnlockAsync(); // 每 1000 毫秒重试一次。解锁间隔一定要超过1s否则导致飞控以后无法解锁 - } - await Task.Delay(25).ConfigureAwait(false); - } - })); - */ - })); - } - } - - private const string NTF_GROUPLED_OFF = "NTF_G_OFF"; - private ICommand _LEDOnOffCommand; - public ICommand LEDOnOffCommand - { - get - { - return _LEDOnOffCommand ?? (_LEDOnOffCommand = new RelayCommand(async onOff=> - { - string paramstr = NTF_GROUPLED_OFF; - float paramvalue = onOff; - await _commModuleManager.SetParamAsync(paramstr, paramvalue, _copterManager.AcceptingControlCopters); - /* - await Task.WhenAll( - _copterManager.AcceptingControlCopters.Select(async copter => - { - paramvalue = await copter.GetParamAsync(paramstr); - })); - - float newParamvalue = paramvalue == 0 ? 1 : 0; - - await Task.WhenAll(_copterManager.AcceptingControlCopters.Select - (copter => copter.SetParamAsync(paramstr, newParamvalue))); - */ - })); - } - } - - private ICommand _UnlockCommand; - public ICommand UnlockCommand - { - get - { - return _UnlockCommand ?? (_UnlockCommand = new RelayCommand(async () => - { - // await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync())); - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - await _commModuleManager.UnlockAsync(_copterManager.AcceptingControlCopters); - - /* - await Task.WhenAll(_copterManager.AcceptingControlCopters.Select( async c => { - await c.UnlockAsync(); - //解锁间隔一定要超过1s否则导致飞控以后无法解锁 - for (int i = 0; !c.IsUnlocked; i++) - { - if (i > 200) - break; - if (i % (1000 / 25) == 1000 / 25 - 1) - { - await c.UnlockAsync(); // 每 1000 毫秒重试一次。解锁间隔一定要超过1s否则导致飞控以后无法解锁 - } - await Task.Delay(25).ConfigureAwait(false); - } - })); - */ - })); - } - } - - private float _LatOffset = 18.4023833f; - public float LatOffset - { - get { return _LatOffset; } - set { Set(nameof(LatOffset), ref _LatOffset, value); } - } - - private float _LngOffset = 109.799761f; - public float LngOffset - { - get { return _LngOffset; } - set { Set(nameof(LngOffset), ref _LngOffset, value); } - } - - - private ICommand _RLTOffsetCommand; - public ICommand RLTOffsetCommand - { - get - { - return _RLTOffsetCommand ?? (_RLTOffsetCommand = new RelayCommand(() => - { -// Message.Show($"偏移Lat = {LatOffset}，Lng = {LngOffset}"); -// if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) -// await _commModuleManager.RLTOffsetAsync(LatOffset, LngOffset, _copterManager.AcceptingControlCopters); - - - })); - } - } - - private ICommand _LockCommand; - public ICommand LockCommand - { - get - { - return _LockCommand ?? (_LockCommand = new RelayCommand(async () => - { - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - { - if (Alert.Show("您确定要上锁吗？飞行器将无视转速，立即强制停止运转！！！", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning) - == MessageBoxResult.OK) - { - _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); - await _commModuleManager.LockAsync(_copterManager.AcceptingControlCopters); - //await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.LockAsync())); - } - } - })); - } - } - - private ICommand _TakeOffCommand; - public ICommand TakeOffCommand - { - get - { - return _TakeOffCommand ?? (_TakeOffCommand = new RelayCommand(async () => - { - FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); - if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) - { - Alert.Show("作为参照的原点未设置，无法开始任务！", "提示"); - return; - } - //飞机用UTC时间，日志用本地时间记录 - DateTime MissionTime = DateTime.UtcNow.AddSeconds(5); - DateTime MissionTime_log = DateTime.Now.AddSeconds(5); - Message.Show("任务开始:" + MissionTime_log.ToString()); - - IEnumerable selcopters = _copterManager.AcceptingControlCopters; - - //循环3次 发送起飞命令 避免通信问题 - for (int i = 0; i < 20; i++) - { - - /*await _commModuleManager.DoMissionStartAsync(_copterManager.Copters, - MissionTime.Hour, - MissionTime.Minute, - MissionTime.Second, - _flightTaskManager.OriginLng, - _flightTaskManager.OriginLat - ); - */ - - - await _commModuleManager.DoMissionStartAsync(selcopters, - MissionTime.Hour, - MissionTime.Minute, - MissionTime.Second, - _flightTaskManager.OriginLng, - _flightTaskManager.OriginLat - ); - - - - /* await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.MissionStartAsync( - MissionTime.Hour, - MissionTime.Minute, - MissionTime.Second, - _flightTaskManager.OriginLng, - _flightTaskManager.OriginLat - ))); - */ - - - await Task.Delay(20).ConfigureAwait(false); - - } - //_copterManager.Net_LogStartMission(MissionTime_log, _flightTaskManager.OriginLng, _flightTaskManager.OriginLat); - Alert.Show("所有选中飞机开始执行航点任务。请勿多次开始任务！", "提示", MessageBoxButton.OK, MessageBoxImage.Information); - })); - } - } - - private ICommand _ThrowoutCommand; - public ICommand ThrowoutCommand - { - get - { - return _ThrowoutCommand ?? (_ThrowoutCommand = new RelayCommand( () => - { - // await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync())); - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - _commModuleManager.ThrowoutAsync(_copterManager.AcceptingControlCopters); - })); - } - } - - private ICommand _HoverCommand; - public ICommand HoverCommand - { - get - { - return _HoverCommand ?? (_HoverCommand = new RelayCommand(async () => - { - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - await _commModuleManager.HoverAsync(_copterManager.AcceptingControlCopters); - //await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.HoverAsync())); - })); - } - } - - private ICommand _FloatCommand; - public ICommand FloatCommand - { - get - { - return _FloatCommand ?? (_FloatCommand = new RelayCommand(async () => - { - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - await _commModuleManager.FloatAsync(_copterManager.AcceptingControlCopters); - //await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.FloatAsync())); - })); - } - } - - private ICommand _ReturnToLaunchCommand; - public ICommand ReturnToLaunchCommand - { - get - { - return _ReturnToLaunchCommand ?? (_ReturnToLaunchCommand = new RelayCommand(async () => - { - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - await _commModuleManager.ReturnToLaunchAsync(_copterManager.AcceptingControlCopters); - //await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.ReturnToLaunchAsync())); - })); - } - } - - - private ICommand _GetVersionsCommand; - public ICommand GetVersionsCommand - { - get - { - return _GetVersionsCommand ?? (_GetVersionsCommand = new RelayCommand(async () => - { - //if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - await _commModuleManager.GetVersionsAsync(); - })); - } - } - private ICommand _GetCommsumCommand; - public ICommand GetCommsumCommand - { - get - { - return _GetCommsumCommand ?? (_GetCommsumCommand = new RelayCommand(async () => - { - //if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - await _commModuleManager.GetCommsumAsync(); - })); - } - } - //打开校准界面 - private ICommand _CalibrationSingleCommand; - public ICommand CalibrationSingleCommand - { - get - { - return _CalibrationSingleCommand ?? (_CalibrationSingleCommand = new RelayCommand(async () => - { - CalibrationWindow calibrationWindow = new CalibrationWindow(); - calibrationWindow.ShowDialog(); - await Task.Delay(100); - })); - } - } - - private ICommand _LoadParamfile; - public ICommand LoadParamfile - { - get - { - return _LoadParamfile ?? (_LoadParamfile = new RelayCommand(async () => - { - - var dialog = new OpenFileDialog - { - DefaultExt = "param", - Filter = "飞行参数 (*.param)|*.param" - }; - if (dialog.ShowDialog() == true) - { - - bool ifallok = true; - var paramlist = ParamFile.loadParamFile(dialog.FileName); - - foreach (string name in paramlist.Keys) - { - float value = float.Parse( paramlist[name].ToString()); - //写5次 - for (int i = 0; i < 5; i++) - { - - if (_copterManager.AcceptingControlCopters.Count() < _copterManager.Copters.Count) - await _commModuleManager.SetParamAsync(name, value, _copterManager.AcceptingControlCopters); - else if (_copterManager.AcceptingControlCopters.Count() == _copterManager.Copters.Count) - { - _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); - await _commModuleManager.SetParamAsync(name, value); - } - - - - // await Task.WhenAll(_copterManager.AcceptingControlCopters.Select - // (copter => copter.SetParamAsync(name, value))); - } - await Task.Delay(2000); - - //发送读取5次 - for (int i = 0; i < 5; i++) - { - if (_copterManager.AcceptingControlCopters.Count() < _copterManager.Copters.Count) - await _commModuleManager.ReadParamAsnyc(name, _copterManager.AcceptingControlCopters); - else if (_copterManager.AcceptingControlCopters.Count() == _copterManager.Copters.Count) - await _commModuleManager.ReadParamAsnyc(name); - } - await Task.Delay(1000); - - //开始检查飞机返回参数是否正确 - - foreach (ICopter copter in _copterManager.AcceptingControlCopters) - { - if (value != copter.RetainInt) - { - ifallok = false; - Alert.Show(copter.Id + "参数[" + name + "]设置失败,读取的值是[" + copter.RetainInt + "]", "提示", MessageBoxButton.OK, MessageBoxImage.Information); - - } - - - } - - - - - - /* - await Task.WhenAll( - - _copterManager.AcceptingControlCopters.Select(async copter => - { - float getvaule = await copter.GetParamAsync(name); - if (getvaule != value) - { - ifallok = false; - Alert.Show(copter.Id +"参数["+ name+ "]设置失败,读取的值是[" + getvaule+"]", "提示", MessageBoxButton.OK, MessageBoxImage.Information); - } - } - - )); - */ - - - } - - if (ifallok) - Alert.Show("所有已选的飞机参数设置完成", "提示", MessageBoxButton.OK, MessageBoxImage.Information); - else - Alert.Show("有部分飞机参数设置失败，请检查并再次设置", "提示", MessageBoxButton.OK, MessageBoxImage.Information); - } - })); - } - } - - - private ICommand _ParamModify; - public ICommand ParamModify - { - get - { - return _ParamModify ?? (_ParamModify = new RelayCommand(async () => - { - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - { - var ModifyParamWindow = new Plane.FormationCreator.ModifyParam(); - if (ModifyParamWindow.ShowDialog() == true) - { - bool isLoadParam = ModifyParamWindow.LoadParam; - string paramstr = ModifyParamWindow.textParamName.Text; - - if(!isLoadParam) - { - float paramvalue = Convert.ToSingle(ModifyParamWindow.textParamValue.Text); - int num = 0; - int writeTime = 0; - //连续写5次，为了都写成功 - for ( writeTime=0; writeTime<5; writeTime++) - { - - if (_copterManager.AcceptingControlCopters.Count() < _copterManager.Copters.Count) - num = await _commModuleManager.SetParamAsync(paramstr, paramvalue, _copterManager.AcceptingControlCopters); - else if (_copterManager.AcceptingControlCopters.Count() == _copterManager.Copters.Count) - { - _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); - num = await _commModuleManager.SetParamAsync(paramstr, paramvalue); - } - } - Alert.Show($"广播完成！ 序列号从：{num-writeTime+1}到{num}"); - } - else - { - if (_copterManager.AcceptingControlCopters.Count() < _copterManager.Copters.Count) - await _commModuleManager.ReadParamAsnyc(paramstr, _copterManager.AcceptingControlCopters); - else if (_copterManager.AcceptingControlCopters.Count() == _copterManager.Copters.Count) - await _commModuleManager.ReadParamAsnyc(paramstr); - } - - /* - 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 _TurnOffTestLightsCommand; - public ICommand TurnOffTestLightsCommand - { - get - { - return _TurnOffTestLightsCommand ?? (_TurnOffTestLightsCommand = new RelayCommand(async () => - { - int num = 0; - //正式飞行 - num = await _commModuleManager.SetMultipleParamAsync( - "NTF_G_RTLOFF", "1", - "FS_GCS_ENABLE", "0", - "NTF_G_RTKTEST", "0", - "WAYPOINT_GLED", "0", - "FS_BATT_VOLTAGE", "14.2", - "RTL_ALT","3000", - "RTL_ALT_MAX","5000", - "FC_LOG_LEVEL", "0", - "FC_LOG_TYPE", "0", - "FS_GPS_RTL", "4"); - - //测试飞行 - /* - num = await _commModuleManager.SetMultipleParamAsync( - "NTF_G_RTLOFF", "0", - "FS_GCS_ENABLE", "1", - "NTF_G_RTKTEST", "1", - "WAYPOINT_GLED", "1", - "FS_BATT_VOLTAGE", "14.2", - "RTL_ALT", "3000", - "RTL_ALT_MAX", "5000", - "FC_LOG_LEVEL","3", - "FC_LOG_TYPE", "1", - "FS_GPS_RTL", "4" - ); - */ - - Alert.Show($"广播完成！ 当前序列号：{num}"); - })); - } - } - - - private string _RTKcomvalue = "COM6"; - public string RTKcomvalue - { - get { return _RTKcomvalue; } - set { Set(nameof(RTKcomvalue), ref _RTKcomvalue, value); } - } - - //RTK收发线程 - private static void mainloop() - { - - - - } - - - private async Task ReadRTKPacketAsync() - { - if (!Rtkport.IsOpen) - { - return null; - } - - // limit to 110 byte packets - byte[] buffer; - int length = 0; - - try - { - length = Math.Min(180, Rtkport.BytesToRead()); - - if (length > 0)//while (Rtkport.BytesToRead() > 0) - - { - buffer = new byte[length]; - - int read = await Rtkport.ReadAsync(buffer, 0, Math.Min(buffer.Length, length)); - - // sendData(buffer, (byte)read); - - } - else - return null; - - /* - if (await Rtkport.ReadAsync(buffer, 0, 1) == 0) - { - // 做简单数据解析，得到协议类型，后续数据长度等 - } - //根据数据长度读取实际数据 - if (await Rtkport.ReadAsync(buffer, 1, 1) == 0) - { - - } - */ - - } - catch (Exception ex) - { - return null; - } - return buffer; - - } - - - - private ICommand _SendRTKCommand; - public ICommand SendRTKCommand - { - get - { - return _SendRTKCommand ?? (_SendRTKCommand = new RelayCommand(async () => - { - if (!trkthreadrun) - { - Rtkport = new SerialPortConnection(RTKcomvalue, 57600) as IConnection; - await Rtkport.OpenAsync(); - if (!Rtkport.IsOpen) - { - Alert.Show("无法打开" + RTKcomvalue, "警告", MessageBoxButton.OK, MessageBoxImage.Warning); - return; - } - trkthreadrun = true; //开始运行后台任务 - /* //线程方式后台运行rtk转发任务 - thrtk = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop)) - { - IsBackground = true, - Name = "injectgps" - }; - thrtk.Start(); - */ - //后台任务方式运行rtk转发任务 - Alert.Show("RTK数据开始发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information); - RTKState = "RTK数据发送中"; - RTKbtntxt = "停止RTK"; - await SaveRTKcomvalue(); - await _commModuleManager.StartRtcmLoop(); - await Task.Run(async () => - { - Message.Show("RTK数据开始发送"); - //读取RTK数据循环 - rtcm3 rtcm3 = new rtcm3(); - while (trkthreadrun) - { - //读入RTK数据 - var packet = await ReadRTKPacketAsync().ConfigureAwait(false); - - if (packet != null && packet.Length > 0) - { - //Message.Show($"{DateTime.Now.ToString("HH:mm:ss fff")}---packet.length = {packet.Length}"); - //新方案的RTK发送，用于双频 - - int read = packet.Length; - if (read > 0) - { - for (int a = 0; a < read; a++) - { - int seenmsg = -1; - // rtcm - if ((seenmsg = rtcm3.Read(packet[a])) > 0) - { - Message.Show($"{DateTime.Now.ToString("HH:mm:ss fff")}---rtcm3.length = {rtcm3.length}"); - await _commModuleManager.InjectGpsRTCMDataAsync(rtcm3.packet, rtcm3.length); - } - } - } - - - - //稳定方案的rtk发送，用于单频 - //_commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); - //await _commModuleManager.InjectGpsDataAsync(packet, (ushort)packet.Length); - } - await Task.Delay(10).ConfigureAwait(false); - - } - }).ConfigureAwait(false); - Alert.Show("RTK数据停止发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information); - } - else//停止RTK - { - await _commModuleManager.CloseRtcmLoop(); - - trkthreadrun = false; - Rtkport.Close(); - Rtkport = null; - - RTKState = "未发送RTK数据"; - RTKbtntxt = "发送RTK"; - } - })); - } - } - - - private ICommand _SendRTCMCommand; - public ICommand SendRTCMCommand - { - get - { - return _SendRTCMCommand ?? (_SendRTCMCommand = new RelayCommand(async () => - { - FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); - if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) - { - Alert.Show("作为参照的原点未设置，无法发送RTCM！", "提示"); - return; - } - - rtcmthreadrun = !rtcmthreadrun; - if (rtcmthreadrun) - { - string messagetips = "格式：http://差分账号:差分密码@域名:端口/坐标系\r\n例如：http://account:password@rtk.ntrip.qxwz.com:8002/RTCM32_GGB"; - string url = ""; - IniFiles inifilse = new IniFiles(); - url = inifilse.IniReadvalue("Default","RTKurl"); - if (PlaneMessageBox.OnShow(messagetips, "发送RTK", ref url) == false) return; - inifilse.IniWritevalue("Default", "RTKurl", url); - - RTKState = "RTK数据发送中"; - NTRIPbtntxt = "停止RTCM"; - Alert.Show("RTCM数据开始发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information); - - await _commModuleManager.StartRtcmLoop(); - await Task.Run(async () => - { - Message.Show("RTK数据开始发送"); - //string url = "http://qxarpz003:1dc880b@rtk.ntrip.qxwz.com:8002/RTCM32_GGB"; - CommsNTRIP commNTRIP = new CommsNTRIP(url, _flightTaskManager.OriginLat, _flightTaskManager.OriginLng); - rtcm3 rtcm3 = new rtcm3(); - commNTRIP.Open(); - //读取RTK数据循环 - while (rtcmthreadrun) - { - byte[] buffer = new byte[180]; - - while (commNTRIP.BytesToRead > 0) - { - int read = commNTRIP.Read(buffer, 0, Math.Min(buffer.Length, commNTRIP.BytesToRead)); - - if (read > 0) - { - for (int a = 0; a < read; a++) - { - int seenmsg = -1; - // rtcm - if ((seenmsg = rtcm3.Read(buffer[a])) > 0) - { - //rtcmDatas.Add(rtcm3.packet); - //await SendRtcmData(); - //Message.Show($"{DateTime.Now.ToString("HH:mm:ss fff")}---rtcm3.length = {rtcm3.length}"); - await _commModuleManager.InjectGpsRTCMDataAsync(rtcm3.packet, rtcm3.length); - } - } - } - } - await Task.Delay(10); - } - NTRIPbtntxt = "发送RTCM"; - RTKState = "未发送RTK数据"; - Alert.Show("RTCM数据停止发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information); - }); - } - })); - } - } - - private RtcmInfoView rtcmInfoView; - private ICommand _OpenRtcmManageCommand; - public ICommand OpenRtcmManageCommand - { - get - { - return _OpenRtcmManageCommand ?? (_OpenRtcmManageCommand = new RelayCommand(() => - { - RtcmInfoView.GetInstance().Show(); - RtcmInfoView.GetInstance().Activate(); - // if (rtcmInfoView == null) - // { - // rtcmInfoView = new RtcmInfoView(); - // } - // rtcmInfoView.Show(); - })); - } - } - - /* - rtcm3 rtcm3 = new rtcm3(); - private async Task AnalysisRendRrcmData(byte[] buffer, int length) - { - for (int a = 0; a < length; a++) - { - int seenmsg = -1; - // rtcm - if ((seenmsg = rtcm3.Read(buffer[a])) > 0) - { - await _commModuleManager.InjectGpsRTCMDataAsync(rtcm3.packet, rtcm3.length); - } - } - await Task.Delay(1); - } - */ - - - private ICommand _LandCommand; - public ICommand LandCommand - { - get - { - return _LandCommand ?? (_LandCommand = new RelayCommand(async () => - { - if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) - await _commModuleManager.LandAsync(_copterManager.AcceptingControlCopters); - /* - await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.LandAsync())); - */ - })); - } - } - - - - - private ICommand _ConnectCommand; - public ICommand ConnectCommand - { - get - { - return _ConnectCommand ?? (_ConnectCommand = new RelayCommand(async () => - { - //await ConnectAsync(); - await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async copter => - { - //await commModule.ConnectAsync(); - await copter.ConnectAsync(); - //await copter.GetCopterDataAsync(); - })); - })); - } - } - - private ICommand _DisconnectCommand; - public ICommand DisconnectCommand - { - get - { - return _DisconnectCommand ?? (_DisconnectCommand = new RelayCommand(async () => - { - await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.DisconnectAsync())); - })); - } - } - - private ICommand _FlyToAltitudeCommand; - public ICommand FlyToAltitudeCommand - { - get - { - return _FlyToAltitudeCommand ?? (_FlyToAltitudeCommand = new RelayCommand(async alt => - { - await _formationController.FlyToAltitudeAsync(_copterManager.AcceptingControlCopters, targetAlt: alt).ConfigureAwait(false); - })); - } - } - - private ICommand _FlyInCircleCommand; - public ICommand FlyInCircleCommand - { - get - { - return _FlyInCircleCommand ?? (_FlyInCircleCommand = new RelayCommand(async () => - { - await _formationController.FlyInCircleAsync(_copterManager.AcceptingControlCopters, centerDirection: 180, radius: 10, loop: true).ConfigureAwait(false); - })); - } - } - - private ICommand _FlyAroundCenterOfCoptersCommand; - public ICommand FlyAroundCenterOfCoptersCommand - { - get - { - return _FlyAroundCenterOfCoptersCommand ?? (_FlyAroundCenterOfCoptersCommand = new RelayCommand(async () => - { - await _formationController.FlyAroundCenterOfCoptersAsync(_copterManager.AcceptingControlCopters).ConfigureAwait(false); - })); - } - } - - private ICommand _FlyInRectangleCommand; - public ICommand FlyInRectangleCommand - { - get - { - return _FlyInRectangleCommand ?? (_FlyInRectangleCommand = new RelayCommand(async () => - { - await _formationController.FlyInRectangleAsync(_copterManager.AcceptingControlCopters, startDirection: 180, sideLength1: 15, sideLength2: 5, loop: true).ConfigureAwait(false); - })); - } - } - - private ICommand _FlyToVerticalLineAndMakeCircleCommand; - public ICommand FlyToVerticalLineAndMakeCircleCommand - { - get - { - return _FlyToVerticalLineAndMakeCircleCommand ?? (_FlyToVerticalLineAndMakeCircleCommand = new RelayCommand(async () => - { - await _formationController.FlyToVerticalLineAndMakeCircle( - _copterManager.AcceptingControlCopters, - centerDirection: 180, - radius: 10 - ).ConfigureAwait(false); - })); - } - } - - private ICommand _MissionStartCommand; - public ICommand MissionStartCommand - { - get - { - return _MissionStartCommand ?? (_MissionStartCommand = new RelayCommand(async () => - { - FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); - if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) - { - Alert.Show("作为参照的原点未设置，无法开始任务！", "提示"); - return; - } - //飞机用UTC时间，日志用本地时间记录 - DateTime MissionTime = DateTime.UtcNow.AddSeconds(5); - DateTime MissionTime_log = DateTime.Now.AddSeconds(5); - Message.Show("任务开始:" + MissionTime_log.ToString()); - //循环3次 发送起飞命令 避免通信问题 - for (int i = 0; i < 20; i++) //20 - { - await _commModuleManager.DoMissionStartAsync(null, - MissionTime.Hour, - MissionTime.Minute, - MissionTime.Second, - _flightTaskManager.OriginLng, - _flightTaskManager.OriginLat - ); - - /* - foreach (var vcopter in _copterManager.Copters) - { - await vcopter.MissionStartAsync(utchour, - utcminute, - utcsecond, - _flightTaskManager.OriginLng, - _flightTaskManager.OriginLat - ); - } - */ - - await Task.Delay(20).ConfigureAwait(false); - - } - _copterManager.Net_LogStartMission(MissionTime_log, _flightTaskManager.OriginLng, _flightTaskManager.OriginLat); - Alert.Show("所有飞机开始执行航点任务。请勿多次开始任务！", "提示", MessageBoxButton.OK, MessageBoxImage.Information); - })); - } - } - - private ICommand _MissionPauseCommand; - public ICommand MissionPauseCommand - { - get - { - return _MissionPauseCommand ?? (_MissionPauseCommand = new RelayCommand(async () => - { - for (int i = 0; i < 3; i++) - { - await _commModuleManager.DoMissionPauseAsync(); - } - })); - } - } - - private ICommand _MissionResumeCommand; - public ICommand MissionResumeCommand - { - get - { - return _MissionResumeCommand ?? (_MissionResumeCommand = new RelayCommand(async () => - { - int utchour = DateTime.UtcNow.AddSeconds(5).Hour; - int utcminute = DateTime.UtcNow.AddSeconds(5).Minute; - int utcsecond = DateTime.UtcNow.AddSeconds(5).Second; - for (int i = 0; i < 3; i++) - { - await _commModuleManager.DoMissionResumeAsync( - utchour, - utcminute, - utcsecond); - } - })); - } - } - - - // private ICommand _SetOriginalPointCommand; - // public ICommand SetOriginalPointCommand - // { - // get - // { - // return _SetOriginalPointCommand ?? (_SetOriginalPointCommand = new RelayCommand(() => - // { - // FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); - // _flightTaskManager.SetOriginal(); - // })); - // } - // } - private ICommand _WriteMissionSingleCommand; - public ICommand WriteMissionSingleCommand - { - get - { - return _WriteMissionSingleCommand ?? (_WriteMissionSingleCommand = new RelayCommand(async () => - { - var _flightTaskManager = ServiceLocator.Current.GetInstance(); - if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) - { - Alert.Show("作为参照的原点未设置，无法写入相对位置！", "提示"); - return; - } - var _RtcmInfoViewModel = ServiceLocator.Current.GetInstance(); - if (_RtcmInfoViewModel.RtcmManager.Rtcmthreadrun) - { - Alert.Show("RTK数据正在发送，将自动关闭！", "提示"); - await _RtcmInfoViewModel.RtcmManager.Close(_RtcmInfoViewModel.SerialPortsSelectdValue); - } - - - int taskcount = _flightTaskManager.Tasks.Count; - if (taskcount < 2) return; - foreach (var copter in _copterManager.AcceptingControlCopters) - { - int i = _copterManager.Copters.IndexOf(copter); - await CollectMissions(i); - } - })); - } - } - - - private ICommand _WriteMissionFailedCommand; - public ICommand WriteMissionFailedCommand - { - get - { - return _WriteMissionFailedCommand ?? (_WriteMissionFailedCommand = new RelayCommand(async () => - { - var _flightTaskManager = ServiceLocator.Current.GetInstance(); - if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) - { - Alert.Show("作为参照的原点未设置，无法写入相对位置！", "提示"); - return; - } - var _RtcmInfoViewModel = ServiceLocator.Current.GetInstance(); - if (_RtcmInfoViewModel.RtcmManager.Rtcmthreadrun) - { - Alert.Show("RTK数据正在发送，将自动关闭！", "提示"); - await _RtcmInfoViewModel.RtcmManager.Close(_RtcmInfoViewModel.SerialPortsSelectdValue); - } - - int coptercount = _copterManager.Copters.Count; - int taskcount = _flightTaskManager.Tasks.Count; - if (taskcount < 2) return; - foreach(KeyValuePair kv in _commModuleManager.MissionWriteState) - { - if (!kv.Value.SendAchieved || !kv.Value.WriteSucceed) - { - var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == kv.Key.ToString()); - int i = _copterManager.Copters.IndexOf(copter); - await CollectMissions(i); - } - } - })); - } - } - - private ICommand _WriteMissionCommand; - public ICommand WriteMissionCommand - { - get - { - return _WriteMissionCommand ?? (_WriteMissionCommand = new RelayCommand(async () => - { - var _flightTaskManager = ServiceLocator.Current.GetInstance(); - if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) - { - Alert.Show("作为参照的原点未设置，无法写入相对位置！", "提示"); - return; - } - - int coptercount = _copterManager.Copters.Count; - int taskcount = _flightTaskManager.Tasks.Count; - if (taskcount < 2) return; - - var _RtcmInfoViewModel = ServiceLocator.Current.GetInstance(); - if (_RtcmInfoViewModel.RtcmManager.Rtcmthreadrun) - { - Alert.Show("RTK数据正在发送，将自动关闭！", "提示"); - await _RtcmInfoViewModel.RtcmManager.Close(_RtcmInfoViewModel.SerialPortsSelectdValue); - } - - Alert.Show("开始写入航点，飞机绿色成功红色失败！(先关灯再开灯可恢复飞机灯光)", "提示"); - _commModuleManager.ClearMissionWriteState(); - for (int i = 0; i < coptercount; i++) - { - ///写航线开始 - await CollectMissions(i).ConfigureAwait(false); - } - Alert.Show($"任务写入完成!请检测失败的飞机"); - })); - } - } - - /// - /// 用通讯模块 写入航点信息 - /// - /// copterIndex - /// - private async Task CollectMissions(int i) - { - int j = 0; - try - { - - - float groudAlt = _copterManager.Copters[i].GroundAlt; - var missions = new List(); - var _flightTaskManager = ServiceLocator.Current.GetInstance(); - for (j = 0; j < _flightTaskManager.Tasks.Count; j++) - { - switch (_flightTaskManager.Tasks[j].TaskType) - { - case FlightTaskType.TakeOff: - //int takeofftime = (int)(_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetAlt / 2.5 + 5); - missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos)); - missions.Add(Mission.CreateTakeoffMission( - _flightTaskManager.Tasks[j].SingleCopterInfos[i].TakeOffWaitTime, //起飞等待时间 - _flightTaskManager.Tasks[j].TakeOffTime, //起飞时间 - _flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLat - _flightTaskManager.OriginLat, //位置没用 - _flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLng - _flightTaskManager.OriginLng, //位置没用 - _flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetAlt - groudAlt) //下一个航点的高度 - ); - break; - - case FlightTaskType.FlyTo: - if (_flightTaskManager.Tasks[j].SingleCopterInfos[i].IsChangeSpeed) - { - missions.Add(Mission.CreateChangeSpeedMission( - _flightTaskManager.Tasks[j].SingleCopterInfos[i].LevelSpeed, - _flightTaskManager.Tasks[j].SingleCopterInfos[i].UpSpeed, - _flightTaskManager.Tasks[j].SingleCopterInfos[i].DownSpeed) - ); - } - missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos)); - - double Lat = _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLat - _flightTaskManager.OriginLat; - double Lng = _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLng - _flightTaskManager.OriginLng; - //如果是返航点，位置写入固定值，这样直接返回起飞点 - if (_flightTaskManager.Tasks[j].SingleCopterInfos[i].IsLandWaypoint) - { - Lat = 90; - Lng = 180; - } - - missions.Add(Mission.CreateWaypointMission( - _flightTaskManager.Tasks[j].LoiterTime, - _flightTaskManager.Tasks[j].FlytoTime, - Lat, - Lng, - _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt - groudAlt) - ); - break; - - case FlightTaskType.Land: - missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos)); - - missions.Add(Mission.CreateLandMission( - _flightTaskManager.Tasks[j].SingleCopterInfos[i].LandWaitTime) - ); - break; - default: - break; - } - } - //186是通讯模块的限制--张伟那边 - if (missions.Count > 186) - { - Message.Show($"飞机：{_copterManager.Copters[i].VirtualId} 航点数量超过186，请减少再重写该飞机!"); - } - else - { - //虚拟ID在排序时已经变为ID了，直接按ID写入 - // var targetCopter = _copterManager.Copters.FirstOrDefault(o => o.VirtualId == (i+1)); - // bool result = await _commModuleManager.WriteMissionListAsync(targetCopter, missions); - bool result = await _commModuleManager.WriteMissionListAsync(_copterManager.Copters[i], missions); - - //CommWriteMissinState state = new CommWriteMissinState(result); - //_commModuleManager.missionWriteState.Add(int.Parse(_copterManager.Copters[i].Id), state); - if (!result) - { - - if (_copterManager.SortType == CopterManager.CopterSortType.ByVID) - Message.Show($"飞机：{_copterManager.Copters[i].VirtualId} 通信模块传输失败!"); - else - Message.Show($"飞机：{_copterManager.Copters[i].Id} 通信模块传输失败!"); - } - else - { - if (_copterManager.SortType == CopterManager.CopterSortType.ByVID) - Message.Show($"飞机：{_copterManager.Copters[i].VirtualId} 通信模块传输完成!"); - else - Message.Show($"飞机：{_copterManager.Copters[i].Id} 通信模块传输完成!"); - } - await Task.Delay(500).ConfigureAwait(false); - } - } - catch (Exception ex) - { - Message.Show(ex.ToString()); - } - - } - - private List CreateLEDMissions(IEnumerable LEDInfos) - { - List missions = new List(); - foreach (LEDInfo ledInfo in LEDInfos) - { - float ledinterval=0; - Color color = (Color)ColorConverter.ConvertFromString("#" + ledInfo.LEDRGB); - int ledMode = ledInfo.LEDMode; - //拉烟任务模式ID是50，需要改为50 - if (_copterManager.FC_VER_NO == 1) - { - //老版本是闪烁频率，且不能为小数 - ledinterval = ledInfo.LEDInterval; - //老版本ledMode == 8 对应拉烟，飞控对应50,ledinterval对应通道号 - if (ledMode==8) ledMode = 50; - } - else - { - //新版本飞控 是闪烁间隔单位是100ms，地面站还是频率可以有1位小数,用户输入0.5hz 飞控应该是(1/0.5)*10=20 - ledinterval = (1 / ledInfo.LEDInterval); - - //新版本 ledMode=16是抛物，飞控对应50，ledinterval对应10 --固定的 - if (ledMode == 16) - { - ledMode = 50; - ledinterval = 10; - } - else //新版本ledMode == 15 对应拉烟，飞控对应50,ledinterval对应通道号 - if (ledMode == 15) - { - ledMode = 50; - } - } - IMission LEDMission = Mission.CreateLEDControlMission( - (int)(ledInfo.Delay * 10), - ledMode, - ledinterval, - 0, //ledInfo.LEDTimes, - color.R, - color.G, - color.B - ); - missions.Add(LEDMission); - } - return missions; - } - - private ICommand _FlagCommand; - public ICommand FlagCommand - { - get - { - return _FlagCommand ?? (_FlagCommand = new RelayCommand(async () => - { - await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => - { - - var copters = _copterManager.Copters; - int index = copters.IndexOf(copter); - _copterManager.CopterStatus[index] = true; - return Task.Run(()=> { }); - })); - })); - } - } - - private async Task UnlockTestSingleCopterAsync(ICopter SingleCopter) - { - await SingleCopter.UnlockAsync(); - await Task.Delay(25).ConfigureAwait(false); - - for (int i = 0; !SingleCopter.IsUnlocked; i++) - { - //8秒内每1000毫秒尝试解锁一次 - //解锁间隔一定要超过1s否则导致飞控以后无法解锁 - - if (i >320) - return; //无法解锁后面不用执行了 - if (i % (1000 / 25) == 1000 / 25 - 1) - { - await SingleCopter.UnlockAsync(); // 每 1000 毫秒重试一次。 - } - await Task.Delay(25).ConfigureAwait(false); - } - - - await Task.Delay(1000); - if (SingleCopter.IsUnlocked) - { - await SingleCopter.SetShowLEDAsync(true); - await Task.Delay(50); - await SingleCopter.SetShowLEDAsync(true); - } - - - await Task.Delay(5000); - - await SingleCopter.LockAsync(); - await Task.Delay(25).ConfigureAwait(false); - for (int i = 0; SingleCopter.IsUnlocked; i++) - { - //8秒内每1000毫秒尝试解锁一次 - //解锁间隔一定要超过1s否则导致飞控以后无法解锁 - - if (i > 320) - return; //无法解锁后面不用执行了 - if (i % (1000 / 25) == 1000 / 25 - 1) - { - await SingleCopter.LockAsync(); // 每 1000 毫秒重试一次。 - } - await Task.Delay(25).ConfigureAwait(false); - } - - await Task.Delay(1000); - if (!SingleCopter.IsUnlocked) - { - await SingleCopter.SetShowLEDAsync(false); - await Task.Delay(50); - await SingleCopter.SetShowLEDAsync(false); - } - } - - - - private ICommand _TestCommand; - public ICommand TestCommand - { - get - { - return _TestCommand ?? (_TestCommand = new RelayCommand(async () => - { - - - Message.Show("解锁测试--开始"); - int i = 0; - var tasksUnlockTest = new Task[_copterManager.Copters.Count]; - foreach (var vcopter in _copterManager.Copters) - { - tasksUnlockTest[i++] = UnlockTestSingleCopterAsync(vcopter); - //i++; - } - await Task.WhenAll(tasksUnlockTest).ConfigureAwait(false); - Message.Show("解锁测试--完成!"); - return; - - - - - - - - - - - foreach (var vcopter in _copterManager.Copters) - { - await vcopter.UnlockAsync(); - } - await Task.Delay(1000); - - foreach (var vcopter in _copterManager.Copters) - { - if (vcopter.IsUnlocked) - await vcopter.SetShowLEDAsync(true); - } - - await Task.Delay(5000); - - foreach (var vcopter in _copterManager.Copters) - { - await vcopter.LockAsync(); - await vcopter.SetShowLEDAsync(false); - - } - - - return; - - - - - - - - - - - - - var firstCopter = _copterManager.Copters.First(); - - // Test TurnAsync. - //var latLng = GeographyUtils.CalcLatLngSomeMetersAway(firstCopter.Latitude, firstCopter.Longitude, 135, 10); - //await _formationController.TurnAsync(_copterManager.AcceptingControlCopters, latLng.Item1, latLng.Item2); - - // Test FlyToLatLngAsync. - //var copters = _copterManager.AcceptingControlCopters; - //var centerLat = copters.Average(c => c.Latitude); - //var centerLng = copters.Average(c => c.Longitude); - //var latDelta = 10 * GeographyUtils.MetersToLocalLat; - //var lngDelta = 10 * GeographyUtils.GetMetersToLocalLon(copters.First().Latitude); - //var targetLat = centerLat + latDelta; - //var targetLng = centerLng + lngDelta; - //await _formationController.FlyToLatLngAsync(copters, targetLat, targetLng); - - // Test FlyInCircleAsync. - //await _formationController.FlyInCircleAsync(_copterManager.AcceptingControlCopters, centerDirection: 180, radius: 10, loop: false); - - // Test FlyAroundCenterOfCoptersAsync. - //await _formationController.FlyAroundCenterOfCoptersAsync(_copterManager.AcceptingControlCopters); - - // Test FlyToLatLngAsync. - //var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway(firstCopter.Latitude, firstCopter.Longitude, 135, 20); - //await _formationController.FlyToLatLngAsync(firstCopter, targetLatLng.Item1, targetLatLng.Item2); - - //firstCopter.LoiterTurns(1, 10); - - //var radius = await firstCopter.GetParamAsync("CIRCLE_RADIUS"); - - ////await firstCopter.SetParamAsync("CIRCLE_RADIUS", 1500); - ////var param = await firstCopter.GetParamAsync("CIRCLE_RADIUS"); - //// Alert.Show($"半径设置成功啦: {param}"); - - ////await firstCopter.SetParamAsync("CIRCLE_RATE", 30); - //var rate = await firstCopter.GetParamAsync("CIRCLE_RATE"); - - //Alert.Show($"半径: {radius} 角速度: {rate}"); - - //await firstCopter.SetMobileControlAsync(yaw: 90); - - //if (!_listeningMissionItemReceived) - //{ - // firstCopter.MissionItemReceived += (sender, e) => Alert.Show($"{e.Index} {e.Total} {(Protocols.MAVLink.MAV_CMD)e.Details.id}"); - // _listeningMissionItemReceived = true; - //} - //await firstCopter.RequestMissionListAsync(); - - var copter = firstCopter; - - ///* 写航线 */ - //// 任务总数。 - //await copter.SetMissionCountAsync(6).ConfigureAwait(false); - //// 起飞前准备。 - //await copter.WritePreTakeOffMissionAsync().ConfigureAwait(false); - //// 起飞。 - //await copter.WriteTakeOffMissionAsync().ConfigureAwait(false); - //// 不断自增的编号。 - //ushort index = 2; - //// 航点。 - //await copter.WriteWaypointMissionAsync(new EHLocation(copter).AddLatLngAlt(0.001, 0.002, 2), index++).ConfigureAwait(false); - //// 航点。 - //await copter.WriteWaypointMissionAsync(new EHLocation(copter).AddLatLngAlt(0.003, 0.002, 2), index++).ConfigureAwait(false); - //// 返航。 - //await copter.WriteReturnToLaunchMissionAsync(index++).ConfigureAwait(false); - //// 降落。 - //await copter.WriteLandMissionAsync(index++).ConfigureAwait(false); - - ///* 读航线 */ - // 监听“任务项已接收”事件。 - //(copter as EHCopter).MissionItemReceived += (sender, e) => Alert.Show($"{e.Mission.Sequence} {e.Mission.Latitude} {e.Mission.Command}"); - // 请求任务列表。 - var missions = await copter.RequestMissionListAsync(); - // Alert.Show(string.Join(Environment.NewLine, missions.Select(m => $"{m.Sequence}\t{m.Command}\t\t{m.Latitude}"))); - - //ICopter previousCopter = firstCopter; - //foreach (var item in _copterManager.Copters.Where(c => c != firstCopter)) - //{ - // item.Follow(previousCopter, keepYawDifference: false, keepFacingTarget: false); - // previousCopter = item; - //} - - //var missions = new Mission[] - //{ - // Mission.CreateWaypointMission(new EHLocation(firstCopter).AddLatLngAlt(0.0001, 0, 10)), - // Mission.CreateReturnToLaunchMission(), - // Mission.CreateLandMission() - //}; - //var result = await firstCopter.WriteMissionsAsync(missions); - //var result2 = await firstCopter.WriteMissionsAsync(missions); - //Debug.WriteLine($"{result1} {result2}"); - - //var missions = new Mission[] - //{ - // Mission.CreateWaypointMission(23.123, 113.123, 10), - // Mission.CreateWaypointMission(23.123, 113.123, 10), - // Mission.CreateWaypointMission(23.123, 113.123, 10), - // Mission.CreateWaypointMission(23.123, 113.123, 10), - // Mission.CreateWaypointMission(23.123, 113.123, 10), - // Mission.CreateWaypointMission(23.123, 113.123, 10), - // Mission.CreateWaypointMission(23.123, 113.123, 10), - // Mission.CreateWaypointMission(23.123, 113.123, 10), - // Mission.CreateWaypointMission(23.123, 113.123, 10), - // Mission.CreateWaypointMission(23.123, 113.123, 10), - // Mission.CreateReturnToLaunchMission(), - // Mission.CreateLandMission() - //}; - //var result = await firstCopter.WriteMissionListAsync(missions); - //MessageBox.Show($"The result of WriteMissions: {result}"); - - // 跟随测试 - /* - ICopter previousCopter = firstCopter; - foreach (var item in _copterManager.Copters.Where(c => c != firstCopter)) - { - item.Follow(previousCopter, keepYawDifference: false, keepFacingTarget: false); - previousCopter = item; - } - */ - await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async c => - { - - float gpsLed = await c.GetParamAsync("NOTI_GPSLED"); - if (gpsLed == 0.0f) - { - await LEDFlashAsync(c, true); - } - else - { - await LEDFlashAsync(c, false); - } - - // for (int jj = 0; jj < 5; jj++) - { - // await LEDFlashAsync(c, true); - // await c.SetParamAsync("THR_ALT_P", AltP); - // await c.SetParamAsync("WPNAV_ACCEL_Z", AltP); - // await c.SetParamAsync("WPNAV_ACCEL", AltP); - // await c.SetParamAsync("WPNAV_SPEED", AltP); - // await c.SetParamAsync("WPNAV_SPEED_DN", AltP); - // await c.SetParamAsync("WPNAV_SPEED_UP", AltP); - // await LEDFlashAsync(c, false); - // await LEDFlashAsync(c, true); - // await Task.Delay(500).ConfigureAwait(false); - - // await Task.Delay(1000).ConfigureAwait(false); - - // await LEDFlashAsync(c, true); - // await Task.Delay(1000).ConfigureAwait(false); - } - })); - })); - } - } - - /// - /// 读取串口号 - /// - private void LoadRTKcomvalue() - { - IniFiles inifilse = new IniFiles(); - RTKcomvalue = inifilse.IniReadvalue("Default", "RTKcomvalue"); - if (RTKcomvalue == "") RTKcomvalue = "COM6"; - } - - - /// - /// 保存上次成功的串口号 - /// - /// - private async Task SaveRTKcomvalue() - { - IniFiles inifilse = new IniFiles(); - inifilse.IniWritevalue("Default", "RTKcomvalue", RTKcomvalue); - await Task.Delay(1); - } - - private async Task LEDFlashAsync(ICopter copter, bool isOn) - { - // float gpsLed = await c.GetParamAsync("NOTI_GPSLED"); - float ledControl = 0.0f; - if (isOn) - { - ledControl = 1.0f; - } - var tasks = new Task[2]; - //tasks[0] = copter.SetParamAsync("NOTI_GPSLED", ledControl); - //tasks[1] = copter.SetParamAsync("NOTI_ARMLED", ledControl); - tasks[0] = Task.Run(async () => - { - try - { - await copter.SetParamAsync("NOTI_GPSLED", ledControl, 5000); - } - catch (TimeoutException e) - { - _logger.Log($"NOTI_GPSLED 超时, CopterId: {copter.Id}。"); - } - }); - tasks[1] = Task.Run(async () => - { - try - { - await copter.SetParamAsync("NOTI_ARMLED", ledControl, 5000); - } - catch (TimeoutException e) - { - _logger.Log($"NOTI_ARMLED 超时, CopterId: {copter.Id}。"); - } - }); - await Task.WhenAll(tasks).ConfigureAwait(false); - - } - - private bool _listeningMissionItemReceived; - - private ICommand _StopTaskCommand; - public ICommand StopTaskCommand - { - get - { - return _StopTaskCommand ?? (_StopTaskCommand = new RelayCommand(async () => - { - await _formationController.AllStop().ConfigureAwait(false); - })); - } - } - - - } -} +using Plane.Copters; +using Plane.FormationCreator.Formation; +using Plane.Geography; +using Plane.Logging; +using Plane.Windows.Messages; +using GalaSoft.MvvmLight; +using GalaSoft.MvvmLight.CommandWpf; +using Microsoft.Practices.ServiceLocation; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows; +using System.Windows.Input; +using Plane.Communication; +using Microsoft.Win32; +using Plane.Util; +using System.Windows.Media; +using Plane.CommunicationManagement; +using Plane.FormationCreator.Views; +using Plane.Windows.IniHelper; + +namespace Plane.FormationCreator.ViewModels +{ + public class ControlPanelViewModel : ViewModelBase + { + // serialport + internal static IConnection Rtkport ; + + private static bool trkthreadrun = false; + + private static bool rtcmthreadrun = false; + + + + public ControlPanelViewModel(FormationController formationController, CopterManager copterManager) + { + _formationController = formationController; + _copterManager = copterManager; + LoadRTKcomvalue(); + } + + private FormationController _formationController; + private CopterManager _copterManager; + private CommModuleManager _commModuleManager = CommModuleManager.Instance; + + private ILogger _logger = ServiceLocator.Current.GetInstance(); + + private float _AltP = 0.5f; + public float AltP + { + get { return _AltP; } + set { Set(nameof(AltP), ref _AltP, value); } + } + + private float _TaskOffAlt = 2; + public float TaskOffAlt + { + get { return _TaskOffAlt; } + set { Set(nameof(TaskOffAlt), ref _TaskOffAlt, value); } + } + + private string _RTKState = "RTK未发送"; + public string RTKState + { + get { return _RTKState; } + set { Set(nameof(RTKState), ref _RTKState, value); } + } + + private string _RTKbtntxt = "发送RTK"; + public string RTKbtntxt + { + get { return _RTKbtntxt; } + set { Set(nameof(RTKbtntxt), ref _RTKbtntxt, value); } + } + + private string _NTRIPbtntxt = "发送RTCM"; + public string NTRIPbtntxt + { + get { return _NTRIPbtntxt; } + set { Set(nameof(NTRIPbtntxt), ref _NTRIPbtntxt, value); } + } + + private ICommand _LockAllCommand; + public ICommand LockAllCommand + { + get + { + return _LockAllCommand ?? (_LockAllCommand = new RelayCommand(async () => + { + // await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync())); + if (Alert.Show("您确定要上锁吗？所有飞行器将无视转速，立即强制停止运转！！！！", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning) + == MessageBoxResult.OK) + { + if (Alert.Show("再次确认上锁？请确认所有飞行器都在地面，否则将自由落体", "再次警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning) + == MessageBoxResult.OK) + { + await _commModuleManager.LockAsync(); + /* + await Task.WhenAll(_copterManager.Copters.Select(async c => + { + await c.LockAsync(); + })); + */ + } + } + })); + } + } + + + private static readonly object locker = new object(); + private ICommand _DetectionVoltage; + public ICommand DetectionVoltage + { + get + { + return _DetectionVoltage ?? (_DetectionVoltage = new RelayCommand(async () => + { + + float minVol; + InputDialog inputDialog = new InputDialog("最低检测电压( 0 恢复灯光):", "15.6"); + if (inputDialog.ShowDialog() == true) + minVol = float.Parse(inputDialog.Answer); + else return; + + //测试电池 + await _commModuleManager.TestBattery((short)0, minVol); + if (minVol == 0.0) return; + Message.Show("---请检查飞机灯光---"); + Message.Show(string.Format("---绿 色 电压高于 {0:F2}v (单节高于 {1:F2}v)--- ", minVol, minVol / 4.0f)); + Message.Show(string.Format("---黄 色 电压低于 {0:F2}v (单节低于 {1:F2}v)--- ", minVol, minVol / 4.0f)); + Message.Show(string.Format("---棕 色 电压低于 {0:F2}v (单节低于 {1:F2}v)--- ", minVol - 0.2, (minVol - 0.2) / 4.0f)); + Message.Show(string.Format("---洋红色 电压低于 {0:F2}v (单节低于 {1:F2}v)--- ", minVol - 0.4, (minVol - 0.4) / 4.0f)); + Message.Show(string.Format("---亮红色 电压低于 {0:F2}v (单节低于 {1:F2}v)--- ", minVol - 0.6, (minVol - 0.6) / 4.0f)); + Message.Show("---输入0恢复灯光---"); + + Message.Show("--------------开始检测单机电压--------------"); + Dictionary dic_voltage = new Dictionary(); + await Task.WhenAll(_copterManager.Copters.Select(async c => + { + float voltageSum = 0.0f; + string name = c.Name; + + for (int i = 0; i < 5; i++) + { + + voltageSum += c.Voltage; + await Task.Delay(1000).ConfigureAwait(false); + } + float voltageAverage = voltageSum / 5; + if (name != null && name != "") + { + lock (locker) + { + dic_voltage.Add(name, voltageAverage); + } + } + })).ConfigureAwait(false); + + + Dictionary dic_voltage_Order = dic_voltage.OrderByDescending(o => o.Value).ToDictionary(o => o.Key, o => o.Value); + bool showch = false; + ICopter vcopter = null; + _copterManager.shiftkeydown = true; + //清除选择 + _copterManager.Select(null); + foreach (KeyValuePair kv in dic_voltage_Order) + { + if ((!showch) && (minVol > 0.00) && (kv.Value <= minVol)) + { + Message.Show(string.Format("以下飞机电压低于[{0}V，单节{1:F2}]", minVol, minVol / 4)); + showch = true; + } + Message.Show(string.Format("{0} --> 5秒平均电压：{1:F2},单节{2:F2}", kv.Key, kv.Value, kv.Value / 4)); + + + //选中低电压飞机 + if ((minVol > 0.00) && (kv.Value <= minVol)) + { + vcopter = _copterManager.Copters.FirstOrDefault(o => o.Name == kv.Key); + if (vcopter != null) + _copterManager.Select(vcopter); //需要访问UI线程 + } + await Task.Delay(5).ConfigureAwait(false); + } + await Task.Delay(500).ConfigureAwait(false); + _copterManager.shiftkeydown = false; + Message.Show(string.Format("----检测电压完成,检测总数：{0}----", dic_voltage.Count)); + //todo:增加电压阈值变成不同颜色灯 + })); + } + } + + private ICommand _DetectionMissionData; + public ICommand DetectionMissionData + { + get + { + return _DetectionMissionData ?? (_DetectionMissionData = new RelayCommand(async () => + { + var missionState = _commModuleManager.MissionWriteState.Where(i => i.Value.IsFailed); + if (missionState.Count() == 0) + Message.Show("航点全部写入成功"); + else + { + Message.Show("----统计失败航点写入失败----"); + foreach (KeyValuePair item in missionState) + { + CommWriteMissinState state = item.Value; + string msg = $"{item.Key}:飞机状态:{state.StateReturn}, 错误码:{state.ErrorCode}, 传输:{state.SendAchieved}, 写入:{state.WriteSucceed} "; + Message.Show(msg); + await Task.Delay(10); + } + } + })); + } + } + + private ICommand _DetectionReturnData; + public ICommand DetectionReturnData + { + get + { + return _DetectionReturnData ?? (_DetectionReturnData = new RelayCommand(async () => + { + Dictionary> dic_returnData = new Dictionary>(); + await Task.WhenAll(_copterManager.Copters.Select(async c => + { + + string name = c.Name; + int return_data = BitConverter.ToInt32(c.Retain, 0); + + lock (locker) + { + List copterList; + if (!dic_returnData.ContainsKey(return_data)) + { + copterList = new List(); + dic_returnData.Add(return_data, copterList); + } + else + copterList = dic_returnData[return_data]; + + copterList.Add(name); + } + await Task.Delay(10).ConfigureAwait(false); + })).ConfigureAwait(false); + + await Task.Run(async () => + { + foreach (KeyValuePair> kv in dic_returnData) + { + byte[] test = BitConverter.GetBytes(kv.Key); + Array.Reverse(test); + string data = string.Join(".", test); + string copters = string.Join(",", kv.Value.ToArray()); + Message.Show(string.Format("返回数据{0}({1})：{2}", data, kv.Key,copters)); + await Task.Delay(5).ConfigureAwait(false); + } + Message.Show("----统计完成----"); + }).ConfigureAwait(false); + + })); + } + } + + private ICommand _ReportGPSTypeCommand; + public ICommand ReportGPSTypeCommand + { + get + { + return _ReportGPSTypeCommand ?? (_ReportGPSTypeCommand = new RelayCommand(async()=> + { + if (_copterManager.Copters.Count > 0) + { + Dictionary> GPSTypes = new Dictionary>(); + + Dictionary> Heading = new Dictionary>(); + + foreach (var copter in _copterManager.Copters) + { + string name = copter.Name; + if (!GPSTypes.Keys.Contains(copter.GpsFixType.ToString())) + { + List copterNames = new List(); + copterNames.Add(name); + GPSTypes.Add(copter.GpsFixType.ToString(), copterNames); + } + else + GPSTypes[copter.GpsFixType.ToString()].Add(name); + + + + + if (!Heading.Keys.Contains(copter.Heading.ToString())) + { + List copterNames = new List(); + copterNames.Add(name); + Heading.Add(copter.Heading.ToString(), copterNames); + } + else + Heading[copter.Heading.ToString()].Add(name); + + + } + Message.Show($"==定位状态=="); + + foreach (var item in GPSTypes) + { + Message.Show($"{item.Key}:{string.Join(",", item.Value)}"); + Message.Show($"------------"); + } + Message.Show($"==机头方向=="); + //从小到大排个序 + Dictionary> SortedHeading = Heading.OrderBy(p => p.Key).ToDictionary(p => p.Key, o => o.Value); + + foreach (var item in SortedHeading) + { + Message.Show($"{item.Key} 度:{string.Join(",", item.Value)}"); + Message.Show($"------------"); + + } + + } + await Task.Delay(10); + })); + } + } + + + + private ICommand _DetectionCommModuleVersion; + public ICommand DetectionCommModuleVersion + { + get + { + return _DetectionCommModuleVersion ?? (_DetectionCommModuleVersion = new RelayCommand(async () => + { + Dictionary> dic_version = new Dictionary>(); + await Task.WhenAll(_copterManager.Copters.Select(async c => + { + string name = c.Name; + int version = c.CommModuleVersion; + + lock (locker) + { + List copterList; + if (!dic_version.ContainsKey(version)) + { + copterList = new List(); + dic_version.Add(version, copterList); + } + else + copterList = dic_version[version]; + + copterList.Add(name); + } + await Task.Delay(10).ConfigureAwait(false); + })).ConfigureAwait(false); + + await Task.Run(async () => + { + foreach (KeyValuePair> kv in dic_version) + { + string copters = string.Join(",", kv.Value.ToArray()); + Message.Show(string.Format("通信模块版本{0}：{1}", kv.Key, copters)); + await Task.Delay(5).ConfigureAwait(false); + } + Message.Show("----统计完成----"); + }).ConfigureAwait(false); + + })); + } + } + + private ICommand _AllLandCommand; + public ICommand AllLandCommand + { + get + { + return _AllLandCommand ?? (_AllLandCommand = new RelayCommand(async () => + { + _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); + await _commModuleManager.LandAsync(); + /* + await Task.WhenAll(_copterManager.Copters.Select(async c => + { + await c.LandAsync(); + })); + */ + })); + } + } + + private ICommand _EmergencyRetCommand; + public ICommand EmergencyRetCommand + { + get + { + return _EmergencyRetCommand ?? (_EmergencyRetCommand = new RelayCommand(async () => + { + if (Alert.Show("您确定要紧急返航吗？紧急返航可能导致飞行器碰撞！！！！", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning) + != MessageBoxResult.OK) + return; //计算当前图案中心点，起飞图案中心点，图案距离起飞图案中心点最近距离 + FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + //计算无人机返航方向和最近的飞机距离 + PLLocation takeoffcentloc; + PLLocation taskcentloc; + double mindistance; //单位米 + bool overlapping; //图案否重叠 + int rettime= (int)(DateTime.Now - _flightTaskManager.taskStartTime).TotalSeconds; + if (!_flightTaskManager.EmergencyRet(rettime,out takeoffcentloc,out taskcentloc, out mindistance,out overlapping)) + { + Alert.Show("紧急返航数据计算失败！", "提示"); + return; + } + rettime += 5; //加5秒延迟用于通讯重复，有些飞机通讯不成功多次发送才收到，比别的飞机慢 + + //模拟测试 + if (_flightTaskManager.TaskState != TasksStatus.Stop) + await _flightTaskManager.sim_DoMissionEmergencyRetAsync(takeoffcentloc, taskcentloc, (float)mindistance, rettime, overlapping); + for (int i = 0; i < 10; i++) + { + await _commModuleManager.DoMissionEmergencyRetAsync(takeoffcentloc,taskcentloc,(float)mindistance, rettime, overlapping); + } + })); + } + } + + + private ICommand _LEDFlickerCommand; + public ICommand LEDFlickerCommand + { + get + { + return _LEDFlickerCommand ?? (_LEDFlickerCommand = new RelayCommand(() => + { + _commModuleManager.LED_FlickerAsync(_copterManager.AcceptingControlCopters); + })); + } + } + + + private ICommand _DelCommand; + public ICommand DelCommand + { + get + { + return _DelCommand ?? (_DelCommand = new RelayCommand(async () => + { + FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + { + if (_flightTaskManager.Tasks.Count > 0) + { + Alert.Show("有航点任务无法删除飞机,请先清除航点！", "警告", MessageBoxButton.OK, MessageBoxImage.Warning); + return; + } + + await _copterManager.DelSelCopters(); + } + })); + } + } + + + private ICommand _MotorTestCommand; + public ICommand MotorTestCommand + { + get + { + return _MotorTestCommand ?? (_MotorTestCommand = new RelayCommand(async () => + { + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + { + await _commModuleManager.MotorTestAsync(_copterManager.AcceptingControlCopters); + } + + /* + + await Task.WhenAll(_copterManager.Copters.Select(async c => + { + for (int i = 1; i <= 4; i++) + { + await c.MotorTestAsync(i).ConfigureAwait(false); + await Task.Delay(10); + } + + })).ConfigureAwait(false); + */ + })); + } + } + + private ICommand _GuidAsyncCommand; + public ICommand GuidAsyncCommand + { + get + { + return _GuidAsyncCommand ?? (_GuidAsyncCommand = new RelayCommand(async () => + { + if (TaskOffAlt <= 0) return; + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + await _commModuleManager.TakeOffAsync(TaskOffAlt, _copterManager.AcceptingControlCopters); + /* + await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async c => + { + await c.GuidAsync(); + await Task.Delay(100).ConfigureAwait(false); + await c.TakeOffAsync(1); + })); + */ + })); + } + } + + /// + /// 是否允许开始任务，全部解锁会在3秒内循环发送3次，该过程中不允许开始任务 + /// + private bool _AllowMissionStart = true; + public bool AllowMissionStart + { + get { return _AllowMissionStart; } + set { Set(nameof(AllowMissionStart), ref _AllowMissionStart, value); } + } + + + private ICommand _UnlockAllCommand; + public ICommand UnlockAllCommand + { + get + { + return _UnlockAllCommand ?? (_UnlockAllCommand = new RelayCommand(async () => + { + AllowMissionStart = false; + // await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync())); + for (int i = 0; i < 3; i++) + { + _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); + await _commModuleManager.UnlockAsync(); + await Task.Delay(1000).ConfigureAwait(false); + } + await Task.Delay(100); + AllowMissionStart = true; + + /* + await Task.WhenAll(_copterManager.Copters.Select(async c => { + await c.UnlockAsync(); + //解锁间隔一定要超过1s否则导致飞控以后无法解锁 + for (int i = 0; !c.IsUnlocked; i++) + { + if (i > 200) + break; + if (i % (1000 / 25) == 1000 / 25 - 1) + { + await c.UnlockAsync(); // 每 1000 毫秒重试一次。解锁间隔一定要超过1s否则导致飞控以后无法解锁 + } + await Task.Delay(25).ConfigureAwait(false); + } + })); + */ + })); + } + } + + private const string NTF_GROUPLED_OFF = "NTF_G_OFF"; + private ICommand _LEDOnOffCommand; + public ICommand LEDOnOffCommand + { + get + { + return _LEDOnOffCommand ?? (_LEDOnOffCommand = new RelayCommand(async onOff=> + { + string paramstr = NTF_GROUPLED_OFF; + float paramvalue = onOff; + await _commModuleManager.SetParamAsync(paramstr, paramvalue, _copterManager.AcceptingControlCopters); + /* + await Task.WhenAll( + _copterManager.AcceptingControlCopters.Select(async copter => + { + paramvalue = await copter.GetParamAsync(paramstr); + })); + + float newParamvalue = paramvalue == 0 ? 1 : 0; + + await Task.WhenAll(_copterManager.AcceptingControlCopters.Select + (copter => copter.SetParamAsync(paramstr, newParamvalue))); + */ + })); + } + } + + private ICommand _UnlockCommand; + public ICommand UnlockCommand + { + get + { + return _UnlockCommand ?? (_UnlockCommand = new RelayCommand(async () => + { + // await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync())); + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + await _commModuleManager.UnlockAsync(_copterManager.AcceptingControlCopters); + + /* + await Task.WhenAll(_copterManager.AcceptingControlCopters.Select( async c => { + await c.UnlockAsync(); + //解锁间隔一定要超过1s否则导致飞控以后无法解锁 + for (int i = 0; !c.IsUnlocked; i++) + { + if (i > 200) + break; + if (i % (1000 / 25) == 1000 / 25 - 1) + { + await c.UnlockAsync(); // 每 1000 毫秒重试一次。解锁间隔一定要超过1s否则导致飞控以后无法解锁 + } + await Task.Delay(25).ConfigureAwait(false); + } + })); + */ + })); + } + } + + private float _LatOffset = 18.4023833f; + public float LatOffset + { + get { return _LatOffset; } + set { Set(nameof(LatOffset), ref _LatOffset, value); } + } + + private float _LngOffset = 109.799761f; + public float LngOffset + { + get { return _LngOffset; } + set { Set(nameof(LngOffset), ref _LngOffset, value); } + } + + + private ICommand _RLTOffsetCommand; + public ICommand RLTOffsetCommand + { + get + { + return _RLTOffsetCommand ?? (_RLTOffsetCommand = new RelayCommand(() => + { +// Message.Show($"偏移Lat = {LatOffset}，Lng = {LngOffset}"); +// if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) +// await _commModuleManager.RLTOffsetAsync(LatOffset, LngOffset, _copterManager.AcceptingControlCopters); + + + })); + } + } + + private ICommand _LockCommand; + public ICommand LockCommand + { + get + { + return _LockCommand ?? (_LockCommand = new RelayCommand(async () => + { + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + { + if (Alert.Show("您确定要上锁吗？飞行器将无视转速，立即强制停止运转！！！", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning) + == MessageBoxResult.OK) + { + _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); + await _commModuleManager.LockAsync(_copterManager.AcceptingControlCopters); + //await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.LockAsync())); + } + } + })); + } + } + + private ICommand _TakeOffCommand; + public ICommand TakeOffCommand + { + get + { + return _TakeOffCommand ?? (_TakeOffCommand = new RelayCommand(async () => + { + FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) + { + Alert.Show("作为参照的原点未设置，无法开始任务！", "提示"); + return; + } + //飞机用UTC时间，日志用本地时间记录 + DateTime MissionTime = DateTime.UtcNow.AddSeconds(5); + DateTime MissionTime_log = DateTime.Now.AddSeconds(5); + Message.Show("任务开始:" + MissionTime_log.ToString()); + + IEnumerable selcopters = _copterManager.AcceptingControlCopters; + + //循环3次 发送起飞命令 避免通信问题 + for (int i = 0; i < 20; i++) + { + + /*await _commModuleManager.DoMissionStartAsync(_copterManager.Copters, + MissionTime.Hour, + MissionTime.Minute, + MissionTime.Second, + _flightTaskManager.OriginLng, + _flightTaskManager.OriginLat + ); + */ + + + await _commModuleManager.DoMissionStartAsync(selcopters, + MissionTime.Hour, + MissionTime.Minute, + MissionTime.Second, + _flightTaskManager.OriginLng, + _flightTaskManager.OriginLat + ); + + + + /* await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.MissionStartAsync( + MissionTime.Hour, + MissionTime.Minute, + MissionTime.Second, + _flightTaskManager.OriginLng, + _flightTaskManager.OriginLat + ))); + */ + + + await Task.Delay(20).ConfigureAwait(false); + + } + //_copterManager.Net_LogStartMission(MissionTime_log, _flightTaskManager.OriginLng, _flightTaskManager.OriginLat); + Alert.Show("所有选中飞机开始执行航点任务。请勿多次开始任务！", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + })); + } + } + + private ICommand _ThrowoutCommand; + public ICommand ThrowoutCommand + { + get + { + return _ThrowoutCommand ?? (_ThrowoutCommand = new RelayCommand( () => + { + // await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync())); + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + _commModuleManager.ThrowoutAsync(_copterManager.AcceptingControlCopters); + })); + } + } + + private ICommand _HoverCommand; + public ICommand HoverCommand + { + get + { + return _HoverCommand ?? (_HoverCommand = new RelayCommand(async () => + { + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + await _commModuleManager.HoverAsync(_copterManager.AcceptingControlCopters); + //await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.HoverAsync())); + })); + } + } + + private ICommand _FloatCommand; + public ICommand FloatCommand + { + get + { + return _FloatCommand ?? (_FloatCommand = new RelayCommand(async () => + { + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + await _commModuleManager.FloatAsync(_copterManager.AcceptingControlCopters); + //await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.FloatAsync())); + })); + } + } + + private ICommand _ReturnToLaunchCommand; + public ICommand ReturnToLaunchCommand + { + get + { + return _ReturnToLaunchCommand ?? (_ReturnToLaunchCommand = new RelayCommand(async () => + { + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + await _commModuleManager.ReturnToLaunchAsync(_copterManager.AcceptingControlCopters); + //await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.ReturnToLaunchAsync())); + })); + } + } + + + private ICommand _GetVersionsCommand; + public ICommand GetVersionsCommand + { + get + { + return _GetVersionsCommand ?? (_GetVersionsCommand = new RelayCommand(async () => + { + //if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + await _commModuleManager.GetVersionsAsync(); + })); + } + } + private ICommand _GetCommsumCommand; + public ICommand GetCommsumCommand + { + get + { + return _GetCommsumCommand ?? (_GetCommsumCommand = new RelayCommand(async () => + { + //if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + await _commModuleManager.GetCommsumAsync(); + })); + } + } + //打开校准界面 + private ICommand _CalibrationSingleCommand; + public ICommand CalibrationSingleCommand + { + get + { + return _CalibrationSingleCommand ?? (_CalibrationSingleCommand = new RelayCommand(async () => + { + CalibrationWindow calibrationWindow = new CalibrationWindow(); + calibrationWindow.ShowDialog(); + await Task.Delay(100); + })); + } + } + + private ICommand _LoadParamfile; + public ICommand LoadParamfile + { + get + { + return _LoadParamfile ?? (_LoadParamfile = new RelayCommand(async () => + { + + var dialog = new OpenFileDialog + { + DefaultExt = "param", + Filter = "飞行参数 (*.param)|*.param" + }; + if (dialog.ShowDialog() == true) + { + + bool ifallok = true; + var paramlist = ParamFile.loadParamFile(dialog.FileName); + + foreach (string name in paramlist.Keys) + { + float value = float.Parse( paramlist[name].ToString()); + //写5次 + for (int i = 0; i < 5; i++) + { + + if (_copterManager.AcceptingControlCopters.Count() < _copterManager.Copters.Count) + await _commModuleManager.SetParamAsync(name, value, _copterManager.AcceptingControlCopters); + else if (_copterManager.AcceptingControlCopters.Count() == _copterManager.Copters.Count) + { + _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); + await _commModuleManager.SetParamAsync(name, value); + } + + + + // await Task.WhenAll(_copterManager.AcceptingControlCopters.Select + // (copter => copter.SetParamAsync(name, value))); + } + await Task.Delay(2000); + + //发送读取5次 + for (int i = 0; i < 5; i++) + { + if (_copterManager.AcceptingControlCopters.Count() < _copterManager.Copters.Count) + await _commModuleManager.ReadParamAsnyc(name, _copterManager.AcceptingControlCopters); + else if (_copterManager.AcceptingControlCopters.Count() == _copterManager.Copters.Count) + await _commModuleManager.ReadParamAsnyc(name); + } + await Task.Delay(1000); + + //开始检查飞机返回参数是否正确 + + foreach (ICopter copter in _copterManager.AcceptingControlCopters) + { + if (value != copter.RetainInt) + { + ifallok = false; + Alert.Show(copter.Id + "参数[" + name + "]设置失败,读取的值是[" + copter.RetainInt + "]", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + + } + + + } + + + + + + /* + await Task.WhenAll( + + _copterManager.AcceptingControlCopters.Select(async copter => + { + float getvaule = await copter.GetParamAsync(name); + if (getvaule != value) + { + ifallok = false; + Alert.Show(copter.Id +"参数["+ name+ "]设置失败,读取的值是[" + getvaule+"]", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + } + } + + )); + */ + + + } + + if (ifallok) + Alert.Show("所有已选的飞机参数设置完成", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + else + Alert.Show("有部分飞机参数设置失败，请检查并再次设置", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + } + })); + } + } + + + private ICommand _ParamModify; + public ICommand ParamModify + { + get + { + return _ParamModify ?? (_ParamModify = new RelayCommand(async () => + { + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + { + var ModifyParamWindow = new Plane.FormationCreator.ModifyParam(); + if (ModifyParamWindow.ShowDialog() == true) + { + bool isLoadParam = ModifyParamWindow.LoadParam; + string paramstr = ModifyParamWindow.textParamName.Text; + + if(!isLoadParam) + { + float paramvalue = Convert.ToSingle(ModifyParamWindow.textParamValue.Text); + int num = 0; + int writeTime = 0; + //连续写5次，为了都写成功 + for ( writeTime=0; writeTime<5; writeTime++) + { + + if (_copterManager.AcceptingControlCopters.Count() < _copterManager.Copters.Count) + num = await _commModuleManager.SetParamAsync(paramstr, paramvalue, _copterManager.AcceptingControlCopters); + else if (_copterManager.AcceptingControlCopters.Count() == _copterManager.Copters.Count) + { + _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); + num = await _commModuleManager.SetParamAsync(paramstr, paramvalue); + } + } + Alert.Show($"广播完成！ 序列号从：{num-writeTime+1}到{num}"); + } + else + { + if (_copterManager.AcceptingControlCopters.Count() < _copterManager.Copters.Count) + await _commModuleManager.ReadParamAsnyc(paramstr, _copterManager.AcceptingControlCopters); + else if (_copterManager.AcceptingControlCopters.Count() == _copterManager.Copters.Count) + await _commModuleManager.ReadParamAsnyc(paramstr); + } + + /* + 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 _TurnOffTestLightsCommand; + public ICommand TurnOffTestLightsCommand + { + get + { + return _TurnOffTestLightsCommand ?? (_TurnOffTestLightsCommand = new RelayCommand(async () => + { + int num = 0; + //正式飞行 + num = await _commModuleManager.SetMultipleParamAsync( + "NTF_G_RTLOFF", "1", + "FS_GCS_ENABLE", "0", + "NTF_G_RTKTEST", "0", + "WAYPOINT_GLED", "0", + "FS_BATT_VOLTAGE", "14.2", + "RTL_ALT","3000", + "RTL_ALT_MAX","5000", + "FC_LOG_LEVEL", "0", + "FC_LOG_TYPE", "0", + "FS_GPS_RTL", "4"); + + //测试飞行 + /* + num = await _commModuleManager.SetMultipleParamAsync( + "NTF_G_RTLOFF", "0", + "FS_GCS_ENABLE", "1", + "NTF_G_RTKTEST", "1", + "WAYPOINT_GLED", "1", + "FS_BATT_VOLTAGE", "14.2", + "RTL_ALT", "3000", + "RTL_ALT_MAX", "5000", + "FC_LOG_LEVEL","3", + "FC_LOG_TYPE", "1", + "FS_GPS_RTL", "4" + ); + */ + + Alert.Show($"广播完成！ 当前序列号：{num}"); + })); + } + } + + + private string _RTKcomvalue = "COM6"; + public string RTKcomvalue + { + get { return _RTKcomvalue; } + set { Set(nameof(RTKcomvalue), ref _RTKcomvalue, value); } + } + + //RTK收发线程 + private static void mainloop() + { + + + + } + + + private async Task ReadRTKPacketAsync() + { + if (!Rtkport.IsOpen) + { + return null; + } + + // limit to 110 byte packets + byte[] buffer; + int length = 0; + + try + { + length = Math.Min(180, Rtkport.BytesToRead()); + + if (length > 0)//while (Rtkport.BytesToRead() > 0) + + { + buffer = new byte[length]; + + int read = await Rtkport.ReadAsync(buffer, 0, Math.Min(buffer.Length, length)); + + // sendData(buffer, (byte)read); + + } + else + return null; + + /* + if (await Rtkport.ReadAsync(buffer, 0, 1) == 0) + { + // 做简单数据解析，得到协议类型，后续数据长度等 + } + //根据数据长度读取实际数据 + if (await Rtkport.ReadAsync(buffer, 1, 1) == 0) + { + + } + */ + + } + catch (Exception ex) + { + return null; + } + return buffer; + + } + + + + private ICommand _SendRTKCommand; + public ICommand SendRTKCommand + { + get + { + return _SendRTKCommand ?? (_SendRTKCommand = new RelayCommand(async () => + { + if (!trkthreadrun) + { + Rtkport = new SerialPortConnection(RTKcomvalue, 57600) as IConnection; + await Rtkport.OpenAsync(); + if (!Rtkport.IsOpen) + { + Alert.Show("无法打开" + RTKcomvalue, "警告", MessageBoxButton.OK, MessageBoxImage.Warning); + return; + } + trkthreadrun = true; //开始运行后台任务 + /* //线程方式后台运行rtk转发任务 + thrtk = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop)) + { + IsBackground = true, + Name = "injectgps" + }; + thrtk.Start(); + */ + //后台任务方式运行rtk转发任务 + Alert.Show("RTK数据开始发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + RTKState = "RTK数据发送中"; + RTKbtntxt = "停止RTK"; + await SaveRTKcomvalue(); + await _commModuleManager.StartRtcmLoop(); + await Task.Run(async () => + { + Message.Show("RTK数据开始发送"); + //读取RTK数据循环 + rtcm3 rtcm3 = new rtcm3(); + while (trkthreadrun) + { + //读入RTK数据 + var packet = await ReadRTKPacketAsync().ConfigureAwait(false); + + if (packet != null && packet.Length > 0) + { + //Message.Show($"{DateTime.Now.ToString("HH:mm:ss fff")}---packet.length = {packet.Length}"); + //新方案的RTK发送，用于双频 + + int read = packet.Length; + if (read > 0) + { + for (int a = 0; a < read; a++) + { + int seenmsg = -1; + // rtcm + if ((seenmsg = rtcm3.Read(packet[a])) > 0) + { + Message.Show($"{DateTime.Now.ToString("HH:mm:ss fff")}---rtcm3.length = {rtcm3.length}"); + await _commModuleManager.InjectGpsRTCMDataAsync(rtcm3.packet, rtcm3.length); + } + } + } + + + + //稳定方案的rtk发送，用于单频 + //_commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); + //await _commModuleManager.InjectGpsDataAsync(packet, (ushort)packet.Length); + } + await Task.Delay(10).ConfigureAwait(false); + + } + }).ConfigureAwait(false); + Alert.Show("RTK数据停止发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + } + else//停止RTK + { + await _commModuleManager.CloseRtcmLoop(); + + trkthreadrun = false; + Rtkport.Close(); + Rtkport = null; + + RTKState = "未发送RTK数据"; + RTKbtntxt = "发送RTK"; + } + })); + } + } + + + private ICommand _SendRTCMCommand; + public ICommand SendRTCMCommand + { + get + { + return _SendRTCMCommand ?? (_SendRTCMCommand = new RelayCommand(async () => + { + FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) + { + Alert.Show("作为参照的原点未设置，无法发送RTCM！", "提示"); + return; + } + + rtcmthreadrun = !rtcmthreadrun; + if (rtcmthreadrun) + { + string messagetips = "格式：http://差分账号:差分密码@域名:端口/坐标系\r\n例如：http://account:password@rtk.ntrip.qxwz.com:8002/RTCM32_GGB"; + string url = ""; + IniFiles inifilse = new IniFiles(); + url = inifilse.IniReadvalue("Default","RTKurl"); + if (PlaneMessageBox.OnShow(messagetips, "发送RTK", ref url) == false) return; + inifilse.IniWritevalue("Default", "RTKurl", url); + + RTKState = "RTK数据发送中"; + NTRIPbtntxt = "停止RTCM"; + Alert.Show("RTCM数据开始发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + + await _commModuleManager.StartRtcmLoop(); + await Task.Run(async () => + { + Message.Show("RTK数据开始发送"); + //string url = "http://qxarpz003:1dc880b@rtk.ntrip.qxwz.com:8002/RTCM32_GGB"; + CommsNTRIP commNTRIP = new CommsNTRIP(url, _flightTaskManager.OriginLat, _flightTaskManager.OriginLng); + rtcm3 rtcm3 = new rtcm3(); + commNTRIP.Open(); + //读取RTK数据循环 + while (rtcmthreadrun) + { + byte[] buffer = new byte[180]; + + while (commNTRIP.BytesToRead > 0) + { + int read = commNTRIP.Read(buffer, 0, Math.Min(buffer.Length, commNTRIP.BytesToRead)); + + if (read > 0) + { + for (int a = 0; a < read; a++) + { + int seenmsg = -1; + // rtcm + if ((seenmsg = rtcm3.Read(buffer[a])) > 0) + { + //rtcmDatas.Add(rtcm3.packet); + //await SendRtcmData(); + //Message.Show($"{DateTime.Now.ToString("HH:mm:ss fff")}---rtcm3.length = {rtcm3.length}"); + await _commModuleManager.InjectGpsRTCMDataAsync(rtcm3.packet, rtcm3.length); + } + } + } + } + await Task.Delay(10); + } + NTRIPbtntxt = "发送RTCM"; + RTKState = "未发送RTK数据"; + Alert.Show("RTCM数据停止发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + }); + } + })); + } + } + + private RtcmInfoView rtcmInfoView; + private ICommand _OpenRtcmManageCommand; + public ICommand OpenRtcmManageCommand + { + get + { + return _OpenRtcmManageCommand ?? (_OpenRtcmManageCommand = new RelayCommand(() => + { + RtcmInfoView.GetInstance().Show(); + RtcmInfoView.GetInstance().Activate(); + // if (rtcmInfoView == null) + // { + // rtcmInfoView = new RtcmInfoView(); + // } + // rtcmInfoView.Show(); + })); + } + } + + /* + rtcm3 rtcm3 = new rtcm3(); + private async Task AnalysisRendRrcmData(byte[] buffer, int length) + { + for (int a = 0; a < length; a++) + { + int seenmsg = -1; + // rtcm + if ((seenmsg = rtcm3.Read(buffer[a])) > 0) + { + await _commModuleManager.InjectGpsRTCMDataAsync(rtcm3.packet, rtcm3.length); + } + } + await Task.Delay(1); + } + */ + + + private ICommand _LandCommand; + public ICommand LandCommand + { + get + { + return _LandCommand ?? (_LandCommand = new RelayCommand(async () => + { + if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0) + await _commModuleManager.LandAsync(_copterManager.AcceptingControlCopters); + /* + await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.LandAsync())); + */ + })); + } + } + + + + + private ICommand _ConnectCommand; + public ICommand ConnectCommand + { + get + { + return _ConnectCommand ?? (_ConnectCommand = new RelayCommand(async () => + { + //await ConnectAsync(); + await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async copter => + { + //await commModule.ConnectAsync(); + await copter.ConnectAsync(); + //await copter.GetCopterDataAsync(); + })); + })); + } + } + + private ICommand _DisconnectCommand; + public ICommand DisconnectCommand + { + get + { + return _DisconnectCommand ?? (_DisconnectCommand = new RelayCommand(async () => + { + await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.DisconnectAsync())); + })); + } + } + + private ICommand _FlyToAltitudeCommand; + public ICommand FlyToAltitudeCommand + { + get + { + return _FlyToAltitudeCommand ?? (_FlyToAltitudeCommand = new RelayCommand(async alt => + { + await _formationController.FlyToAltitudeAsync(_copterManager.AcceptingControlCopters, targetAlt: alt).ConfigureAwait(false); + })); + } + } + + private ICommand _FlyInCircleCommand; + public ICommand FlyInCircleCommand + { + get + { + return _FlyInCircleCommand ?? (_FlyInCircleCommand = new RelayCommand(async () => + { + await _formationController.FlyInCircleAsync(_copterManager.AcceptingControlCopters, centerDirection: 180, radius: 10, loop: true).ConfigureAwait(false); + })); + } + } + + private ICommand _FlyAroundCenterOfCoptersCommand; + public ICommand FlyAroundCenterOfCoptersCommand + { + get + { + return _FlyAroundCenterOfCoptersCommand ?? (_FlyAroundCenterOfCoptersCommand = new RelayCommand(async () => + { + await _formationController.FlyAroundCenterOfCoptersAsync(_copterManager.AcceptingControlCopters).ConfigureAwait(false); + })); + } + } + + private ICommand _FlyInRectangleCommand; + public ICommand FlyInRectangleCommand + { + get + { + return _FlyInRectangleCommand ?? (_FlyInRectangleCommand = new RelayCommand(async () => + { + await _formationController.FlyInRectangleAsync(_copterManager.AcceptingControlCopters, startDirection: 180, sideLength1: 15, sideLength2: 5, loop: true).ConfigureAwait(false); + })); + } + } + + private ICommand _FlyToVerticalLineAndMakeCircleCommand; + public ICommand FlyToVerticalLineAndMakeCircleCommand + { + get + { + return _FlyToVerticalLineAndMakeCircleCommand ?? (_FlyToVerticalLineAndMakeCircleCommand = new RelayCommand(async () => + { + await _formationController.FlyToVerticalLineAndMakeCircle( + _copterManager.AcceptingControlCopters, + centerDirection: 180, + radius: 10 + ).ConfigureAwait(false); + })); + } + } + + private ICommand _MissionStartCommand; + public ICommand MissionStartCommand + { + get + { + return _MissionStartCommand ?? (_MissionStartCommand = new RelayCommand(async () => + { + FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) + { + Alert.Show("作为参照的原点未设置，无法开始任务！", "提示"); + return; + } + //飞机用UTC时间，日志用本地时间记录 + DateTime MissionTime = DateTime.UtcNow.AddSeconds(5); + DateTime MissionTime_log = DateTime.Now.AddSeconds(5); + Message.Show("任务开始:" + MissionTime_log.ToString()); + //循环3次 发送起飞命令 避免通信问题 + for (int i = 0; i < 20; i++) //20 + { + await _commModuleManager.DoMissionStartAsync(null, + MissionTime.Hour, + MissionTime.Minute, + MissionTime.Second, + _flightTaskManager.OriginLng, + _flightTaskManager.OriginLat + ); + + /* + foreach (var vcopter in _copterManager.Copters) + { + await vcopter.MissionStartAsync(utchour, + utcminute, + utcsecond, + _flightTaskManager.OriginLng, + _flightTaskManager.OriginLat + ); + } + */ + + await Task.Delay(20).ConfigureAwait(false); + + } + _copterManager.Net_LogStartMission(MissionTime_log, _flightTaskManager.OriginLng, _flightTaskManager.OriginLat); + Alert.Show("所有飞机开始执行航点任务。请勿多次开始任务！", "提示", MessageBoxButton.OK, MessageBoxImage.Information); + })); + } + } + + private ICommand _MissionPauseCommand; + public ICommand MissionPauseCommand + { + get + { + return _MissionPauseCommand ?? (_MissionPauseCommand = new RelayCommand(async () => + { + for (int i = 0; i < 3; i++) + { + await _commModuleManager.DoMissionPauseAsync(); + } + })); + } + } + + private ICommand _MissionResumeCommand; + public ICommand MissionResumeCommand + { + get + { + return _MissionResumeCommand ?? (_MissionResumeCommand = new RelayCommand(async () => + { + int utchour = DateTime.UtcNow.AddSeconds(5).Hour; + int utcminute = DateTime.UtcNow.AddSeconds(5).Minute; + int utcsecond = DateTime.UtcNow.AddSeconds(5).Second; + for (int i = 0; i < 3; i++) + { + await _commModuleManager.DoMissionResumeAsync( + utchour, + utcminute, + utcsecond); + } + })); + } + } + + + // private ICommand _SetOriginalPointCommand; + // public ICommand SetOriginalPointCommand + // { + // get + // { + // return _SetOriginalPointCommand ?? (_SetOriginalPointCommand = new RelayCommand(() => + // { + // FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + // _flightTaskManager.SetOriginal(); + // })); + // } + // } + private ICommand _WriteMissionSingleCommand; + public ICommand WriteMissionSingleCommand + { + get + { + return _WriteMissionSingleCommand ?? (_WriteMissionSingleCommand = new RelayCommand(async () => + { + var _flightTaskManager = ServiceLocator.Current.GetInstance(); + if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) + { + Alert.Show("作为参照的原点未设置，无法写入相对位置！", "提示"); + return; + } + var _RtcmInfoViewModel = ServiceLocator.Current.GetInstance(); + if (_RtcmInfoViewModel.RtcmManager.Rtcmthreadrun) + { + Alert.Show("RTK数据正在发送，将自动关闭！", "提示"); + await _RtcmInfoViewModel.RtcmManager.Close(_RtcmInfoViewModel.SerialPortsSelectdValue); + } + + + int taskcount = _flightTaskManager.Tasks.Count; + if (taskcount < 2) return; + foreach (var copter in _copterManager.AcceptingControlCopters) + { + int i = _copterManager.Copters.IndexOf(copter); + await CollectMissions(i); + } + })); + } + } + + + private ICommand _WriteMissionFailedCommand; + public ICommand WriteMissionFailedCommand + { + get + { + return _WriteMissionFailedCommand ?? (_WriteMissionFailedCommand = new RelayCommand(async () => + { + var _flightTaskManager = ServiceLocator.Current.GetInstance(); + if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) + { + Alert.Show("作为参照的原点未设置，无法写入相对位置！", "提示"); + return; + } + var _RtcmInfoViewModel = ServiceLocator.Current.GetInstance(); + if (_RtcmInfoViewModel.RtcmManager.Rtcmthreadrun) + { + Alert.Show("RTK数据正在发送，将自动关闭！", "提示"); + await _RtcmInfoViewModel.RtcmManager.Close(_RtcmInfoViewModel.SerialPortsSelectdValue); + } + + int coptercount = _copterManager.Copters.Count; + int taskcount = _flightTaskManager.Tasks.Count; + if (taskcount < 2) return; + foreach(KeyValuePair kv in _commModuleManager.MissionWriteState) + { + if (!kv.Value.SendAchieved || !kv.Value.WriteSucceed) + { + var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == kv.Key.ToString()); + int i = _copterManager.Copters.IndexOf(copter); + await CollectMissions(i); + } + } + })); + } + } + + private ICommand _WriteMissionCommand; + public ICommand WriteMissionCommand + { + get + { + return _WriteMissionCommand ?? (_WriteMissionCommand = new RelayCommand(async () => + { + var _flightTaskManager = ServiceLocator.Current.GetInstance(); + if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) + { + Alert.Show("作为参照的原点未设置，无法写入相对位置！", "提示"); + return; + } + + int coptercount = _copterManager.Copters.Count; + int taskcount = _flightTaskManager.Tasks.Count; + if (taskcount < 2) return; + + var _RtcmInfoViewModel = ServiceLocator.Current.GetInstance(); + if (_RtcmInfoViewModel.RtcmManager.Rtcmthreadrun) + { + Alert.Show("RTK数据正在发送，将自动关闭！", "提示"); + await _RtcmInfoViewModel.RtcmManager.Close(_RtcmInfoViewModel.SerialPortsSelectdValue); + } + + Alert.Show("开始写入航点，飞机绿色成功红色失败！(先关灯再开灯可恢复飞机灯光)", "提示"); + _commModuleManager.ClearMissionWriteState(); + for (int i = 0; i < coptercount; i++) + { + ///写航线开始 + await CollectMissions(i).ConfigureAwait(false); + } + Alert.Show($"任务写入完成!请检测失败的飞机"); + })); + } + } + + /// + /// 用通讯模块 写入航点信息 + /// + /// copterIndex + /// + private async Task CollectMissions(int i) + { + int j = 0; + try + { + + + float groudAlt = _copterManager.Copters[i].GroundAlt; + var missions = new List(); + var _flightTaskManager = ServiceLocator.Current.GetInstance(); + for (j = 0; j < _flightTaskManager.Tasks.Count; j++) + { + switch (_flightTaskManager.Tasks[j].TaskType) + { + case FlightTaskType.TakeOff: + //int takeofftime = (int)(_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetAlt / 2.5 + 5); + missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos)); + missions.Add(Mission.CreateTakeoffMission( + _flightTaskManager.Tasks[j].SingleCopterInfos[i].TakeOffWaitTime, //起飞等待时间 + _flightTaskManager.Tasks[j].TakeOffTime, //起飞时间 + _flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLat - _flightTaskManager.OriginLat, //位置没用 + _flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLng - _flightTaskManager.OriginLng, //位置没用 + _flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetAlt - groudAlt) //下一个航点的高度 + ); + break; + + case FlightTaskType.FlyTo: + if (_flightTaskManager.Tasks[j].SingleCopterInfos[i].IsChangeSpeed) + { + missions.Add(Mission.CreateChangeSpeedMission( + _flightTaskManager.Tasks[j].SingleCopterInfos[i].LevelSpeed, + _flightTaskManager.Tasks[j].SingleCopterInfos[i].UpSpeed, + _flightTaskManager.Tasks[j].SingleCopterInfos[i].DownSpeed) + ); + } + missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos)); + + double Lat = _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLat - _flightTaskManager.OriginLat; + double Lng = _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLng - _flightTaskManager.OriginLng; + //如果是返航点，位置写入固定值，这样直接返回起飞点 + if (_flightTaskManager.Tasks[j].SingleCopterInfos[i].IsLandWaypoint) + { + Lat = 90; + Lng = 180; + } + + missions.Add(Mission.CreateWaypointMission( + _flightTaskManager.Tasks[j].LoiterTime, + _flightTaskManager.Tasks[j].FlytoTime, + Lat, + Lng, + _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt - groudAlt) + ); + break; + + case FlightTaskType.Land: + missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos)); + + missions.Add(Mission.CreateLandMission( + _flightTaskManager.Tasks[j].SingleCopterInfos[i].LandWaitTime) + ); + break; + default: + break; + } + } + //186是通讯模块的限制--张伟那边 + if (missions.Count > 186) + { + Message.Show($"飞机：{_copterManager.Copters[i].VirtualId} 航点数量超过186，请减少再重写该飞机!"); + } + else + { + //虚拟ID在排序时已经变为ID了，直接按ID写入 + // var targetCopter = _copterManager.Copters.FirstOrDefault(o => o.VirtualId == (i+1)); + // bool result = await _commModuleManager.WriteMissionListAsync(targetCopter, missions); + bool result = await _commModuleManager.WriteMissionListAsync(_copterManager.Copters[i], missions); + + //CommWriteMissinState state = new CommWriteMissinState(result); + //_commModuleManager.missionWriteState.Add(int.Parse(_copterManager.Copters[i].Id), state); + if (!result) + { + + if (_copterManager.SortType == CopterManager.CopterSortType.ByVID) + Message.Show($"飞机：{_copterManager.Copters[i].VirtualId} 通信模块传输失败!"); + else + Message.Show($"飞机：{_copterManager.Copters[i].Id} 通信模块传输失败!"); + } + else + { + if (_copterManager.SortType == CopterManager.CopterSortType.ByVID) + Message.Show($"飞机：{_copterManager.Copters[i].VirtualId} 通信模块传输完成!"); + else + Message.Show($"飞机：{_copterManager.Copters[i].Id} 通信模块传输完成!"); + } + await Task.Delay(500).ConfigureAwait(false); + } + } + catch (Exception ex) + { + Message.Show(ex.ToString()); + } + + } + + private List CreateLEDMissions(IEnumerable LEDInfos) + { + List missions = new List(); + foreach (LEDInfo ledInfo in LEDInfos) + { + float ledinterval=0; + Color color = (Color)ColorConverter.ConvertFromString("#" + ledInfo.LEDRGB); + int ledMode = ledInfo.LEDMode; + //拉烟任务模式ID是50，需要改为50 + if (_copterManager.FC_VER_NO == 1) + { + //老版本是闪烁频率，且不能为小数 + ledinterval = ledInfo.LEDInterval; + //老版本ledMode == 8 对应拉烟，飞控对应50,ledinterval对应通道号 + if (ledMode==8) ledMode = 50; + } + else + { + //新版本飞控 是闪烁间隔单位是100ms，地面站还是频率可以有1位小数,用户输入0.5hz 飞控应该是(1/0.5)*10=20 + ledinterval = (1 / ledInfo.LEDInterval); + + //新版本 ledMode=16是抛物，飞控对应50，ledinterval对应10 --固定的 + if (ledMode == 16) + { + ledMode = 50; + ledinterval = 10; + } + else //新版本ledMode == 15 对应拉烟，飞控对应50,ledinterval对应通道号 + if (ledMode == 15) + { + ledMode = 50; + } + } + IMission LEDMission = Mission.CreateLEDControlMission( + (int)(ledInfo.Delay * 10), + ledMode, + ledinterval, + 0, //ledInfo.LEDTimes, + color.R, + color.G, + color.B + ); + missions.Add(LEDMission); + } + return missions; + } + + private ICommand _FlagCommand; + public ICommand FlagCommand + { + get + { + return _FlagCommand ?? (_FlagCommand = new RelayCommand(async () => + { + await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => + { + + var copters = _copterManager.Copters; + int index = copters.IndexOf(copter); + _copterManager.CopterStatus[index] = true; + return Task.Run(()=> { }); + })); + })); + } + } + + private async Task UnlockTestSingleCopterAsync(ICopter SingleCopter) + { + await SingleCopter.UnlockAsync(); + await Task.Delay(25).ConfigureAwait(false); + + for (int i = 0; !SingleCopter.IsUnlocked; i++) + { + //8秒内每1000毫秒尝试解锁一次 + //解锁间隔一定要超过1s否则导致飞控以后无法解锁 + + if (i >320) + return; //无法解锁后面不用执行了 + if (i % (1000 / 25) == 1000 / 25 - 1) + { + await SingleCopter.UnlockAsync(); // 每 1000 毫秒重试一次。 + } + await Task.Delay(25).ConfigureAwait(false); + } + + + await Task.Delay(1000); + if (SingleCopter.IsUnlocked) + { + await SingleCopter.SetShowLEDAsync(true); + await Task.Delay(50); + await SingleCopter.SetShowLEDAsync(true); + } + + + await Task.Delay(5000); + + await SingleCopter.LockAsync(); + await Task.Delay(25).ConfigureAwait(false); + for (int i = 0; SingleCopter.IsUnlocked; i++) + { + //8秒内每1000毫秒尝试解锁一次 + //解锁间隔一定要超过1s否则导致飞控以后无法解锁 + + if (i > 320) + return; //无法解锁后面不用执行了 + if (i % (1000 / 25) == 1000 / 25 - 1) + { + await SingleCopter.LockAsync(); // 每 1000 毫秒重试一次。 + } + await Task.Delay(25).ConfigureAwait(false); + } + + await Task.Delay(1000); + if (!SingleCopter.IsUnlocked) + { + await SingleCopter.SetShowLEDAsync(false); + await Task.Delay(50); + await SingleCopter.SetShowLEDAsync(false); + } + } + + + + private ICommand _TestCommand; + public ICommand TestCommand + { + get + { + return _TestCommand ?? (_TestCommand = new RelayCommand(async () => + { + + + Message.Show("解锁测试--开始"); + int i = 0; + var tasksUnlockTest = new Task[_copterManager.Copters.Count]; + foreach (var vcopter in _copterManager.Copters) + { + tasksUnlockTest[i++] = UnlockTestSingleCopterAsync(vcopter); + //i++; + } + await Task.WhenAll(tasksUnlockTest).ConfigureAwait(false); + Message.Show("解锁测试--完成!"); + return; + + + + + + + + + + + foreach (var vcopter in _copterManager.Copters) + { + await vcopter.UnlockAsync(); + } + await Task.Delay(1000); + + foreach (var vcopter in _copterManager.Copters) + { + if (vcopter.IsUnlocked) + await vcopter.SetShowLEDAsync(true); + } + + await Task.Delay(5000); + + foreach (var vcopter in _copterManager.Copters) + { + await vcopter.LockAsync(); + await vcopter.SetShowLEDAsync(false); + + } + + + return; + + + + + + + + + + + + + var firstCopter = _copterManager.Copters.First(); + + // Test TurnAsync. + //var latLng = GeographyUtils.CalcLatLngSomeMetersAway(firstCopter.Latitude, firstCopter.Longitude, 135, 10); + //await _formationController.TurnAsync(_copterManager.AcceptingControlCopters, latLng.Item1, latLng.Item2); + + // Test FlyToLatLngAsync. + //var copters = _copterManager.AcceptingControlCopters; + //var centerLat = copters.Average(c => c.Latitude); + //var centerLng = copters.Average(c => c.Longitude); + //var latDelta = 10 * GeographyUtils.MetersToLocalLat; + //var lngDelta = 10 * GeographyUtils.GetMetersToLocalLon(copters.First().Latitude); + //var targetLat = centerLat + latDelta; + //var targetLng = centerLng + lngDelta; + //await _formationController.FlyToLatLngAsync(copters, targetLat, targetLng); + + // Test FlyInCircleAsync. + //await _formationController.FlyInCircleAsync(_copterManager.AcceptingControlCopters, centerDirection: 180, radius: 10, loop: false); + + // Test FlyAroundCenterOfCoptersAsync. + //await _formationController.FlyAroundCenterOfCoptersAsync(_copterManager.AcceptingControlCopters); + + // Test FlyToLatLngAsync. + //var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway(firstCopter.Latitude, firstCopter.Longitude, 135, 20); + //await _formationController.FlyToLatLngAsync(firstCopter, targetLatLng.Item1, targetLatLng.Item2); + + //firstCopter.LoiterTurns(1, 10); + + //var radius = await firstCopter.GetParamAsync("CIRCLE_RADIUS"); + + ////await firstCopter.SetParamAsync("CIRCLE_RADIUS", 1500); + ////var param = await firstCopter.GetParamAsync("CIRCLE_RADIUS"); + //// Alert.Show($"半径设置成功啦: {param}"); + + ////await firstCopter.SetParamAsync("CIRCLE_RATE", 30); + //var rate = await firstCopter.GetParamAsync("CIRCLE_RATE"); + + //Alert.Show($"半径: {radius} 角速度: {rate}"); + + //await firstCopter.SetMobileControlAsync(yaw: 90); + + //if (!_listeningMissionItemReceived) + //{ + // firstCopter.MissionItemReceived += (sender, e) => Alert.Show($"{e.Index} {e.Total} {(Protocols.MAVLink.MAV_CMD)e.Details.id}"); + // _listeningMissionItemReceived = true; + //} + //await firstCopter.RequestMissionListAsync(); + + var copter = firstCopter; + + ///* 写航线 */ + //// 任务总数。 + //await copter.SetMissionCountAsync(6).ConfigureAwait(false); + //// 起飞前准备。 + //await copter.WritePreTakeOffMissionAsync().ConfigureAwait(false); + //// 起飞。 + //await copter.WriteTakeOffMissionAsync().ConfigureAwait(false); + //// 不断自增的编号。 + //ushort index = 2; + //// 航点。 + //await copter.WriteWaypointMissionAsync(new EHLocation(copter).AddLatLngAlt(0.001, 0.002, 2), index++).ConfigureAwait(false); + //// 航点。 + //await copter.WriteWaypointMissionAsync(new EHLocation(copter).AddLatLngAlt(0.003, 0.002, 2), index++).ConfigureAwait(false); + //// 返航。 + //await copter.WriteReturnToLaunchMissionAsync(index++).ConfigureAwait(false); + //// 降落。 + //await copter.WriteLandMissionAsync(index++).ConfigureAwait(false); + + ///* 读航线 */ + // 监听“任务项已接收”事件。 + //(copter as EHCopter).MissionItemReceived += (sender, e) => Alert.Show($"{e.Mission.Sequence} {e.Mission.Latitude} {e.Mission.Command}"); + // 请求任务列表。 + var missions = await copter.RequestMissionListAsync(); + // Alert.Show(string.Join(Environment.NewLine, missions.Select(m => $"{m.Sequence}\t{m.Command}\t\t{m.Latitude}"))); + + //ICopter previousCopter = firstCopter; + //foreach (var item in _copterManager.Copters.Where(c => c != firstCopter)) + //{ + // item.Follow(previousCopter, keepYawDifference: false, keepFacingTarget: false); + // previousCopter = item; + //} + + //var missions = new Mission[] + //{ + // Mission.CreateWaypointMission(new EHLocation(firstCopter).AddLatLngAlt(0.0001, 0, 10)), + // Mission.CreateReturnToLaunchMission(), + // Mission.CreateLandMission() + //}; + //var result = await firstCopter.WriteMissionsAsync(missions); + //var result2 = await firstCopter.WriteMissionsAsync(missions); + //Debug.WriteLine($"{result1} {result2}"); + + //var missions = new Mission[] + //{ + // Mission.CreateWaypointMission(23.123, 113.123, 10), + // Mission.CreateWaypointMission(23.123, 113.123, 10), + // Mission.CreateWaypointMission(23.123, 113.123, 10), + // Mission.CreateWaypointMission(23.123, 113.123, 10), + // Mission.CreateWaypointMission(23.123, 113.123, 10), + // Mission.CreateWaypointMission(23.123, 113.123, 10), + // Mission.CreateWaypointMission(23.123, 113.123, 10), + // Mission.CreateWaypointMission(23.123, 113.123, 10), + // Mission.CreateWaypointMission(23.123, 113.123, 10), + // Mission.CreateWaypointMission(23.123, 113.123, 10), + // Mission.CreateReturnToLaunchMission(), + // Mission.CreateLandMission() + //}; + //var result = await firstCopter.WriteMissionListAsync(missions); + //MessageBox.Show($"The result of WriteMissions: {result}"); + + // 跟随测试 + /* + ICopter previousCopter = firstCopter; + foreach (var item in _copterManager.Copters.Where(c => c != firstCopter)) + { + item.Follow(previousCopter, keepYawDifference: false, keepFacingTarget: false); + previousCopter = item; + } + */ + await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async c => + { + + float gpsLed = await c.GetParamAsync("NOTI_GPSLED"); + if (gpsLed == 0.0f) + { + await LEDFlashAsync(c, true); + } + else + { + await LEDFlashAsync(c, false); + } + + // for (int jj = 0; jj < 5; jj++) + { + // await LEDFlashAsync(c, true); + // await c.SetParamAsync("THR_ALT_P", AltP); + // await c.SetParamAsync("WPNAV_ACCEL_Z", AltP); + // await c.SetParamAsync("WPNAV_ACCEL", AltP); + // await c.SetParamAsync("WPNAV_SPEED", AltP); + // await c.SetParamAsync("WPNAV_SPEED_DN", AltP); + // await c.SetParamAsync("WPNAV_SPEED_UP", AltP); + // await LEDFlashAsync(c, false); + // await LEDFlashAsync(c, true); + // await Task.Delay(500).ConfigureAwait(false); + + // await Task.Delay(1000).ConfigureAwait(false); + + // await LEDFlashAsync(c, true); + // await Task.Delay(1000).ConfigureAwait(false); + } + })); + })); + } + } + + /// + /// 读取串口号 + /// + private void LoadRTKcomvalue() + { + IniFiles inifilse = new IniFiles(); + RTKcomvalue = inifilse.IniReadvalue("Default", "RTKcomvalue"); + if (RTKcomvalue == "") RTKcomvalue = "COM6"; + } + + + /// + /// 保存上次成功的串口号 + /// + /// + private async Task SaveRTKcomvalue() + { + IniFiles inifilse = new IniFiles(); + inifilse.IniWritevalue("Default", "RTKcomvalue", RTKcomvalue); + await Task.Delay(1); + } + + private async Task LEDFlashAsync(ICopter copter, bool isOn) + { + // float gpsLed = await c.GetParamAsync("NOTI_GPSLED"); + float ledControl = 0.0f; + if (isOn) + { + ledControl = 1.0f; + } + var tasks = new Task[2]; + //tasks[0] = copter.SetParamAsync("NOTI_GPSLED", ledControl); + //tasks[1] = copter.SetParamAsync("NOTI_ARMLED", ledControl); + tasks[0] = Task.Run(async () => + { + try + { + await copter.SetParamAsync("NOTI_GPSLED", ledControl, 5000); + } + catch (TimeoutException e) + { + _logger.Log($"NOTI_GPSLED 超时, CopterId: {copter.Id}。"); + } + }); + tasks[1] = Task.Run(async () => + { + try + { + await copter.SetParamAsync("NOTI_ARMLED", ledControl, 5000); + } + catch (TimeoutException e) + { + _logger.Log($"NOTI_ARMLED 超时, CopterId: {copter.Id}。"); + } + }); + await Task.WhenAll(tasks).ConfigureAwait(false); + + } + + private bool _listeningMissionItemReceived; + + private ICommand _StopTaskCommand; + public ICommand StopTaskCommand + { + get + { + return _StopTaskCommand ?? (_StopTaskCommand = new RelayCommand(async () => + { + await _formationController.AllStop().ConfigureAwait(false); + })); + } + } + + + } +} diff --git a/Plane.FormationCreator/Views/ControlPanelView.xaml b/Plane.FormationCreator/Views/ControlPanelView.xaml index cf08c2e..0105a49 100644 --- a/Plane.FormationCreator/Views/ControlPanelView.xaml +++ b/Plane.FormationCreator/Views/ControlPanelView.xaml @@ -1,235 +1,238 @@ - - - - - - - - - - - - - - - - -