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

添加了航点导出用于C4D

Browse Source 
测试自动计算路线
导入航点数量不一致报错
写入航点错误提示
This commit is contained in:
zxd 2019-11-23 22:59:05 +08:00
parent e5ded3d92a
commit b098acc7d0
6 changed files with 303 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1070,17 +1070,161 @@ namespace Plane.FormationCreator.Formation
}
}
public void OptimizeRouteNew()
{
Dictionary<int, PLLocation> curTaskPoint = new Dictionary<int, PLLocation>();
Dictionary<int, PLLocation> prevTaskPoint = new Dictionary<int, PLLocation>();
//获取当前航点与前一航点所有经纬高
for (int i = 0; i < _copterManager.Copters.Count; i++)
{
//当前任务
var curinfo = SelectedTask.SingleCopterInfos[i];
PLLocation curLoc = new PLLocation(curinfo.TargetLat, curinfo.TargetLng, curinfo.TargetAlt);
curTaskPoint.Add(i, curLoc);
//前一任务
var prevInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i];
PLLocation prevLoc = new PLLocation(prevInfo.TargetLat, prevInfo.TargetLng, prevInfo.TargetAlt);
prevTaskPoint.Add(i, prevLoc);
}
double farDistance;
double nearDistance;
int index;
Dictionary<int, LatLng> recordLatLng = new Dictionary<int, LatLng>();
while (curTaskPoint.Count > 0)
{
farDistance = double.MinValue;
index = 0;
LatLng centerLatLng = CenterLatLng(curTaskPoint.Values.ToList(), prevTaskPoint.Values.ToList());
//计算两个列表距离中心最远距离
double distance1;
bool farIsCurTaskPoint = true;
foreach (var item in curTaskPoint)
{
distance1 = GeographyUtils.CalcDistance(centerLatLng.Lat, centerLatLng.Lng, 1,
item.Value.Latitude, item.Value.Longitude, 1);
if (distance1 > farDistance)
{
index = item.Key;
farDistance = distance1;
farIsCurTaskPoint = true;
}
}
foreach (var item in prevTaskPoint)
{
distance1 = GeographyUtils.CalcDistance(centerLatLng.Lat, centerLatLng.Lng, 1,
item.Value.Latitude, item.Value.Longitude, 1);
if (distance1 > farDistance)
{
index = item.Key;
farDistance = distance1;
farIsCurTaskPoint = false;
}
}
double tempDistance1;
nearDistance = double.MaxValue;
int nearIndex = 0;
if (farIsCurTaskPoint)
{
double curLat = curTaskPoint[index].Latitude;
double curLng = curTaskPoint[index].Longitude;
//最远的航点在当前任务
foreach (var item in prevTaskPoint)
{
tempDistance1 = GeographyUtils.CalcDistance(curLat, curLng, 1,
item.Value.Latitude, item.Value.Longitude, 1);
if (tempDistance1 < nearDistance)
{
nearDistance = tempDistance1;
nearIndex = item.Key;
}
}
recordLatLng.Add(nearIndex, new LatLng(curLat, curLng));
curTaskPoint.Remove(index);
prevTaskPoint.Remove(nearIndex);
}
else
{
//最远的航点在前一任务
double prevLat = prevTaskPoint[index].Latitude;
double prevLng = prevTaskPoint[index].Longitude;
//最远的航点在当前任务
foreach (var item in curTaskPoint)
{
tempDistance1 = GeographyUtils.CalcDistance(prevLat, prevLng, 1,
item.Value.Latitude, item.Value.Longitude, 1);
if (tempDistance1 < nearDistance)
{
nearDistance = tempDistance1;
nearIndex = item.Key;
}
}
recordLatLng.Add(index, new LatLng(curTaskPoint[nearIndex].Latitude, curTaskPoint[nearIndex].Longitude));
curTaskPoint.Remove(nearIndex);
prevTaskPoint.Remove(index);
}
}
for (int i = 0; i < _copterManager.Copters.Count; i++)
{
SelectedTask.SingleCopterInfos[i].TargetLat = recordLatLng[i].Lat;
SelectedTask.SingleCopterInfos[i].TargetLng = recordLatLng[i].Lng;
}
}
//计算中心点 (二维的-只算水平面)
private LatLng CenterLatLng(List<PLLocation> point1, List<PLLocation> point2)
{
LatLng centerLatLng = new LatLng(0, 0);
if (point1.Count != point2.Count) return centerLatLng;
double minLat = point1[0].Latitude;
double maxLat = point1[0].Latitude;
double minLng = point1[0].Longitude;
double maxLng = point1[0].Longitude;
int count = point1.Count;
for (int i = 0; i < count; i++)
{
minLat = Math.Min(minLat, Math.Min(point1[i].Latitude, point2[i].Latitude));
maxLat = Math.Min(maxLat, Math.Min(point1[i].Latitude, point2[i].Latitude));
minLng = Math.Min(minLng, Math.Min(point1[i].Longitude, point2[i].Longitude));
maxLng = Math.Max(maxLng, Math.Max(point1[i].Longitude, point2[i].Longitude));
}
centerLatLng.Lat = (minLat + maxLat) / 2;
centerLatLng.Lng = (minLng + maxLng) / 2;
return centerLatLng;
}
public void OptimizeRoute2()
{
double minLat = SelectedTask.SingleCopterInfos[0].TargetLat;
double maxLat = SelectedTask.SingleCopterInfos[0].TargetLat;
double minLng = SelectedTask.SingleCopterInfos[0].TargetLng;
double maxLng = SelectedTask.SingleCopterInfos[0].TargetLng;
Dictionary<int, LatLng> recordLatLng = new Dictionary<int, LatLng>();
for (int i = 0; i < _copterManager.Copters.Count; i++)
{
var curinfo = SelectedTask.SingleCopterInfos[i];
recordLatLng.Add(i, new LatLng(curinfo.TargetLat, curinfo.TargetLng));
//recordLatLng.Add(i, new LatLng(curinfo.TargetLat, curinfo.TargetLng));
var lastInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i];
minLat = Math.Min(minLat, Math.Min(curinfo.TargetLat, lastInfo.TargetLat));
maxLat = Math.Max(maxLat, Math.Max(curinfo.TargetLat, lastInfo.TargetLat));
@ -1088,8 +1232,10 @@ namespace Plane.FormationCreator.Formation
minLng = Math.Min(minLng, Math.Min(curinfo.TargetLng, lastInfo.TargetLng));
maxLng = Math.Max(maxLng, Math.Max(curinfo.TargetLng, lastInfo.TargetLng));
}
double CenterLat = (maxLat - minLat) / 2;
double CenterLng = (maxLng - minLng) / 2;
double CenterLat = (maxLat + minLat) / 2;
double CenterLng = (maxLng + minLng) / 2;
Message.Show($"中心点：{CenterLat} {CenterLng}");
Dictionary<int, double> distanceDic = new Dictionary<int, double>();
for (int i = 0; i < _copterManager.Copters.Count; i++)
@ -1109,7 +1255,9 @@ namespace Plane.FormationCreator.Formation
distanceDic.Add(nums, distance2);
}
distanceDic = distanceDic.OrderByDescending(o => o.Value).ToDictionary(p => p.Key, o => o.Value);
Dictionary<int, LatLng> recordLatLng = new Dictionary<int, LatLng>();
while (distanceDic.Count > 0)
{
KeyValuePair<int, double> kv = distanceDic.First();
@ -1122,17 +1270,23 @@ namespace Plane.FormationCreator.Formation
int index = 0;
for (int i = 0; i < Tasks[taskIndex - 1].SingleCopterInfos.Count; i++)
{
var destInfo = Tasks[taskIndex - 1].SingleCopterInfos[i];
double distance = GeographyUtils.CalcDistance(curInfo.TargetLat, curInfo.TargetLng, 1,
destInfo.TargetLat, destInfo.TargetLng, 1);
if (distance < mindistance)
if (distanceDic.ContainsKey((taskIndex - 1) << 16 ^ i))
{
mindistance = distance;
index = i;
var destInfo = Tasks[taskIndex - 1].SingleCopterInfos[i];
double distance = GeographyUtils.CalcDistance(curInfo.TargetLat, curInfo.TargetLng, 1,
destInfo.TargetLat, destInfo.TargetLng, 1);
if (distance < mindistance)
{
mindistance = distance;
index = i;
}
}
}
Tasks[taskIndex].SingleCopterInfos[index].TargetLat = recordLatLng[copterID].Lat;
Tasks[taskIndex].SingleCopterInfos[index].TargetLng = recordLatLng[copterID].Lng;
recordLatLng.Add(index,
new LatLng(SelectedTask.SingleCopterInfos[copterID].TargetLat,
SelectedTask.SingleCopterInfos[copterID].TargetLng));
Message.Show($"航点{taskIndex} ID{copterID + 1} --航点{taskIndex - 1} id{index + 1}");
distanceDic.Remove(kv.Key);
distanceDic.Remove(( taskIndex - 1) << 16 ^ index );
}
@ -1142,22 +1296,34 @@ namespace Plane.FormationCreator.Formation
int index = 0;
for (int i = 0; i < Tasks[taskIndex + 1].SingleCopterInfos.Count; i++)
{
var destInfo = Tasks[taskIndex + 1].SingleCopterInfos[i];
double distance = GeographyUtils.CalcDistance(curInfo.TargetLat, curInfo.TargetLng, 1,
destInfo.TargetLat, destInfo.TargetLng, 1);
if (distance < mindistance)
if (distanceDic.ContainsKey((taskIndex + 1) << 16 ^ i))
{
mindistance = distance;
index = i;
var destInfo = Tasks[taskIndex + 1].SingleCopterInfos[i];
double distance = GeographyUtils.CalcDistance(curInfo.TargetLat, curInfo.TargetLng, 1,
destInfo.TargetLat, destInfo.TargetLng, 1);
if (distance < mindistance && distanceDic.ContainsKey((SelectedTaskIndex) << 16 ^ i))
{
mindistance = distance;
index = i;
}
}
}
Tasks[taskIndex + 1].SingleCopterInfos[copterID].TargetLat = recordLatLng[index].Lat;
Tasks[taskIndex + 1].SingleCopterInfos[copterID].TargetLng = recordLatLng[index].Lng;
recordLatLng.Add(copterID,
new LatLng(SelectedTask.SingleCopterInfos[index].TargetLat,
SelectedTask.SingleCopterInfos[index].TargetLng));
Message.Show($"航点{taskIndex} ID{copterID + 1} --航点{taskIndex + 1} id{index + 1}");
distanceDic.Remove(kv.Key);
distanceDic.Remove(( taskIndex + 1) << 16 ^ index );
}
}
for (int i = 0; i < _copterManager.Copters.Count; i++)
{
SelectedTask.SingleCopterInfos[i].TargetLat = recordLatLng[i].Lat;
SelectedTask.SingleCopterInfos[i].TargetLng = recordLatLng[i].Lng;
}
}
public void OptimizeRoute()

3
Plane.FormationCreator/ViewModels/ConnectViewModel.cs
View File

@ -182,11 +182,10 @@ namespace Plane.FormationCreator.ViewModels
if (commModule.CommModuleCopterCount > 0)
{
short queryCount = 40;
short begin = 1;
while (begin < commModule.CommModuleCopterCount)
{
short end = (short)(begin + queryCount -1);
short end = (short)(begin + queryCount - 1);
if (end > commModule.CommModuleCopterCount)
end = (short)commModule.CommModuleCopterCount;
byte[] beginByte = BitConverter.GetBytes(begin);

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

@ -901,6 +901,8 @@ namespace Plane.FormationCreator.ViewModels
}
else//停止RTK
{
await _commModuleManager.CloseRtcmLoop();
trkthreadrun = false;
Rtkport.Close();
Rtkport = null;
@ -968,7 +970,7 @@ namespace Plane.FormationCreator.ViewModels
{
//rtcmDatas.Add(rtcm3.packet);
//await SendRtcmData();
Message.Show($"{DateTime.Now.ToString("HH:mm:ss fff")}---rtcm3.length = {rtcm3.length}");
//Message.Show($"{DateTime.Now.ToString("HH:mm:ss fff")}---rtcm3.length = {rtcm3.length}");
await _commModuleManager.InjectGpsRTCMDataAsync(rtcm3.packet, rtcm3.length);
}
}
@ -1318,82 +1320,93 @@ namespace Plane.FormationCreator.ViewModels
/// <returns></returns>
private async Task CollectMissions(int i)
{
float groudAlt = _copterManager.Copters[i].GroundAlt;
var missions = new List<IMission>();
var _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
for (int j = 0; j < _flightTaskManager.Tasks.Count; j++)
int j = 0;
try
{
switch (_flightTaskManager.Tasks[j].TaskType)
float groudAlt = _copterManager.Copters[i].GroundAlt;
var missions = new List<IMission>();
var _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
for (j = 0; j < _flightTaskManager.Tasks.Count; j++)
{
case FlightTaskType.TakeOff:
int takeofftime = (int)(_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetAlt / 2.5 + 5);
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
missions.Add(Mission.CreateTakeoffMission(
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TakeOffWaitTime,
_flightTaskManager.Tasks[j].TakeOffTime,
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLat - _flightTaskManager.OriginLat,
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLng - _flightTaskManager.OriginLng,
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetAlt - groudAlt)
);
break;
case FlightTaskType.FlyTo:
if (_flightTaskManager.Tasks[j].SingleCopterInfos[i].IsChangeSpeed)
{
missions.Add(Mission.CreateChangeSpeedMission(
_flightTaskManager.Tasks[j].SingleCopterInfos[i].LevelSpeed,
_flightTaskManager.Tasks[j].SingleCopterInfos[i].UpSpeed,
_flightTaskManager.Tasks[j].SingleCopterInfos[i].DownSpeed)
switch (_flightTaskManager.Tasks[j].TaskType)
{
case FlightTaskType.TakeOff:
int takeofftime = (int)(_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetAlt / 2.5 + 5);
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
missions.Add(Mission.CreateTakeoffMission(
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TakeOffWaitTime,
_flightTaskManager.Tasks[j].TakeOffTime,
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLat - _flightTaskManager.OriginLat,
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLng - _flightTaskManager.OriginLng,
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetAlt - groudAlt)
);
break;
case FlightTaskType.FlyTo:
if (_flightTaskManager.Tasks[j].SingleCopterInfos[i].IsChangeSpeed)
{
missions.Add(Mission.CreateChangeSpeedMission(
_flightTaskManager.Tasks[j].SingleCopterInfos[i].LevelSpeed,
_flightTaskManager.Tasks[j].SingleCopterInfos[i].UpSpeed,
_flightTaskManager.Tasks[j].SingleCopterInfos[i].DownSpeed)
);
}
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
double Lat = _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLat - _flightTaskManager.OriginLat;
double Lng = _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLng - _flightTaskManager.OriginLng;
if (_flightTaskManager.Tasks[j].SingleCopterInfos[i].IsLandWaypoint)
{
Lat = 90;
Lng = 180;
}
missions.Add(Mission.CreateWaypointMission(
_flightTaskManager.Tasks[j].LoiterTime,
_flightTaskManager.Tasks[j].FlytoTime,
Lat,
Lng,
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt - groudAlt)
);
}
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
double Lat = _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLat - _flightTaskManager.OriginLat;
double Lng = _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLng - _flightTaskManager.OriginLng;
if (_flightTaskManager.Tasks[j].SingleCopterInfos[i].IsLandWaypoint)
{
Lat = 90;
Lng = 180;
}
break;
case FlightTaskType.Loiter:
break;
case FlightTaskType.ReturnToLand:
missions.Add(Mission.CreateReturnToLaunchMission());
break;
missions.Add(Mission.CreateWaypointMission(
_flightTaskManager.Tasks[j].LoiterTime,
_flightTaskManager.Tasks[j].FlytoTime,
Lat,
Lng,
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt - groudAlt)
);
break;
case FlightTaskType.Loiter:
break;
case FlightTaskType.ReturnToLand:
missions.Add(Mission.CreateReturnToLaunchMission());
break;
case FlightTaskType.Land:
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
case FlightTaskType.Land:
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
missions.Add(Mission.CreateLandMission(
_flightTaskManager.Tasks[j].SingleCopterInfos[i].LandWaitTime)
);
break;
default:
break;
missions.Add(Mission.CreateLandMission(
_flightTaskManager.Tasks[j].SingleCopterInfos[i].LandWaitTime)
);
break;
default:
break;
}
}
bool result = await _commModuleManager.WriteMissionListAsync(short.Parse(_copterManager.Copters[i].Id), missions);
//CommWriteMissinState state = new CommWriteMissinState(result);
//_commModuleManager.missionWriteState.Add(int.Parse(_copterManager.Copters[i].Id), state);
if (!result)
{
Message.Show($"飞机：{_copterManager.Copters[i].Id} 通信模块传输失败!");
}
else
{
Message.Show($"飞机：{_copterManager.Copters[i].Id} 通信模块传输完成!");
}
await Task.Delay(500).ConfigureAwait(false);
}
bool result = await _commModuleManager.WriteMissionListAsync(short.Parse(_copterManager.Copters[i].Id), missions);
//CommWriteMissinState state = new CommWriteMissinState(result);
//_commModuleManager.missionWriteState.Add(int.Parse(_copterManager.Copters[i].Id), state);
if (!result)
catch (Exception ex)
{
Message.Show($"飞机：{_copterManager.Copters[i].Id} 通信模块传输失败!");
Message.Show(ex.ToString());
}
else
{
Message.Show($"飞机：{_copterManager.Copters[i].Id} 通信模块传输完成!");
}
await Task.Delay(500).ConfigureAwait(false);
}
private List<IMission> CreateLEDMissions(IEnumerable<LEDInfo> LEDInfos)

39
Plane.FormationCreator/ViewModels/MainViewModel.cs
View File

@ -206,6 +206,8 @@ namespace Plane.FormationCreator.ViewModels
{
var groupObject = _groupManager.ExportGroups();
var locateObject = ExportLocate();
object obj = new
{
groups = groupObject,
@ -243,6 +245,21 @@ namespace Plane.FormationCreator.ViewModels
return locateList;
}
//导出航点 单位米
private List<object> ExportLocateToMeter()
{
List<object> locateList = new List<object>();
foreach (var copter in _copterManager.Copters)
{
double[] locate = new double[3];
locate[0] = copter.Latitude - _flightTaskManager.OriginLat;
locate[1] = copter.Longitude - _flightTaskManager.OriginLng;
locate[2] = copter.GroundAlt;
locateList.Add(locate);
}
return locateList;
}
private int _txtStarindex = 0;
public int txtStarindex
@ -301,6 +318,8 @@ namespace Plane.FormationCreator.ViewModels
taskinfo = importInfo;
}
//int task Newtonsoft.Json.Linq.JArray
if ((txtStarindex == 0) && (txtendindex == 0))
{
@ -372,9 +391,7 @@ namespace Plane.FormationCreator.ViewModels
};
if (dialog.ShowDialog() == true)
{
var importText = File.ReadAllText(dialog.FileName);
dynamic importInfo = JsonConvert.DeserializeObject(importText);
dynamic taskinfo = null;
if (importInfo is Newtonsoft.Json.Linq.JObject)
@ -396,13 +413,23 @@ namespace Plane.FormationCreator.ViewModels
private void ImportCoptersLocate(dynamic coptersLocate)
{
int index = 0;
foreach (var locate in coptersLocate)
//if (Newtonsoft.Json.Linq.JArray)
try
{
_copterManager.Copters[index].GroundAlt = Convert.ToSingle(locate[2]);
index++;
int index = 0;
foreach (var locate in coptersLocate)
{
_copterManager.Copters[index].GroundAlt = Convert.ToSingle(locate[2]);
index++;
}
}
catch (Exception)
{
}
}
private ICommand _ClearLogs;

3
Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs
View File

@ -565,7 +565,8 @@ namespace Plane.FormationCreator.ViewModels
{
return _OptimizeRouteCommand ?? (_OptimizeRouteCommand = new RelayCommand<int>(async =>
{
_flightTaskManager.OptimizeRoute2();
//_flightTaskManager.OptimizeRoute2();
_flightTaskManager.OptimizeRouteNew();
}));
}
}

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

@ -80,7 +80,7 @@
<Button Content="导入航点" Command="{Binding ImportWayPointCommand}"
Visibility="Collapsed"/>
<Button Content="优化路线" Command="{Binding OptimizeRouteCommand}"
Visibility="Collapsed"/>
/>
<Button Content="导入航点" Command="{Binding ImportBlenderWayPointCommand}"/>
<Button Content="导出航点" Command="{Binding ExportWayPointCommand}"/>
</StackPanel>