group/Plane.FormationCreator
1
0
Fork 0
Code Issues 3 Pull Requests Actions Packages Projects Releases Wiki Activity

小航点计算飞行路径,计算4种情况，匀速。加速匀速减速，加速匀速，匀速减速

Browse Source
This commit is contained in:
pxzleo 2023-12-04 23:48:27 +08:00
parent b289618a87
commit f8f3f36304
4 changed files with 99 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
Plane.FormationCreator/Formation/FlightTaskManager.cs
View File

@ -509,15 +509,13 @@ namespace Plane.FormationCreator.Formation
//第一个航点，回起飞点 //第一个航点，回起飞点
if (Tasks.Count == 2) if (Tasks.Count == 2)
{ {
SelectedTask.FlytoTime = 2;
SelectedTask.LoiterTime = 2;
foreach (FlightTaskSingleCopterInfo info in SelectedTask.SingleCopterInfos) foreach (FlightTaskSingleCopterInfo info in SelectedTask.SingleCopterInfos)
{ {
info.TargetLat = info.Copter.Latitude; info.TargetLat = info.Copter.Latitude;
info.TargetLng = info.Copter.Longitude; info.TargetLng = info.Copter.Longitude;
} }
// SelectedTask.TaskCnName = "检查航点";
} }
/* /*
@ -1065,7 +1063,7 @@ namespace Plane.FormationCreator.Formation
} }
var lastTask = Tasks.LastOrDefault(); 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; newTask.TaskCnName = taskName;
for (int k = 0; k < PointDic.Count; k++) for (int k = 0; k < PointDic.Count; k++)
{ {
@ -1719,6 +1717,22 @@ namespace Plane.FormationCreator.Formation
return; 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, public void SetTaskFlytime_v2(int taskIndex,
float acc_xy, float acc_z, float acc_xy, float acc_z,
float defvel_xy, float defvel_up, float defvel_down, float defvel_xy, float defvel_up, float defvel_down,
@ -1736,7 +1750,7 @@ namespace Plane.FormationCreator.Formation
string copterName_xy = ""; string copterName_xy = "";
string copterName_up = ""; string copterName_up = "";
string copterName_down = ""; string copterName_down = "";
int flytimetype = getflytype(taskIndex);
foreach (var copter in _copterManager.Copters) foreach (var copter in _copterManager.Copters)
{ {
var prevWaypoint = Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter); 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_xy = getMinfligthtime((float)maxDistance_xy, acc_xy, defvel_xy, flytimetype);
double time_up = getMinfligthtime((float)maxDistance_up, acc_z, defvel_up); double time_up = getMinfligthtime((float)maxDistance_up, acc_z, defvel_up, flytimetype);
double time_down = getMinfligthtime((float)maxDistance_down, acc_z, defvel_down); double time_down = getMinfligthtime((float)maxDistance_down, acc_z, defvel_down, flytimetype);
double tasktime = time_xy; double tasktime = time_xy;
copterName = copterName_xy; copterName = copterName_xy;
string divstr = "水平"; string divstr = "水平";
@ -1803,7 +1817,7 @@ namespace Plane.FormationCreator.Formation
$"最长耗时：{Math.Round(tasktime, 2)}秒,用于{divstr}飞行，编号:{copterName}" $"最长耗时：{Math.Round(tasktime, 2)}秒,用于{divstr}飞行，编号:{copterName}"
); );
if (settime) 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_xy = "";
string copterName_up = ""; string copterName_up = "";
string copterName_down = ""; string copterName_down = "";
int flytimetype = getflytype(taskIndex);
foreach (var copter in _copterManager.Copters) foreach (var copter in _copterManager.Copters)
{ {
var prevWaypoint = Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter); 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_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); 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); double time_down = getMinfligthtime((float)maxDistance_down, FC_acc_z, FC_maxvel_down, flytimetype);
double tasktime = time_xy; double tasktime = time_xy;
copterName = copterName_xy; copterName = copterName_xy;
string divstr = "水平"; 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) if (Alert.Show($"任务{taskIndex + 1}时间过短，目前{Tasks[taskIndex].FlytoTime}秒至少{Math.Round(tasktime, 2)}秒才能全部飞到目标，需要更改吗？", "提示", MessageBoxButton.OKCancel, MessageBoxImage.Warning)
== MessageBoxResult.OK) == MessageBoxResult.OK)
{ {
Message.Show($"任务{taskIndex + 1}飞行时间{Tasks[taskIndex].FlytoTime}秒更改为{Math.Round(tasktime, 0)}秒"); Message.Show($"任务{taskIndex + 1}飞行时间{Tasks[taskIndex].FlytoTime}秒更改为{Math.Ceiling(tasktime)}秒");
Tasks[taskIndex].FlytoTime = (int)Math.Round(tasktime, 0); 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; if ((Distance == 0) || (fc_acc == 0) || (fc_maxspeed == 0)) return 0;
float fDis = fabsf(Distance); float fDis = fabsf(Distance);
@ -1978,6 +2028,22 @@ namespace Plane.FormationCreator.Formation
return realflytime; 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) public static double CalculateFlyIime(double s, double v)
{ {
double t; double t;
@ -2665,6 +2731,14 @@ namespace Plane.FormationCreator.Formation
{ {
Alert.Show($"模拟任务需要全部是模拟飞机，编号：{copter.Id } 不是模拟飞机，请检查"); Alert.Show($"模拟任务需要全部是模拟飞机，编号：{copter.Id } 不是模拟飞机，请检查");
return ; 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;
} }
} }

27
Plane.FormationCreator/Formation/FlightTask_FlyTo.cs
View File

@ -144,15 +144,6 @@ namespace Plane.FormationCreator.Formation
await info.Copter.GuidAsync(); 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 targetLat = info.TargetLat;
double targetLng = info.TargetLng; double targetLng = info.TargetLng;
if (info.IsLandWaypoint) if (info.IsLandWaypoint)
@ -160,9 +151,8 @@ namespace Plane.FormationCreator.Formation
targetLat = info.Copter.TakeOffPoint.Latitude; targetLat = info.Copter.TakeOffPoint.Latitude;
targetLng = info.Copter.TakeOffPoint.Longitude; targetLng = info.Copter.TakeOffPoint.Longitude;
} }
//发送目标航点1次 int flytype = _flightTaskManager.getflytype(taskIndex);
await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt, task.FlytoTime, flytype);
await info.Copter.FlyToAsync(targetLat, targetLng, info.TargetAlt, task.FlytoTime);
// await Task.Delay(10).ConfigureAwait(false); // await Task.Delay(10).ConfigureAwait(false);
dtNow = DateTime.Now; 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]) if ((bool)_copterManager.CopterStatus[copterIndex])
{ {

4
Plane.FormationCreator/Formation/FlightTask_TakeOff.cs
View File

@ -110,9 +110,11 @@ namespace Plane.FormationCreator.Formation
float takeOffAlt = copterNextTask.TargetAlt; float takeOffAlt = copterNextTask.TargetAlt;
info.TargetLat = info.Copter.Latitude; info.TargetLat = info.Copter.Latitude;
info.TargetLng = info.Copter.Longitude; info.TargetLng = info.Copter.Longitude;
//info.Copter.takeOffTargetAltitude = takeOffAlt;
FlightTask task = _flightTaskManager.CurrentRunningTask; 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; info.Copter.LEDColor = CopterManager.CopterTakeoffColor;

23
Plane.FormationCreator/Views/MapView.xaml.cs
View File

@ -140,29 +140,6 @@ namespace Plane.FormationCreator.Views
gmap.MouseDoubleClick += async (sender, e) => 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; var center = _appConfig.Center;