小航点计算飞行路径,计算4种情况，匀速。加速匀速减速，加速匀速，匀速减速
This commit is contained in:
parent
b289618a87
commit
f8f3f36304
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -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])
|
||||
{
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user