将Flyto切换到清理前版本
航点到达距离改为1米
This commit is contained in:
parent
625bd3ade9
commit
69d36723d6
@ -13,7 +13,7 @@ namespace Plane.FormationCreator.Formation
|
||||
static class Extensions
|
||||
{
|
||||
public const float GpsArrivedDis = 2.0f; //GPS模式下航点到达精度
|
||||
public const float RTKArrivedDis = 1.5f; //RTK模式航点达到精度
|
||||
public const float RTKArrivedDis = 1.0f; //RTK模式航点达到精度
|
||||
public const float GpsCloseDis = 2.0f; //GPS模式下碰撞检测最近距离
|
||||
public const float RTKClosedDis = 1.0f; //RTK模式下碰撞检测最近距离
|
||||
|
||||
|
||||
@ -11,8 +11,7 @@ namespace Plane.FormationCreator.Formation
|
||||
{
|
||||
public partial class FlightTask
|
||||
{
|
||||
//默认不开这个功能
|
||||
private bool _StaggerRoutes = false;
|
||||
private bool _StaggerRoutes = true;
|
||||
|
||||
//是否有交错
|
||||
public bool StaggerRoutes
|
||||
@ -46,6 +45,473 @@ namespace Plane.FormationCreator.Formation
|
||||
}
|
||||
}
|
||||
|
||||
public async Task RunFollowFourLeadersTaskAsync() // 跟随前面飞机
|
||||
{
|
||||
int[] followHeadsIndex = { 0, 0, 0, 0};
|
||||
int followHeadsNum = 0;
|
||||
int startTaskIndex = 0;
|
||||
int endTaskIndex = 0;
|
||||
|
||||
int taskIndex = _flightTaskManager.CurrentRunningTaskIndex;
|
||||
if (taskIndex >= 2 && taskIndex <= 4)
|
||||
{
|
||||
followHeadsNum = 1;
|
||||
followHeadsIndex[0] = 0;
|
||||
followHeadsIndex[1] = 4;
|
||||
followHeadsIndex[2] = 8;
|
||||
// followHeadsIndex[3] = 6;
|
||||
|
||||
startTaskIndex = 2;
|
||||
endTaskIndex = 4;
|
||||
}
|
||||
|
||||
if (taskIndex >= 18 && taskIndex <= 20)
|
||||
{
|
||||
followHeadsNum = 3;
|
||||
followHeadsIndex[0] = 3;
|
||||
followHeadsIndex[1] = 7;
|
||||
followHeadsIndex[2] = 11;
|
||||
// followHeadsIndex[3] = 6;
|
||||
|
||||
startTaskIndex = 18;
|
||||
endTaskIndex = 20;
|
||||
}
|
||||
|
||||
/*
|
||||
if (taskIndex >= 6 && taskIndex <= 6)
|
||||
{
|
||||
followHeadsNum = 2;
|
||||
followHeadsIndex[0] = 0;
|
||||
followHeadsIndex[1] = 5;
|
||||
|
||||
startTaskIndex = 6;
|
||||
endTaskIndex = 6;
|
||||
}
|
||||
*/
|
||||
|
||||
if (StaggerRoutes)
|
||||
{
|
||||
var infos = SingleCopterInfos;
|
||||
if (_flightTaskManager.Tasks.IndexOf(this) == startTaskIndex)
|
||||
{
|
||||
var tasks = new Task[infos.Count];
|
||||
for (int i = 0; i < infos.Count; i++)
|
||||
{
|
||||
var info = infos[i];
|
||||
var copter = info.Copter;
|
||||
var i1 = i;
|
||||
tasks[i1] = await Task.Factory.StartNew(async () =>
|
||||
{
|
||||
await FollowFourLeadersTaskFlySingleCopterAsync(info);
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
if (startTaskIndex == endTaskIndex)
|
||||
{
|
||||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < followHeadsNum; i++)
|
||||
{
|
||||
await tasks[followHeadsIndex[i]];
|
||||
}
|
||||
}
|
||||
|
||||
// await Task.WhenAll(tasks[0]).ConfigureAwait(false);
|
||||
}
|
||||
else if ((_flightTaskManager.Tasks.IndexOf(this) > startTaskIndex) && (_flightTaskManager.Tasks.IndexOf(this) < endTaskIndex))
|
||||
{
|
||||
var tasks = new Task[followHeadsNum];
|
||||
for (int i = 0; i < followHeadsNum; i++)
|
||||
{
|
||||
var info = infos[followHeadsIndex[i]];
|
||||
var copter = info.Copter;
|
||||
var i1 = i;
|
||||
tasks[i1] = await Task.Factory.StartNew(async () =>
|
||||
{
|
||||
await FollowFourLeadersTaskFlySingleCopterAsync(info);
|
||||
});
|
||||
|
||||
}
|
||||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
||||
}
|
||||
else
|
||||
//(_flightTaskManager.Tasks.IndexOf(this) == endTaskIndex)
|
||||
{
|
||||
var tasks = new Task[followHeadsNum];
|
||||
for (int i = 0; i < followHeadsNum; i++)
|
||||
{
|
||||
var info = infos[followHeadsIndex[i]];
|
||||
var copter = info.Copter;
|
||||
var i1 = i;
|
||||
tasks[i1] = await Task.Factory.StartNew(async () =>
|
||||
{
|
||||
await FollowFourLeadersTaskFlySingleCopterAsync(info);
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
||||
while (true)
|
||||
{
|
||||
bool isFinishedFlag = true;
|
||||
for (int i=0; i<infos.Count(); i++)
|
||||
{
|
||||
isFinishedFlag = isFinishedFlag && _flightTaskManager.Tasks[startTaskIndex].SingleCopterInfos[i].isFinished;
|
||||
}
|
||||
if (isFinishedFlag)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
await Task.WhenAll(
|
||||
SingleCopterInfos.Select(info => FollowFourLeadersTaskFlySingleCopterAsync(info))
|
||||
).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
|
||||
private async Task FollowFourLeadersTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
|
||||
{
|
||||
var infos = SingleCopterInfos; // 本次任务所有飞机编号
|
||||
int index = infos.FindIndex(s => s == info); // 本次任务当前飞机编号
|
||||
|
||||
int followHead = 0;
|
||||
int indexHead = 0;
|
||||
|
||||
int startTaskIndex = 0;
|
||||
int endTaskIndex = _flightTaskManager.Tasks.Count();
|
||||
float directionAngle = 0;
|
||||
|
||||
int taskIndex = _flightTaskManager.CurrentRunningTaskIndex;
|
||||
// 如果当前任务编号属于跟随过程的任务
|
||||
if (taskIndex >= 2 && taskIndex <= 4)
|
||||
{
|
||||
if (index == 0 ) // 当前飞机编号属于领航的飞机
|
||||
{
|
||||
followHead = 1;
|
||||
}
|
||||
else // 当前飞机编号属于跟随的飞机
|
||||
{
|
||||
indexHead = index - 1; // 计算跟随的前方飞机编号
|
||||
startTaskIndex = 2; // 本次跟随过程开始任务编号
|
||||
endTaskIndex = 4; // 本次跟随过程结束任务编号
|
||||
info.isFinished = false;// 赋值结束本次task的初始标志位
|
||||
directionAngle = 0; // 与前方跟随飞机的角度，0为正北方向
|
||||
}
|
||||
}
|
||||
|
||||
if (taskIndex >= 18 && taskIndex <= 20)
|
||||
{
|
||||
if (index == 3 || index == 7 || index == 11)
|
||||
{
|
||||
followHead = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
indexHead = index + 1;
|
||||
startTaskIndex = 18;
|
||||
endTaskIndex = 20;
|
||||
info.isFinished = false;
|
||||
directionAngle = 180;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
if (taskIndex >= 6 && taskIndex <= 6)
|
||||
{
|
||||
if (index == 0 || index == 5)
|
||||
{
|
||||
followHead = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (index % 2 == 0)
|
||||
{
|
||||
indexHead = index - 2;
|
||||
directionAngle = 90;
|
||||
}
|
||||
else
|
||||
{
|
||||
indexHead = index + 2;
|
||||
directionAngle = 270;
|
||||
}
|
||||
|
||||
startTaskIndex = 6;
|
||||
endTaskIndex = 6;
|
||||
info.isFinished = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (taskIndex >= 18 && taskIndex <= 20)
|
||||
{
|
||||
if (index == 1 || index == 3 || index == 5 || index == 7)
|
||||
{
|
||||
followHead = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
indexHead = index + 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (taskIndex >= 7 && taskIndex <= 9)
|
||||
{
|
||||
if (index == 0 || index == 7 )
|
||||
{
|
||||
followHead = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
indexHead = index - 2;
|
||||
}
|
||||
}
|
||||
|
||||
if (taskIndex >= 10 && taskIndex <= 12)
|
||||
{
|
||||
if (index == 1 || index == 6)
|
||||
{
|
||||
followHead = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
indexHead = index + 2;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
// 如果不是被跟随的飞机
|
||||
if (followHead != 1)
|
||||
{
|
||||
var infoBefore = infos[indexHead]; // 跟随的飞机编号
|
||||
DateTime dtNow = DateTime.Now; // 记录当前时间
|
||||
DateTime dtLastTime = DateTime.Now; // 记录gps上次更新时间
|
||||
TimeSpan ts = dtNow - dtLastTime; // 记录gps是否更新时间段
|
||||
int flagRefresh = 1; // gps更新标志
|
||||
int flagStopTask = 0; // 如果飞机超过5s没有更新gps坐标，则认为本次跟随任务结束, 即flagStopTask>=10
|
||||
double GPSRate = 500; // 500 ms, gps更新时间
|
||||
|
||||
Tuple<double, double> targetLatLng = null; // 更新的gps坐标
|
||||
|
||||
// int taskIndex = 0;
|
||||
int stopFlag = 10; // 结束任务标志, 10即10*500ms = 5s
|
||||
|
||||
// while (flagStopTask < stopFlag)
|
||||
while (true)
|
||||
{
|
||||
taskIndex = _flightTaskManager.CurrentRunningTaskIndex; // 获取当前任务编号
|
||||
|
||||
if (flagRefresh == 1) // 需要更新gps坐标
|
||||
{
|
||||
flagRefresh = 0; // 更新标志清零
|
||||
flagStopTask++; // 结束任务标志加1
|
||||
|
||||
// var direction = GeographyUtils.CalcDirection(info.Copter.Latitude, info.Copter.Longitude, infoBefore.Copter.Latitude, infoBefore.Copter.Longitude);
|
||||
// targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway(infoBefore.Copter.Latitude, infoBefore.Copter.Longitude, (float)( direction), 5);
|
||||
|
||||
// 计算目标经纬度
|
||||
targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(infoBefore.Copter.Latitude, infoBefore.Copter.Longitude, directionAngle, 8);
|
||||
|
||||
// 判断飞机当前是否在目标范围内. 球半径1m
|
||||
if (!info.Copter.ArrivedTarget(targetLatLng.Item1, targetLatLng.Item2, infoBefore.Copter.Altitude))
|
||||
{
|
||||
flagStopTask = 0;
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
// 发送目标坐标给飞控
|
||||
await info.Copter.FlyToAsync(targetLatLng.Item1, targetLatLng.Item2, infoBefore.Copter.Altitude);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 如果没有在目标范围内，则一直在循环内。当更新gps标志置位，退出循环
|
||||
// 修改到达目标半径为1m，与飞控的判断条件相同, by ZJF
|
||||
while (!info.Copter.ArrivedTarget(targetLatLng.Item1, targetLatLng.Item2, infoBefore.Copter.Altitude))
|
||||
{
|
||||
if (_flightTaskManager.IsPaused == true)
|
||||
{
|
||||
await info.Copter.HoverAsync();
|
||||
return;
|
||||
}
|
||||
await Task.Delay(25).ConfigureAwait(false);
|
||||
|
||||
dtNow = DateTime.Now;
|
||||
ts = dtNow - dtLastTime;
|
||||
if (ts.TotalMilliseconds > GPSRate)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 如果1、已经是最后一步任务；2、结束任务标志位置位；3、跟随的飞机已经结束任务，则本飞机结束当前任务，且切换到悬停模式
|
||||
if ((taskIndex == _flightTaskManager.Tasks.Count() - 1) && (flagStopTask >= stopFlag) && infoBefore.isFinished)
|
||||
// if ((taskIndex == _flightTaskManager.Tasks.Count() - 1) && (flagStopTask >= stopFlag) && _flightTaskManager.Tasks[taskIndex].SingleCopterInfos[indexHead].isFinished)
|
||||
{
|
||||
info.isFinished = true;
|
||||
await info.Copter.HoverAsync();
|
||||
// return;
|
||||
}
|
||||
|
||||
// 如果1、已经是本次跟随过程的最后一步任务；2、结束任务标志位置位；3、跟随的飞机已经结束任务，则本飞机结束当前任务，退出循环
|
||||
if ((taskIndex == endTaskIndex) && (flagStopTask >= stopFlag) && infoBefore.isFinished)
|
||||
// if ((taskIndex == endTaskIndex) && (flagStopTask >= stopFlag) && _flightTaskManager.Tasks[taskIndex].SingleCopterInfos[indexHead].isFinished)
|
||||
{
|
||||
info.isFinished = true;
|
||||
break;
|
||||
// return;
|
||||
}
|
||||
|
||||
// 如果任务暂停，则切换到悬停模式
|
||||
if (_flightTaskManager.IsPaused == true)
|
||||
{
|
||||
await info.Copter.HoverAsync();
|
||||
return;
|
||||
}
|
||||
await Task.Delay(20).ConfigureAwait(false);
|
||||
|
||||
// 计算是否更新gps标志位
|
||||
dtNow = DateTime.Now;
|
||||
ts = dtNow - dtLastTime;
|
||||
if (ts.TotalMilliseconds > GPSRate)
|
||||
{
|
||||
flagRefresh = 1;
|
||||
dtLastTime = dtNow;
|
||||
}
|
||||
}
|
||||
}
|
||||
else // 如果是被跟随的飞机
|
||||
{
|
||||
DateTime dtNow = DateTime.Now;
|
||||
DateTime dtLastTime = DateTime.Now;
|
||||
TimeSpan ts = dtNow - dtLastTime;
|
||||
|
||||
taskIndex = _flightTaskManager.CurrentRunningTaskIndex;
|
||||
for (int j = 0; j < 3; j++)
|
||||
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
|
||||
while (!info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, info.TargetAlt))
|
||||
{
|
||||
if (_flightTaskManager.IsPaused == true)
|
||||
{
|
||||
await info.Copter.HoverAsync();
|
||||
return;
|
||||
}
|
||||
await Task.Delay(25).ConfigureAwait(false);
|
||||
|
||||
// 为了防止3次发送更新gps坐标失败，此处添加每隔1s重新发送目标gps坐标
|
||||
dtNow = DateTime.Now;
|
||||
ts = dtNow - dtLastTime;
|
||||
if (ts.TotalMilliseconds > 1000)
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
dtLastTime = dtNow;
|
||||
}
|
||||
}
|
||||
|
||||
info.isFinished = true;
|
||||
if (taskIndex == _flightTaskManager.Tasks.Count() - 1)
|
||||
{
|
||||
await info.Copter.HoverAsync();
|
||||
}
|
||||
// await info.Copter.HoverAsync();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public async Task RunFlyToOnlyAltTaskAsync() // 仅改变高度
|
||||
{
|
||||
if (StaggerRoutes)
|
||||
{
|
||||
var infos = SingleCopterInfos;
|
||||
var tasks = new Task[infos.Count];
|
||||
for (int i = 0; i < infos.Count; i++)
|
||||
{
|
||||
var info = infos[i];
|
||||
var copter = info.Copter;
|
||||
var i1 = i;
|
||||
tasks[i1] = await Task.Factory.StartNew(async () =>
|
||||
{
|
||||
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 FlyToOnlyAltTaskFlySingleCopterAsync(info);
|
||||
});
|
||||
}
|
||||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
||||
}
|
||||
else
|
||||
{
|
||||
await Task.WhenAll(
|
||||
SingleCopterInfos.Select(info => FlyToOnlyAltTaskFlySingleCopterAsync(info))
|
||||
).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
|
||||
private async Task FlyToOnlyAltTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) // 仅高度变化
|
||||
{
|
||||
int taskIndex = _flightTaskManager.CurrentRunningTaskIndex;
|
||||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
||||
double infoTargetLat = 0.0;
|
||||
double infoTargetLng = 0.0;
|
||||
|
||||
//if (taskIndex > 0 && taskIndex <= 2)
|
||||
//{
|
||||
// infoTargetLat = _flightTaskManager.Tasks[0].SingleCopterInfos[copterIndex].TargetLat;
|
||||
// infoTargetLng = _flightTaskManager.Tasks[0].SingleCopterInfos[copterIndex].TargetLng;
|
||||
//}
|
||||
|
||||
DateTime dtNow = DateTime.Now;
|
||||
DateTime dtLastTime = DateTime.Now;
|
||||
TimeSpan ts = dtNow - dtLastTime;
|
||||
|
||||
infoTargetLat = info.Copter.Latitude;
|
||||
infoTargetLng = info.Copter.Longitude;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
await info.Copter.FlyToAsync(infoTargetLat, infoTargetLng, info.TargetAlt);
|
||||
}
|
||||
|
||||
while (!info.Copter.ArrivedTarget(infoTargetLat, infoTargetLng, info.TargetAlt))
|
||||
{
|
||||
if (_flightTaskManager.IsPaused == true)
|
||||
{
|
||||
await info.Copter.HoverAsync();
|
||||
return;
|
||||
}
|
||||
await Task.Delay(25).ConfigureAwait(false);
|
||||
|
||||
dtNow = DateTime.Now;
|
||||
ts = dtNow - dtLastTime;
|
||||
if (ts.TotalMilliseconds > 2000)
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
await info.Copter.FlyToAsync(infoTargetLat, infoTargetLng, info.TargetAlt);
|
||||
}
|
||||
dtLastTime = dtNow;
|
||||
}
|
||||
}
|
||||
// await info.Copter.HoverAsync();
|
||||
if (taskIndex == _flightTaskManager.Tasks.Count() - 1)
|
||||
{
|
||||
await info.Copter.HoverAsync();
|
||||
}
|
||||
}
|
||||
|
||||
public async Task RunFlyToTaskAsync() // 全部飞到指定航点
|
||||
{
|
||||
@ -56,36 +522,23 @@ namespace Plane.FormationCreator.Formation
|
||||
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);
|
||||
});
|
||||
|
||||
/*
|
||||
//检测交叉可能导致整个编队停主无法继续组队，前面飞机没到位后面就没法飞
|
||||
//如果后面要加，需要多测试解决前序飞机可靠到达和记录日志确认是这里导致任务暂定
|
||||
var info = infos[i];
|
||||
var copter = info.Copter;
|
||||
var i1 = i;
|
||||
tasks[i1] = await Task.Factory.StartNew(async () =>
|
||||
{
|
||||
if (i1 > 0)
|
||||
{
|
||||
var prevCopter = infos[i1 - 1].Copter;
|
||||
//检测平面是否有相交的线
|
||||
while (CheckCrossing(infos, i1) &&
|
||||
Math.Abs(prevCopter.Altitude - copter.Altitude) < 2)
|
||||
{
|
||||
await Task.Delay(25).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
await FlyToTaskFlySingleCopterAsync(info);
|
||||
});
|
||||
*/
|
||||
}
|
||||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
||||
|
||||
@ -98,6 +551,92 @@ namespace Plane.FormationCreator.Formation
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
private async Task FlyToTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
|
||||
{
|
||||
int taskIndex = _flightTaskManager.CurrentRunningTaskIndex;
|
||||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
||||
|
||||
|
||||
DateTime dtNow = DateTime.Now;
|
||||
DateTime dtLastTime = DateTime.Now;
|
||||
TimeSpan ts = dtNow - dtLastTime;
|
||||
|
||||
int pointNum = 10;
|
||||
EHLocation[] targetLanLngArray = new EHLocation[pointNum];
|
||||
|
||||
if (taskIndex >= 2)
|
||||
// if (false)
|
||||
{
|
||||
var copterPreviousTaskInfo = _flightTaskManager.Tasks[taskIndex - 1].SingleCopterInfos[copterIndex];
|
||||
var direction = GeographyUtils.CalcDirection2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, info.TargetLat, info.TargetLng);
|
||||
var distance = GeographyUtils.CalcDistance(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, copterPreviousTaskInfo.TargetAlt, info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
var altDiff = info.TargetAlt - copterPreviousTaskInfo.TargetAlt;
|
||||
var horizontalDiff = GeographyUtils.CalcDistance2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, info.TargetLat, info.TargetLng);
|
||||
|
||||
for (int ii = 0; ii < pointNum; ii++)
|
||||
{
|
||||
// var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, (float)(GeographyUtils.RadToDeg(direction)), (float)distance * (ii + 1) / pointNum);
|
||||
var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, (float)(GeographyUtils.RadToDeg(direction)), (float)horizontalDiff * (ii + 1) / pointNum);
|
||||
var targetAltTmp = copterPreviousTaskInfo.TargetAlt + altDiff * (ii + 1) / pointNum;
|
||||
targetLanLngArray[ii] = new EHLocation(targetLatLng.Item1, targetLatLng.Item2, targetAltTmp);
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int ii = 0; ii < pointNum; ii++)
|
||||
{
|
||||
targetLanLngArray[ii] = new EHLocation(info.TargetLat,info.TargetLng, info.TargetAlt);
|
||||
}
|
||||
}
|
||||
|
||||
int pointStage = 0;
|
||||
while (pointStage < 10)
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
// await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
await info.Copter.FlyToAsync(targetLanLngArray[pointStage].Latitude, targetLanLngArray[pointStage].Longitude, targetLanLngArray[pointStage].Altitude);
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
while (!info.Copter.ArrivedTarget(targetLanLngArray[pointStage].Latitude, targetLanLngArray[pointStage].Longitude, targetLanLngArray[pointStage].Altitude))
|
||||
{
|
||||
if (_flightTaskManager.IsPaused == true)
|
||||
{
|
||||
await info.Copter.HoverAsync();
|
||||
return;
|
||||
}
|
||||
await Task.Delay(25).ConfigureAwait(false);
|
||||
|
||||
|
||||
dtNow = DateTime.Now;
|
||||
ts = dtNow - dtLastTime;
|
||||
if (ts.TotalMilliseconds > 2000)
|
||||
{
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
// await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
await info.Copter.FlyToAsync(targetLanLngArray[pointStage].Latitude, targetLanLngArray[pointStage].Longitude, targetLanLngArray[pointStage].Altitude);
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
}
|
||||
dtLastTime = dtNow;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
pointStage++;
|
||||
}
|
||||
|
||||
|
||||
// await info.Copter.HoverAsync();
|
||||
if (taskIndex == _flightTaskManager.Tasks.Count() - 1)
|
||||
{
|
||||
await info.Copter.HoverAsync();
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
private async Task FlyToTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
|
||||
{
|
||||
@ -108,7 +647,6 @@ namespace Plane.FormationCreator.Formation
|
||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
||||
return;
|
||||
|
||||
|
||||
await info.Copter.SetShowLEDAsync(info.FlytoShowLED);
|
||||
|
||||
if (info.Copter.State != Plane.Copters.CopterState.CommandMode)
|
||||
@ -117,38 +655,79 @@ namespace Plane.FormationCreator.Formation
|
||||
DateTime dtNow = DateTime.Now;
|
||||
DateTime dtLastTime = DateTime.Now;
|
||||
TimeSpan ts = dtNow - dtLastTime;
|
||||
//发送2次目标航点
|
||||
|
||||
int pointNum = 10;
|
||||
PLLocation[] targetLanLngArray = new PLLocation[pointNum];
|
||||
|
||||
// if (taskIndex >= 2) // 将地图上两个目标点分成10份
|
||||
if (false) // 只发送最终目标点
|
||||
{
|
||||
var copterPreviousTaskInfo = _flightTaskManager.Tasks[taskIndex - 1].SingleCopterInfos[copterIndex];
|
||||
var direction = GeographyUtils.CalcDirection2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, info.TargetLat, info.TargetLng);
|
||||
var distance = GeographyUtils.CalcDistance(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, copterPreviousTaskInfo.TargetAlt, info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
var altDiff = info.TargetAlt - copterPreviousTaskInfo.TargetAlt;
|
||||
var horizontalDiff = GeographyUtils.CalcDistance2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, info.TargetLat, info.TargetLng);
|
||||
|
||||
for (int ii = 0; ii < pointNum; ii++)
|
||||
{
|
||||
// var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, (float)(GeographyUtils.RadToDeg(direction)), (float)distance * (ii + 1) / pointNum);
|
||||
var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(copterPreviousTaskInfo.TargetLat, copterPreviousTaskInfo.TargetLng, (float)(GeographyUtils.RadToDeg(direction)), (float)horizontalDiff * (ii + 1) / pointNum);
|
||||
var targetAltTmp = copterPreviousTaskInfo.TargetAlt + altDiff * (ii + 1) / pointNum;
|
||||
targetLanLngArray[ii] = new PLLocation(targetLatLng.Item1, targetLatLng.Item2, targetAltTmp);
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int ii = 0; ii < pointNum; ii++)
|
||||
{
|
||||
targetLanLngArray[ii] = new PLLocation(info.TargetLat,info.TargetLng, info.TargetAlt);
|
||||
}
|
||||
}
|
||||
|
||||
int pointStage = 0;
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
await Task.Delay(20).ConfigureAwait(false);
|
||||
// await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
await info.Copter.FlyToAsync(targetLanLngArray[pointStage].Latitude, targetLanLngArray[pointStage].Longitude, targetLanLngArray[pointStage].Altitude);
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
}
|
||||
info.Copter.MissionStatus = 1;
|
||||
//判断是否到达
|
||||
pointStage++;
|
||||
|
||||
while (!info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, info.TargetAlt))
|
||||
{
|
||||
if (_flightTaskManager.IsPaused == true)
|
||||
{
|
||||
info.Copter.MissionStatus = 0;
|
||||
await info.Copter.HoverAsync();
|
||||
return;
|
||||
}
|
||||
await Task.Delay(25).ConfigureAwait(false); //判断是否到达位置20hz
|
||||
|
||||
|
||||
dtNow = DateTime.Now;
|
||||
ts = dtNow - dtLastTime;
|
||||
if (ts.TotalMilliseconds > 1000) // 每1000ms发送一遍指点坐标
|
||||
{
|
||||
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
// await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
await info.Copter.FlyToAsync(targetLanLngArray[pointStage].Latitude, targetLanLngArray[pointStage].Longitude, targetLanLngArray[pointStage].Altitude);
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
}
|
||||
pointStage++;
|
||||
if (pointStage == pointNum)
|
||||
{
|
||||
pointStage = pointNum - 1;
|
||||
}
|
||||
dtLastTime = dtNow;
|
||||
}
|
||||
await Task.Delay(25).ConfigureAwait(false); //判断是否到达位置20hz
|
||||
|
||||
}
|
||||
info.Copter.MissionStatus = 0;
|
||||
|
||||
// await info.Copter.HoverAsync();
|
||||
if (taskIndex == _flightTaskManager.Tasks.Count() - 1)
|
||||
{
|
||||
await info.Copter.HoverAsync();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user