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

[app.xaml.cs]网络不正常，加了try保护

Browse Source 
[Extensions.cs]碰撞判断逻辑改了一下
[FlightTaskManager.cs]任务开始前加了预检测,RTK没fix或GPS没有收到8颗以上卫星
[FlightTask_FlyTo.cs]清理飞往逻辑，优化通讯
[FlightTask_ReturnToLand.cs]加入MissionStatus控制
【FlightTask_TakeOff.cs】加入MissionStatus控制
【ControlPanelViewModel.cs】加入批量修改参数功能
[ControlPanelView.xaml]加入批量修改参数功能
[MapView_CopterDrawing.cs]MissionStatus显示，在现有飞机上加个小头
This commit is contained in:
pxzleo 2017-09-11 04:04:39 +08:00
parent 7d8646b4d8
commit dae8be8e18
10 changed files with 229 additions and 653 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
Plane.FormationCreator/App.xaml.cs
View File

@ -114,10 +114,13 @@ namespace Plane.FormationCreator
MainWindow = new MainWindow();
MainWindow.Show();
try
{
TcpServerConnectionManager.Instance.ConnectionEstablished += ConnectionManager_ConnectionEstablished;
if (!TcpServerConnectionManager.Instance.StartListening())
{
Alert.Show("网络连接不正常，无法启动监听。");
Alert.Show("网络连接不正常，无法连接飞机。");
return;
}
UdpServerConnectionManager.Instance.ExceptionThrown += (sender, e1) =>
@ -126,6 +129,16 @@ namespace Plane.FormationCreator
};
UdpServerConnectionManager.Instance.ConnectionEstablished += ConnectionManager_ConnectionEstablished;
UdpServerConnectionManager.Instance.StartReceiving();
}
catch (Exception ex)
{
Alert.Show("网络连接不正常，无法连接飞机。");
return;
}
}
private void Copter_TextReceived(object sender, MessageCreatedEventArgs e)
{

22
Plane.FormationCreator/Formation/Extensions.cs
View File

@ -59,38 +59,30 @@ namespace Plane.FormationCreator.Formation
{
return GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, targetLat, targetLng);
}
//是否达到目标航点
public static bool ArrivedTarget(this ICopter copter, double targetLat, double targetLng, float targetAlt)
{
// return copter.DistanceTo(targetLat, targetLng, targetAlt) < 0.6; //到达航点精度
if (copter.GpsFixType == GpsFixType.RTKFIXED)
return copter.DistanceTo(targetLat, targetLng, targetAlt) < RTKArrivedDis; //RTK到达航点精度0.6米
else
return copter.DistanceTo(targetLat, targetLng, targetAlt) < GpsArrivedDis; //GPS到达航点精度2.0米
}
public static bool ArrivedTargetInPlane(this ICopter copter, double targetLat, double targetLng)
{
return copter.InPlaneDistanceTo(targetLat, targetLng) < 1;
}
//是否已达到目标朝向
public static bool ArrivedHeading(this ICopter copter, short targetHeading)
{
return Math.Abs(copter.Heading - targetHeading) < 2;
}
//是否距离太近
public static bool IsTooCloseTo(this ICopter copter, ICopter copter2)
{
if (copter.GpsFixType == GpsFixType.RTKFIXED)
return copter.DistanceTo(copter2) < RTKClosedDis; //最近距离0.5米
if ((copter.GpsFixType == GpsFixType.RTKFIXED)&&(copter2.GpsFixType == GpsFixType.RTKFIXED))
return copter.DistanceTo(copter2) < RTKClosedDis; //都是RTKFIXED则最近距离0.5米
else
return copter.DistanceTo(copter2) < GpsCloseDis; //最近距离2米
return copter.DistanceTo(copter2) < GpsCloseDis; //其他任何情况最近距离2米
}
}
}
}

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

@ -924,14 +924,16 @@ namespace Plane.FormationCreator.Formation
public bool? IsPaused
{
get { return _IsPaused; }
private set {
private set
{
if (Set(nameof(IsPaused), ref _IsPaused, value))
{
if (_IsPaused ?? false)
{
MessageText = "任务暂停！";
}else
}
else
{
MessageText = "任务运行中";
}
@ -941,15 +943,69 @@ namespace Plane.FormationCreator.Formation
}
}
private bool _CanRunTask=false;
public bool CanRunTask
{
get { return _CanRunTask; }
private set
{
if (Set(nameof(CanRunTask), ref _CanRunTask, value))
{
}
}
}
public async Task RunTaskAsync()
{
if (IsPaused != true)
{
await RunCheckAsync();
if (!CanRunTask)
return;
}
Message.Show("任务开始");
await RunAsync();
if ((IsPaused ?? false) == false)
Message.Show("任务完成");
}
public async Task RunCheckAsync()
{
CanRunTask = false;
foreach (var copter in _copterManager.Copters)
{
float gpstype = await copter.GetParamAsync("GPS_TYPE");
//RTK
if (gpstype == 15)
{
if (copter.GpsFixType != GpsFixType.RTKFIXED)
{
Message.Show(copter.Id+ ",是RTK定位，但未进入RTKFIX模式");
return;
}
}
//auto--普通GPS
if (gpstype == 0)
{
if (!copter.IsGpsAccurate)
{
Message.Show(copter.Id + ",GPS未定位");
return;
}
}
}
CanRunTask = true;
}
public async Task RunAsync()
{
IsPaused = false;

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

@ -45,389 +45,10 @@ 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() // 仅改变高度
public async Task RunFlyToTaskAsync() // 全部飞到指定航点
{
//是否有交错
if (StaggerRoutes)
{
var infos = SingleCopterInfos;
@ -442,102 +63,14 @@ namespace Plane.FormationCreator.Formation
if (i1 > 0)
{
var prevCopter = infos[i1 - 1].Copter;
//检测平面是否有相交的线
while (CheckCrossing(infos, i1) &&
prevCopter.Altitude - copter.Altitude < 2)
Math.Abs(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() // 全部飞到指定航点
{
//是否有交错
if (StaggerRoutes)
{
var infos = SingleCopterInfos;
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);
await FlyToTaskFlySingleCopterAsync(info);
});
}
await Task.WhenAll(tasks).ConfigureAwait(false);
@ -551,92 +84,6 @@ 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)
{
@ -647,6 +94,7 @@ namespace Plane.FormationCreator.Formation
if ((bool)_copterManager.CopterStatus[copterIndex])
return;
await info.Copter.SetShowLEDAsync(info.FlytoShowLED);
if (info.Copter.State != Plane.Copters.CopterState.CommandMode)
@ -655,79 +103,38 @@ namespace Plane.FormationCreator.Formation
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) // 只发送最终目标点
//发送2次目标航点
for (int i = 0; i < 2; 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);
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
await Task.Delay(20).ConfigureAwait(false);
}
}
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 Task.Delay(10).ConfigureAwait(false);
}
pointStage++;
info.Copter.MissionStatus = 1;
//判断是否到达
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 > 500) // 每500ms发送一遍指点坐标
if (ts.TotalMilliseconds > 1000) // 每1000ms发送一遍指点坐标
{
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;
}
await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, info.TargetAlt);
dtLastTime = dtNow;
}
await Task.Delay(25).ConfigureAwait(false); //判断是否到达位置20hz
}
// await info.Copter.HoverAsync();
info.Copter.MissionStatus = 0;
if (taskIndex == _flightTaskManager.Tasks.Count() - 1)
{
await info.Copter.HoverAsync();
}
}

8
Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs
View File

@ -325,11 +325,12 @@ namespace Plane.FormationCreator.Formation
{
await info.Copter.FlyToAsync(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt);
}
info.Copter.MissionStatus = 1;
while (!info.Copter.ArrivedTarget(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt))
{
if (_flightTaskManager.IsPaused == true)
{
info.Copter.MissionStatus = 0;
await info.Copter.HoverAsync();
return;
}
@ -346,6 +347,8 @@ namespace Plane.FormationCreator.Formation
dtLastTime = dtNow;
}
}
info.Copter.MissionStatus = 0;
info.RTLStage++;
}
@ -362,11 +365,13 @@ namespace Plane.FormationCreator.Formation
{
await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt);
}
info.Copter.MissionStatus = 1;
while (!info.Copter.ArrivedTarget(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt))
{
if (_flightTaskManager.IsPaused == true)
{
info.Copter.MissionStatus = 0;
await info.Copter.HoverAsync();
return;
}
@ -383,6 +388,7 @@ namespace Plane.FormationCreator.Formation
dtLastTime = dtNow;
}
}
info.Copter.MissionStatus = 0;
info.RTLStage++;
}

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

@ -271,11 +271,13 @@ namespace Plane.FormationCreator.Formation
await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt);
await Task.Delay(10).ConfigureAwait(false);
}
info.Copter.MissionStatus = 1;
//直到到达第一个航点并高度15米
while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt))
{
if (_flightTaskManager.IsPaused == true)
{
info.Copter.MissionStatus = 0;
await info.Copter.HoverAsync();
return;
}
@ -294,10 +296,14 @@ namespace Plane.FormationCreator.Formation
dtLastTime = dtNow;
}
}
info.Copter.MissionStatus = 0;
if (_flightTaskManager.IsPaused == false)
{
info.takeOffStage++; //当前变为2
}
info.Copter.MissionStatus = 0;
}
dtNow = DateTime.Now;
@ -312,11 +318,15 @@ namespace Plane.FormationCreator.Formation
await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt);
await Task.Delay(10).ConfigureAwait(false);
}
info.Copter.MissionStatus = 1;
//直到到达第一个航点
while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt))
{
if (_flightTaskManager.IsPaused == true)
{
info.Copter.MissionStatus = 0;
await info.Copter.HoverAsync();
return;
}
@ -335,6 +345,8 @@ namespace Plane.FormationCreator.Formation
dtLastTime = dtNow;
}
}
info.Copter.MissionStatus = 0;
}
// });

7
Plane.FormationCreator/Plane.FormationCreator.csproj
View File

@ -139,6 +139,9 @@
<Compile Include="Formation\FlightTask_TakeOff.cs" />
<Compile Include="Maps\OpenStreetMapTileLayer.cs" />
<Compile Include="Maps\OpenStreetMapTileSource.cs" />
<Compile Include="ModifyParam.xaml.cs">
<DependentUpon>ModifyParam.xaml</DependentUpon>
</Compile>
<Compile Include="ServiceLocatorConfigurer.cs" />
<Compile Include="Test.cs" />
<Compile Include="CalculationLogLatDistance.cs" />
@ -211,6 +214,10 @@
<SubType>Designer</SubType>
<Generator>MSBuild:Compile</Generator>
</Page>
<Page Include="ModifyParam.xaml">
<SubType>Designer</SubType>
<Generator>MSBuild:Compile</Generator>
</Page>
<Page Include="Styles.xaml">
<SubType>Designer</SubType>
<Generator>MSBuild:Compile</Generator>

53
Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs
View File

@ -140,6 +140,51 @@ namespace Plane.FormationCreator.ViewModels
}
}
private ICommand _ParamModify;
public ICommand ParamModify
{
get
{
return _ParamModify ?? (_ParamModify = new RelayCommand(async () =>
{
var ModifyParamWindow = new Plane.FormationCreator.ModifyParam();
if (ModifyParamWindow.ShowDialog() == true)
{
string paramstr = ModifyParamWindow.textParamName.Text;
float paramvalue = Convert.ToSingle(ModifyParamWindow.textParamValue.Text);
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select
(copter => copter.SetParamAsync(paramstr, paramvalue) ));
bool ifallok = true;
await Task.WhenAll(
_copterManager.AcceptingControlCopters.Select(async copter =>
{
float getvaule = await copter.GetParamAsync(paramstr);
if (getvaule != paramvalue)
{
ifallok = false;
Alert.Show(copter.Id+ "设置失败,读取的参数是"+ getvaule, "提示", MessageBoxButton.OK, MessageBoxImage.Information);
}
}
));
if (ifallok)
Alert.Show("所有已选的飞机参数设置完成", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
else
Alert.Show("有部分飞机参数设置失败，请检查并再次设置", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
}
}));
}
}
private ICommand _ReturnToLaunchCommand;
public ICommand ReturnToLaunchCommand
{
@ -254,13 +299,15 @@ namespace Plane.FormationCreator.ViewModels
var packet = await ReadRTKPacketAsync().ConfigureAwait(false);
//分发到每个飞机
foreach (var copter in _copterManager.Copters)
{
//RTK定位才发送
if (copter.LocationType == CopterLocationType.RTK)
{
await copter.InjectGpsDataAsync(packet, (ushort)packet.Length);
await Task.Delay(10).ConfigureAwait(false); //为了在时间上均摊RTK数据包
}
}
await Task.Delay(200).ConfigureAwait(false);
}
}).ConfigureAwait(false);
Alert.Show("RTK数据停止发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information);

2
Plane.FormationCreator/Views/ControlPanelView.xaml
View File

@ -40,6 +40,8 @@
Command="{Binding HoverCommand}" />
<Button Content="手动"
Command="{Binding FloatCommand}" />
<Button Content="参数"
Command="{Binding ParamModify}" />
</WrapPanel>
<WrapPanel>

34
Plane.FormationCreator/Views/MapView_CopterDrawing.cs
View File

@ -105,12 +105,20 @@ namespace Plane.FormationCreator.Views
//实时飞行航线
// _map.Children.Add(Track);
//画三角形飞机
Dot = new Polygon();
// (0,-12)(-8,12)(8,12)是个倒三角形
//
//
Dot.Points.Add(new Point(0, -COPTER_RADIUS));
Dot.Points.Add(new Point(-COPTER_RADIUS * 2 / 3, COPTER_RADIUS));
Dot.Points.Add(new Point(COPTER_RADIUS * 2 / 3, COPTER_RADIUS));
Dot.Fill = _brush; // new SolidColorBrush(Color.FromArgb(200, 0, 122, 204));
Dot.StrokeThickness = 1;
Dot.Width = COPTER_RADIUS * 2;
@ -121,6 +129,7 @@ namespace Plane.FormationCreator.Views
DotContainer = new Grid { Tag = COPTER_TAG };
DotContainer.Children.Add(Dot);
//飞机里面的编号
DotContainer.Children.Add(new TextBlock
{
Text = Copter.Name,
@ -129,7 +138,11 @@ namespace Plane.FormationCreator.Views
HorizontalAlignment = HorizontalAlignment.Center,
VerticalAlignment = VerticalAlignment.Center
});
//飞机加入地图
_map.Children.Add(DotContainer);
MapLayer.SetZIndex(DotContainer, 100);
this.Route = new MapPolyline { Locations = new LocationCollection { location } };
@ -147,6 +160,23 @@ namespace Plane.FormationCreator.Views
}
else
{
Dot.Points.Clear();
if (Copter.MissionStatus == 1)
{
Dot.Points.Add(new Point(-4, -COPTER_RADIUS));
Dot.Points.Add(new Point(0, -COPTER_RADIUS));
Dot.Points.Add(new Point(-COPTER_RADIUS * 2 / 3, COPTER_RADIUS));
Dot.Points.Add(new Point(COPTER_RADIUS * 2 / 3, COPTER_RADIUS));
}
else {
Dot.Points.Add(new Point(0, -COPTER_RADIUS));
Dot.Points.Add(new Point(-COPTER_RADIUS * 2 / 3, COPTER_RADIUS));
Dot.Points.Add(new Point(COPTER_RADIUS * 2 / 3, COPTER_RADIUS));
}
if (LastLocation != null && location.CalcDistance(LastLocation) < 0.1)
{
locationUpdated = false;
@ -162,6 +192,10 @@ namespace Plane.FormationCreator.Views
blackBrush.Color = Colors.White;
else
blackBrush.Color = Colors.Transparent;
Dot.Stroke = blackBrush;
var trans = Dot.RenderTransform as RotateTransform;