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

导入导出高度计算上摆放高度

Browse Source
This commit is contained in:
zxd 2019-04-08 17:55:01 +08:00
parent cce5722a52
commit 0f19e2addf
8 changed files with 144 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -79,16 +79,17 @@ namespace Plane.FormationCreator.Formation
return Math.Abs(copter.Heading - targetHeading) < 2;
}
public static bool IsTooCloseTo(this ICopter copter, ICopter copter2)
public static bool IsTooCloseTo(this ICopter copter, ICopter copter2, out double distance)
{
distance = Math.Round(copter.DistanceTo(copter2), 2) ;
//return false; //效果图用 不检测碰撞
if (copter.Altitude < 3 || copter2.Altitude < 3)
return false;
if ((copter.GpsFixType == GpsFixType.RTKFIXED) && (copter2.GpsFixType == GpsFixType.RTKFIXED))
return copter.DistanceTo(copter2) < RTKClosedDis; //最近距离0.5米
return distance < RTKClosedDis; //最近距离0.5米
else
return copter.DistanceTo(copter2) < GpsCloseDis; //最近距离2米
return distance < GpsCloseDis; //最近距离2米
}
}
}

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

@ -421,20 +421,30 @@ namespace Plane.FormationCreator.Formation
string[] lines = txt.Replace("\r\n", "\n").Split('\n');
if (lines.Length != _copterManager.Copters.Count)
return;
var lastTask = Tasks.LastOrDefault();
var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = true, FlytoTime = 10, LoiterTime = 10 };
int i = 0;
Dictionary<int, string[]> PointDic = new Dictionary<int, string[]>();
foreach (string line in lines)
{
string[] parameters = line.Split(' ');
string id = parameters[0];
string frame = parameters[1];
string x = parameters[2]; //左右 经度
string y = parameters[3]; //上下 高度
string z = parameters[4]; //前后 纬度
string[] point = new string[3];
point[0] = parameters[2]; //左右 经度
point[1] = parameters[3]; //上下 高度
point[2] = parameters[4]; //前后 纬度
PointDic.Add(Convert.ToInt32(id), point);
}
var lastTask = Tasks.LastOrDefault();
var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = true, FlytoTime = 10, LoiterTime = 10 };
for (int i = 0; i < PointDic.Count; i++)
{
string[] point = PointDic[i + 1];
//string frame = parameters[1];
string x = point[0]; //左右 经度
string y = point[1]; //上下 高度
string z = point[2]; //前后 纬度
Tuple<double, double> observationLatLng = null;
observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
OriginLat,
@ -449,10 +459,10 @@ namespace Plane.FormationCreator.Formation
Convert.ToSingle(z) / 100);
var thisSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
_copterManager.Copters[Convert.ToInt32(id)], new LatLng(observationLatLng.Item1 - OriginLat, observationLatLng.Item2 - OriginLng),
_copterManager.Copters[i], new LatLng(observationLatLng.Item1 - OriginLat, observationLatLng.Item2 - OriginLng),
Convert.ToSingle(y)/100, false);
newTask.SingleCopterInfos.Add(thisSingleCopterInfo);
i++;
}
Tasks.Add(newTask);
TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask });
@ -528,7 +538,7 @@ namespace Plane.FormationCreator.Formation
tagalt = (float)singleCopterInfoObj.targetAlt;
newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(
copter, new LatLng((double)singleCopterInfoObj.latOffset,
(double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (bool)singleCopterInfoObj.isLandWaypoint);
(double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt - copter.GroundAlt, (bool)singleCopterInfoObj.isLandWaypoint);
var jsonArray = singleCopterInfoObj.ledInfos as Newtonsoft.Json.Linq.JArray;
ObservableCollection<LEDInfo> ledList = jsonArray.ToObject<ObservableCollection<LEDInfo>>();
@ -660,6 +670,16 @@ namespace Plane.FormationCreator.Formation
{
var singleCopterInfoObj = singleCopterInfos[i];
takeOffTask.SingleCopterInfos[i].TakeOffWaitTime = (ushort)singleCopterInfoObj.waitTime;
if (singleCopterInfoObj.ledInfos!= null)
{
var jsonArray = singleCopterInfoObj.ledInfos as Newtonsoft.Json.Linq.JArray;
ObservableCollection<LEDInfo> ledList = jsonArray.ToObject<ObservableCollection<LEDInfo>>();
foreach (LEDInfo info in ledList)
{
takeOffTask.SingleCopterInfos[i].AddLEDInfo(info);
}
}
//Message.Show(((ushort)singleCopterInfoObj.waitTime).ToString());
}
}
@ -832,7 +852,8 @@ namespace Plane.FormationCreator.Formation
{
return new
{
waitTime = info.TakeOffWaitTime
waitTime = info.TakeOffWaitTime,
ledInfos = info.LEDInfos
};
})
};
@ -850,7 +871,7 @@ namespace Plane.FormationCreator.Formation
{
latOffset = offset.Lat,
lngOffset = offset.Lng,
targetAlt = info.TargetAlt,
targetAlt = info.TargetAlt + info.Copter.GroundAlt, //导出时加上地面摆放高度，导入后若没有摆放高度，模拟就会清楚的看到高度错误
//showLED = info.FlytoShowLED,
ledInfos = info.LEDInfos,
isLandWaypoint = info.IsLandWaypoint,
@ -1210,10 +1231,10 @@ namespace Plane.FormationCreator.Formation
}
}
private void ImportCoptersLocate(dynamic coptersLocate)
{
}
// private void ImportCoptersLocate(dynamic coptersLocate)
// {
//
// }
private void ImportGroupsInfo(dynamic coptersLocate)
{
@ -1417,15 +1438,15 @@ namespace Plane.FormationCreator.Formation
// bool RTLFlag = false;
//if ( (CurrentRunningTaskIndex != 0) && !RTLFlag)
{
double distance;
foreach (var copter in _copterManager.Copters)
{
foreach (var anotherCopter in _copterManager.Copters)
{
if (copter != anotherCopter && copter.IsTooCloseTo(anotherCopter))
if (copter != anotherCopter && copter.IsTooCloseTo(anotherCopter, out distance))
{
//Pause();
Message.Show($"{copter.Name} 与 {anotherCopter.Name} 距离过近，已中止任务。");
Message.Show($"{copter.Name} 与 {anotherCopter.Name} 距离过近，间距{distance}米。");
// Alert.Show($"{copter.Name} 与 {anotherCopter.Name} 距离过近，已中止任务。");
//return;
}

4
Plane.FormationCreator/MainWindow.xaml
View File

@ -103,8 +103,8 @@
<TextBox VerticalAlignment="Bottom" Width="40"
Text="{Binding txtendindex, UpdateSourceTrigger=PropertyChanged}"/>
</StackPanel>
<Button BorderThickness="1" BorderBrush="#FFFFFF" Content="导入摆放位置" Margin="25,8,0,8" Grid.Row="2"
Command="{Binding ImportTasksCommand}"/>
<Button BorderThickness="1" BorderBrush="#FFFFFF" Content="导入分组信息" Margin="25,8,0,8" Grid.Row="2"
Command="{Binding ImportGroupCommand}"/>
</Grid>
</MenuItem>
</Menu>

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

@ -441,6 +441,37 @@ namespace Plane.FormationCreator.ViewModels
}
}
private float _LatOffset = 18.4023833f;
public float LatOffset
{
get { return _LatOffset; }
set { Set(nameof(LatOffset), ref _LatOffset, value); }
}
private float _LngOffset = 109.799761f;
public float LngOffset
{
get { return _LngOffset; }
set { Set(nameof(LngOffset), ref _LngOffset, value); }
}
private ICommand _RLTOffsetCommand;
public ICommand RLTOffsetCommand
{
get
{
return _RLTOffsetCommand ?? (_RLTOffsetCommand = new RelayCommand(async () =>
{
Message.Show($"偏移Lat = {LatOffset}，Lng = {LngOffset}");
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
await _commModuleManager.RLTOffsetAsync(LatOffset, LngOffset, _copterManager.AcceptingControlCopters);
}));
}
}
private ICommand _LockCommand;
public ICommand LockCommand
{

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

@ -175,7 +175,7 @@ namespace Plane.FormationCreator.ViewModels
}
private bool _OnlyImpotWaypointS = true;
private bool _OnlyImpotWaypointS = false;
public bool OnlyImpotWaypointS
{
get { return _OnlyImpotWaypointS; }
@ -289,7 +289,7 @@ namespace Plane.FormationCreator.ViewModels
taskinfo = importInfo.tasks;
if (importInfo.locate != null)
{
//ImportCoptersLocate(importInfo.locate);
ImportCoptersLocate(importInfo.locate);
}
if (importInfo.groups != null)
{
@ -322,6 +322,53 @@ namespace Plane.FormationCreator.ViewModels
}
}
private ICommand _ImportGroupCommand;
public ICommand ImportGroupCommand
{
get
{
return _ImportGroupCommand ?? (_ImportGroupCommand = new RelayCommand(() =>
{
var dialog = new OpenFileDialog
{
DefaultExt = "fcg",
Filter = "编队飞行任务 (*.fcg)|*.fcg"
};
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)
{
taskinfo = importInfo.tasks;
if (importInfo.groups != null)
{
_groupManager.ImportGroupsInfo(importInfo.groups);
}
}
else if (importInfo is Newtonsoft.Json.Linq.JArray)
{
taskinfo = importInfo;
}
}
}));
}
}
private void ImportCoptersLocate(dynamic coptersLocate)
{
int index = 0;
foreach (var locate in coptersLocate)
{
_copterManager.Copters[index].GroundAlt = Convert.ToSingle(locate[2]);
index++;
}
}
private ICommand _ClearLogs;
public ICommand ClearLogs
{

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

@ -1219,7 +1219,7 @@ public ICommand VerticlAlignmentCommand
(_flightTaskManager.Tasks[i].TaskType == FlightTaskType.ReturnToLand))
continue;
for (int j = 0; j < _flightTaskManager.Tasks[i].SingleCopterInfos.Count; j++)
{
_flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetAlt += Modialtvalue;
@ -1229,7 +1229,6 @@ public ICommand VerticlAlignmentCommand
lowtask = i;
lowCopter = j;
}
}
}
Alert.Show("指定步骤任务高度已改变"+ Modialtvalue + "米，步骤["+ lowtask + "],ID["+

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

@ -84,6 +84,12 @@
Command="{Binding WriteMissionSingleCommand}" />
<Button Content="统计模块"
Command="{Binding DetectionCommModuleVersion}" />
<Label>Lat</Label>
<TextBox Text="{Binding LatOffset}" Width="50"/>
<Label>Lng</Label>
<TextBox Text="{Binding LngOffset}" Width="50"/>
<Button Content="设置返航点"
Command="{Binding RLTOffsetCommand}" />
</WrapPanel>
<Separator/>
<WrapPanel>

10
Plane.FormationCreator/Views/MapView.xaml.cs
View File

@ -230,7 +230,15 @@ namespace Plane.FormationCreator.Views
seekPoint.Y > leftTopPoint.Y && seekPoint.Y < leftTopPoint.Y + rectangle.Height)
{
selectedCount++;
_copterManager.Select(taskCopterInfo.Copter);
if (_copterManager.ShowCopter.Count != 0 )
{
if (_copterManager.ShowCopter.Contains(taskCopterInfo.Copter))
{
_copterManager.Select(taskCopterInfo.Copter);
}
}
else
_copterManager.Select(taskCopterInfo.Copter);
}
}
_copterManager.shiftkeydown = false;