去掉flyto的无用代码，降低数据传输量
加入航点写入和启动任务
This commit is contained in:
parent
2ae79ec811
commit
db60b6b8cc
@ -45,473 +45,12 @@ 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() // 全部飞到指定航点
|
||||
{
|
||||
@ -551,93 +90,8 @@ 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)
|
||||
{
|
||||
int taskIndex = _flightTaskManager.CurrentRunningTaskIndex;
|
||||
@ -651,51 +105,26 @@ namespace Plane.FormationCreator.Formation
|
||||
}
|
||||
|
||||
await info.Copter.SetShowLEDAsync(info.FlytoShowLED);
|
||||
|
||||
|
||||
if (info.Copter.State != Plane.Copters.CopterState.CommandMode)
|
||||
await info.Copter.GuidAsync();
|
||||
await info.Copter.GuidAsync();
|
||||
|
||||
DateTime dtNow = DateTime.Now;
|
||||
DateTime dtLastTime = DateTime.Now;
|
||||
TimeSpan ts = dtNow - dtLastTime;
|
||||
|
||||
int pointNum = 10;
|
||||
PLLocation[] targetLanLngArray = new PLLocation[pointNum];
|
||||
|
||||
// if (taskIndex >= 2) // 将地图上两个目标点分成10份
|
||||
if (false) // 只发送最终目标点
|
||||
/*
|
||||
for (int i = 0; i < 5; i++)
|
||||
{
|
||||
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 < 5; 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 info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
}
|
||||
pointStage++;
|
||||
*/
|
||||
//发送目标航点1次
|
||||
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
|
||||
|
||||
while (!info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, info.TargetAlt))
|
||||
{
|
||||
@ -704,7 +133,7 @@ namespace Plane.FormationCreator.Formation
|
||||
await info.Copter.HoverAsync();
|
||||
return;
|
||||
}
|
||||
await Task.Delay(25).ConfigureAwait(false); //判断是否到达位置20hz
|
||||
await Task.Delay(250).ConfigureAwait(false); //判断是否到达位置10hz
|
||||
|
||||
|
||||
dtNow = DateTime.Now;
|
||||
@ -713,15 +142,10 @@ namespace Plane.FormationCreator.Formation
|
||||
{
|
||||
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 info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
}
|
||||
pointStage++;
|
||||
if (pointStage == pointNum)
|
||||
{
|
||||
pointStage = pointNum - 1;
|
||||
}
|
||||
|
||||
dtLastTime = dtNow;
|
||||
}
|
||||
// 当该飞机被标记时，悬停并跳过飞行任务
|
||||
|
||||
@ -434,6 +434,248 @@ namespace Plane.FormationCreator.ViewModels
|
||||
}
|
||||
}
|
||||
|
||||
private ICommand _MissionStartCommand;
|
||||
public ICommand MissionStartCommand
|
||||
{
|
||||
get
|
||||
{
|
||||
return _MissionStartCommand ?? (_MissionStartCommand = new RelayCommand(async () =>
|
||||
{
|
||||
int utchour = DateTime.UtcNow.AddSeconds(5).Hour;
|
||||
int utcminute = DateTime.UtcNow.AddSeconds(5).Minute;
|
||||
int utcsecond = DateTime.UtcNow.AddSeconds(5).Second;
|
||||
foreach (var vcopter in _copterManager.Copters)
|
||||
{
|
||||
|
||||
await vcopter.MissionStartAsync(utchour,
|
||||
utcminute,
|
||||
utcsecond,
|
||||
110.23456,
|
||||
40.23432);
|
||||
}
|
||||
}));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
private ICommand _WriteMissionCommand;
|
||||
public ICommand WriteMissionCommand
|
||||
{
|
||||
get
|
||||
{
|
||||
return _WriteMissionCommand ?? (_WriteMissionCommand = new RelayCommand(async () =>
|
||||
{
|
||||
|
||||
|
||||
ViewModels.ModifyTaskViewModel _taskmodimodel = ServiceLocator.Current.GetInstance<ViewModels.ModifyTaskViewModel>();
|
||||
var _flightTaskManager = _taskmodimodel.FlightTaskManager;
|
||||
int coptercount = _copterManager.Copters.Count;
|
||||
int taskcount = _flightTaskManager.Tasks.Count;
|
||||
if (taskcount < 2) return;
|
||||
|
||||
for (int i = 0; i < coptercount; i++)
|
||||
{
|
||||
|
||||
///写航线开始
|
||||
var missions = new IMission[taskcount + 1]; //不要起飞任务但增加一个起飞后低空航点，见起飞任务,再增加一个降落低空航点，见降落部分
|
||||
int missindex = 0;
|
||||
|
||||
for (int j = 0; j < _flightTaskManager.Tasks.Count; j++)
|
||||
{
|
||||
|
||||
|
||||
|
||||
if (_flightTaskManager.Tasks[j].TaskType == FlightTaskType.FlyTo)
|
||||
{
|
||||
missions[missindex++] = Mission.CreateWaypointMission(_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLat,
|
||||
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLng, _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt);
|
||||
|
||||
}
|
||||
|
||||
if (_flightTaskManager.Tasks[j].TaskType == FlightTaskType.TakeOff)
|
||||
{
|
||||
|
||||
//起飞低空航点
|
||||
missions[missindex++] = Mission.CreateWaypointMission(_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLat,
|
||||
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLng, 10);
|
||||
//不要起飞任务，在写入时会自动加入
|
||||
//自动起飞10米后，需要飞到第一个航点下方
|
||||
}
|
||||
|
||||
if (_flightTaskManager.Tasks[j].TaskType == FlightTaskType.Loiter)
|
||||
{
|
||||
missions[missindex++] = Mission.CreateWaypointMission(_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLat,
|
||||
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLng, _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt);
|
||||
|
||||
}
|
||||
if (_flightTaskManager.Tasks[j].TaskType == FlightTaskType.ReturnToLand)
|
||||
{
|
||||
//降落低空航点
|
||||
missions[missindex++] = Mission.CreateWaypointMission(_flightTaskManager.Tasks[j - 1].SingleCopterInfos[i].TargetLat,
|
||||
_flightTaskManager.Tasks[j - 1].SingleCopterInfos[i].TargetLng, 10);
|
||||
//返航
|
||||
missions[missindex++] = Mission.CreateReturnToLaunchMission();
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
var result = await _copterManager.Copters[i].WriteMissionListAsync(missions);
|
||||
//MessageBox.Show($"The result of WriteMissions: {result}");
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 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 PLCopter).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 IMission[]
|
||||
{
|
||||
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}");
|
||||
*/
|
||||
/////////////////写航向结束
|
||||
|
||||
|
||||
}));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
private ICommand _FlagCommand;
|
||||
public ICommand FlagCommand
|
||||
{
|
||||
|
||||
@ -40,6 +40,10 @@
|
||||
Command="{Binding HoverCommand}" />
|
||||
<Button Content="手动"
|
||||
Command="{Binding FloatCommand}" />
|
||||
<Button Content="航点"
|
||||
Command="{Binding WriteMissionCommand}" />
|
||||
<Button Content="开始"
|
||||
Command="{Binding MissionStartCommand}" />
|
||||
|
||||
</WrapPanel>
|
||||
<WrapPanel>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user