diff --git a/Plane.FormationCreator/Formation/FlightTaskManager.cs b/Plane.FormationCreator/Formation/FlightTaskManager.cs index a5a7280..d0eae8d 100644 --- a/Plane.FormationCreator/Formation/FlightTaskManager.cs +++ b/Plane.FormationCreator/Formation/FlightTaskManager.cs @@ -508,16 +508,14 @@ namespace Plane.FormationCreator.Formation //第一个航点，回起飞点 if (Tasks.Count == 2) - { + { + SelectedTask.FlytoTime = 2; + SelectedTask.LoiterTime = 2; foreach (FlightTaskSingleCopterInfo info in SelectedTask.SingleCopterInfos) { info.TargetLat = info.Copter.Latitude; info.TargetLng = info.Copter.Longitude; } - // SelectedTask.TaskCnName = "检查航点"; - - - } /* @@ -1065,7 +1063,7 @@ namespace Plane.FormationCreator.Formation } var lastTask = Tasks.LastOrDefault(); - var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = true, FlytoTime = flytime, LoiterTime = 1 }; + var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = true, FlytoTime = flytime, LoiterTime = 0 }; newTask.TaskCnName = taskName; for (int k = 0; k < PointDic.Count; k++) { @@ -1719,6 +1717,22 @@ namespace Plane.FormationCreator.Formation return; } + + + public int getflytype(int taskIndex) + { + bool addspeedmk = false; //是否要加速 + FlightTask prevTask = Tasks[taskIndex - 1]; + addspeedmk = prevTask.TaskType == FlightTaskType.FlyTo && prevTask.LoiterTime > 0; + bool decspeedmk = (Tasks[taskIndex].LoiterTime > 0); //是否要减速 + + int flytype = 0;//匀速 + if (addspeedmk & decspeedmk) + flytype = 1; //加减速 + else if (addspeedmk) flytype = 2; //单加速 + else if (decspeedmk) flytype = 3; //单减速 + return flytype; + } public void SetTaskFlytime_v2(int taskIndex, float acc_xy, float acc_z, float defvel_xy, float defvel_up, float defvel_down, @@ -1736,7 +1750,7 @@ namespace Plane.FormationCreator.Formation string copterName_xy = ""; string copterName_up = ""; string copterName_down = ""; - + int flytimetype = getflytype(taskIndex); foreach (var copter in _copterManager.Copters) { var prevWaypoint = Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter); @@ -1778,9 +1792,9 @@ namespace Plane.FormationCreator.Formation } }//循环结束 - double time_xy = getMinfligthtime((float)maxDistance_xy, acc_xy, defvel_xy); - double time_up = getMinfligthtime((float)maxDistance_up, acc_z, defvel_up); - double time_down = getMinfligthtime((float)maxDistance_down, acc_z, defvel_down); + double time_xy = getMinfligthtime((float)maxDistance_xy, acc_xy, defvel_xy, flytimetype); + double time_up = getMinfligthtime((float)maxDistance_up, acc_z, defvel_up, flytimetype); + double time_down = getMinfligthtime((float)maxDistance_down, acc_z, defvel_down, flytimetype); double tasktime = time_xy; copterName = copterName_xy; string divstr = "水平"; @@ -1803,7 +1817,7 @@ namespace Plane.FormationCreator.Formation $"最长耗时：{Math.Round(tasktime, 2)}秒,用于{divstr}飞行，编号:{copterName}" ); if (settime) - Tasks[taskIndex].FlytoTime = (int)Math.Round(tasktime, 0); + Tasks[taskIndex].FlytoTime = (int)(int)Math.Ceiling(tasktime); } } @@ -1852,7 +1866,7 @@ namespace Plane.FormationCreator.Formation string copterName_xy = ""; string copterName_up = ""; string copterName_down = ""; - + int flytimetype = getflytype(taskIndex); foreach (var copter in _copterManager.Copters) { var prevWaypoint = Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter); @@ -1894,9 +1908,9 @@ namespace Plane.FormationCreator.Formation } }//循环结束 - double time_xy = getMinfligthtime((float)maxDistance_xy, FC_acc_xy, FC_maxvel_xy); - double time_up = getMinfligthtime((float)maxDistance_up, FC_acc_z, FC_maxvel_up); - double time_down = getMinfligthtime((float)maxDistance_down, FC_acc_z, FC_maxvel_down); + double time_xy = getMinfligthtime((float)maxDistance_xy, FC_acc_xy, FC_maxvel_xy,flytimetype); + double time_up = getMinfligthtime((float)maxDistance_up, FC_acc_z, FC_maxvel_up, flytimetype); + double time_down = getMinfligthtime((float)maxDistance_down, FC_acc_z, FC_maxvel_down, flytimetype); double tasktime = time_xy; copterName = copterName_xy; string divstr = "水平"; @@ -1924,8 +1938,8 @@ namespace Plane.FormationCreator.Formation if (Alert.Show($"任务{taskIndex + 1}时间过短，目前{Tasks[taskIndex].FlytoTime}秒至少{Math.Round(tasktime, 2)}秒才能全部飞到目标，需要更改吗？", "提示", MessageBoxButton.OKCancel, MessageBoxImage.Warning) == MessageBoxResult.OK) { - Message.Show($"任务{taskIndex + 1}飞行时间{Tasks[taskIndex].FlytoTime}秒更改为{Math.Round(tasktime, 0)}秒"); - Tasks[taskIndex].FlytoTime = (int)Math.Round(tasktime, 0); + Message.Show($"任务{taskIndex + 1}飞行时间{Tasks[taskIndex].FlytoTime}秒更改为{Math.Ceiling(tasktime)}秒"); + Tasks[taskIndex].FlytoTime = (int)Math.Ceiling(tasktime); //上取整 } @@ -1951,7 +1965,43 @@ namespace Plane.FormationCreator.Formation } - float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed) + //单算加速或减速和匀速部分的最小飞行时间 + float getMinfligthtime_acc(float Distance, float fc_acc, float fc_maxspeed) + { + float fDis = fabsf(Distance); // 总距离 + float facc = fabsf(fc_acc); // 减速度 + + // 物体开始时即以最大速度运动，不考虑加速过程 + float vel = fc_maxspeed; + + // 计算减速所需的时间和距离 + // 减速时间 (从最大速度减速到0) + float dectime = vel / facc; + // 减速阶段覆盖的距离 + float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime); + + // 判断是否有足够的距离进行减速 + if (decdis >= fDis) + { + // 如果减速所需距离已经超过或等于总距离，调整最大速度 + // 使用公式 v^2 = u^2 + 2as 解出 v + vel = (float)Math.Sqrt(2 * facc * fDis); + // 重新计算减速时间 + dectime = vel / facc; + } + + // 计算匀速阶段时间 + float unftime = 0.0f; + if (decdis < fDis) + { + // 如果有剩余距离进行匀速运动 + unftime = (fDis - decdis) / vel; + } + + // 总飞行时间 = 匀速阶段时间 + 减速阶段时间 + return unftime + dectime; + } + float getMinfligthtime_acc_dec(float Distance, float fc_acc, float fc_maxspeed) { if ((Distance == 0) || (fc_acc == 0) || (fc_maxspeed == 0)) return 0; float fDis = fabsf(Distance); @@ -1978,6 +2028,22 @@ namespace Plane.FormationCreator.Formation return realflytime; } + float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed,int timetype) + { + switch (timetype) + { + case 0://匀速 + return Distance / fc_maxspeed; + case 1: //同时计算加减速 + return getMinfligthtime_acc_dec(Distance, fc_acc, fc_maxspeed); + case 2: //计算加速 + case 3://计算减速 + return getMinfligthtime_acc(Distance, fc_acc, fc_maxspeed); + default: + return 0; + } + } + public static double CalculateFlyIime(double s, double v) { double t; @@ -2665,7 +2731,15 @@ namespace Plane.FormationCreator.Formation { Alert.Show($"模拟任务需要全部是模拟飞机，编号：{copter.Id } 不是模拟飞机，请检查"); return ; - } + }else + { + //设置模拟飞机的飞行速度加速度等参数，用于计算位置 + copter.maxspeed_down = FC_maxvel_down; + copter.maxspeed_up = FC_maxvel_up; + copter.maxspeed_xy = FC_maxvel_xy; + copter.acc_z =FC_acc_z; + copter.acc_xy = FC_acc_xy; + } } diff --git a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs index d683e01..48c2770 100644 --- a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs +++ b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs @@ -144,15 +144,6 @@ namespace Plane.FormationCreator.Formation await info.Copter.GuidAsync(); - - - /* - for (int i = 0; i < 5; i++) - { - await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt); - await Task.Delay(10).ConfigureAwait(false); - } - */ double targetLat = info.TargetLat; double targetLng = info.TargetLng; if (info.IsLandWaypoint) @@ -160,9 +151,8 @@ namespace Plane.FormationCreator.Formation targetLat = info.Copter.TakeOffPoint.Latitude; targetLng = info.Copter.TakeOffPoint.Longitude; } - //发送目标航点1次 - - await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt, task.FlytoTime); + 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; @@ -237,19 +227,6 @@ namespace Plane.FormationCreator.Formation } - /* - if (ts.TotalMilliseconds / 10 > sendFlyToTimes) // 每500ms发送一遍指点坐标 - { - sendFlyToTimes++; - for (int i = 0; i < 2; i++) - { - await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt); - await Task.Delay(10).ConfigureAwait(false); - } - - //dtLastTime = dtNow; - } - */ // 当该飞机被标记时，悬停并跳过飞行任务 if ((bool)_copterManager.CopterStatus[copterIndex]) { diff --git a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs index a9b9ec9..a536192 100644 --- a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs +++ b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs @@ -110,9 +110,11 @@ namespace Plane.FormationCreator.Formation 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, task.TakeOffTime- info.TakeOffWaitTime); //秒 + await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt, takeflytime, 1); //秒 //解锁起飞用暗紫色 info.Copter.LEDColor = CopterManager.CopterTakeoffColor; diff --git a/Plane.FormationCreator/Views/MapView.xaml.cs b/Plane.FormationCreator/Views/MapView.xaml.cs index 2e20f30..46dcfbd 100644 --- a/Plane.FormationCreator/Views/MapView.xaml.cs +++ b/Plane.FormationCreator/Views/MapView.xaml.cs @@ -140,30 +140,7 @@ namespace Plane.FormationCreator.Views gmap.MouseDoubleClick += async (sender, e) => { - e.Handled = true; - - //// Test - //var pos = map.ViewportPointToLocation(e.GetPosition(map)); - //AddOrMoveCopter(new FakeCopter(pos.Latitude, pos.Longitude)); - - // 王海, 20150930, AppMode 存在意义已不大；只有正在运行任务的时候才不允许指点。 - //if (AppEx.Current.AppMode != AppMode.ControllingCopters) return; - if (_flightTaskManager.IsPaused == false) return; - - var copters = _copterManager.AcceptingControlCopters; - - if (!copters.Any()) return; - - Point clickPoint = e.GetPosition(gmap); - var pos = gmap.FromLocalToLatLng((int)clickPoint.X, (int)clickPoint.Y); - var centerLat = copters.Average(c => c.Latitude); - var centerLng = copters.Average(c => c.Longitude); - var latDelta = pos.Lat - centerLat; - var lngDelta = pos.Lng - centerLng; - - - await Task.WhenAll(copters.Select(copter => copter.FlyToAsync(copter.Latitude + latDelta, copter.Longitude + lngDelta, copter.Altitude))); - }; + }; var center = _appConfig.Center; gmap.Position = new LatLng(center.Lat, center.Lng).ToGCJ02();