From 54a20e79e65d411ba94f0f8f7c7f430e1677eee2 Mon Sep 17 00:00:00 2001 From: zxd Date: Wed, 8 Aug 2018 15:35:01 +0800 Subject: [PATCH] =?UTF-8?q?=E7=94=B5=E5=8E=8B=E4=BC=98=E5=8C=96=E3=80=81?= =?UTF-8?q?=E9=97=B4=E9=9A=94=E9=80=89=E4=B8=AD=E3=80=81=E8=88=AA=E7=82=B9?= =?UTF-8?q?=E5=81=8F=E7=A7=BB=E4=BC=98=E5=8C=96=E3=80=81=E5=8E=BB=E6=8E=89?= =?UTF-8?q?=E9=98=B4=E5=BD=B1=E4=B8=BA=E4=BC=98=E5=8C=96=E9=80=9F=E5=BA=A6?= =?UTF-8?q?=E3=80=81=E5=AF=BC=E5=85=A5=E8=88=AA=E7=82=B9=E3=80=81=E4=BC=98?= =?UTF-8?q?=E5=8C=96=E8=B7=AF=E7=BA=BF(=E7=9B=AE=E5=89=8D=E6=97=A0?= =?UTF-8?q?=E6=B3=95=E4=BD=BF=E7=94=A8)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Formation/FlightTaskManager.cs | 141 ++++++++++++++++ .../ViewModels/ConnectViewModel.cs | 53 +++++- .../ViewModels/ControlPanelViewModel.cs | 34 ++-- .../ViewModels/CopterListViewModel.cs | 48 +++--- .../ViewModels/ModifyTaskViewModel.cs | 33 +++- .../Views/ConnectWindow.xaml | 21 ++- .../Views/CopterInfoView.xaml | 2 +- .../Views/CopterListView.xaml | 3 + Plane.FormationCreator/Views/MapView.xaml | 8 +- Plane.FormationCreator/Views/MapView.xaml.cs | 20 ++- .../Views/MapView_CopterDrawing.cs | 156 ++++++++++++++---- .../Views/ModifyTaskView.xaml | 7 +- 12 files changed, 444 insertions(+), 82 deletions(-) diff --git a/Plane.FormationCreator/Formation/FlightTaskManager.cs b/Plane.FormationCreator/Formation/FlightTaskManager.cs index 668b80a..420ab09 100644 --- a/Plane.FormationCreator/Formation/FlightTaskManager.cs +++ b/Plane.FormationCreator/Formation/FlightTaskManager.cs @@ -878,6 +878,147 @@ namespace Plane.FormationCreator.Formation i++; } } + + 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 recordLatLng = new Dictionary(); + for (int i = 0; i < _copterManager.Copters.Count; i++) + { + var curinfo = SelectedTask.SingleCopterInfos[i]; + 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)); + + 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; + + Dictionary distanceDic = new Dictionary(); + for (int i = 0; i < _copterManager.Copters.Count; i++) + { + var curinfo = SelectedTask.SingleCopterInfos[i]; + var lastInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i]; + + + double distance1 = GeographyUtils.CalcDistance(CenterLat, CenterLng, 1, + curinfo.TargetLat, curinfo.TargetLng, 1); + //int[] nums = new int[] { SelectedTaskIndex, i}; + int nums = SelectedTaskIndex << 16 ^ i; + distanceDic.Add(nums, distance1); + double distance2 = GeographyUtils.CalcDistance(CenterLat, CenterLng, 1, + lastInfo.TargetLat, lastInfo.TargetLng, 1); + nums = (SelectedTaskIndex - 1) << 16 ^ i; + distanceDic.Add(nums, distance2); + } + distanceDic = distanceDic.OrderByDescending(o => o.Value).ToDictionary(p => p.Key, o => o.Value); + + while (distanceDic.Count > 0) + { + KeyValuePair kv = distanceDic.First(); + int taskIndex = kv.Key >> 16; + int copterID = kv.Key & 0xffff; + var curInfo = Tasks[taskIndex].SingleCopterInfos[copterID]; + if (taskIndex == SelectedTaskIndex) + { + double mindistance = double.MaxValue; + 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) + { + mindistance = distance; + index = i; + } + } + Tasks[taskIndex].SingleCopterInfos[index].TargetLat = recordLatLng[copterID].Lat; + Tasks[taskIndex].SingleCopterInfos[index].TargetLng = recordLatLng[copterID].Lng; + distanceDic.Remove(kv.Key); + distanceDic.Remove(( taskIndex - 1) << 16 ^ index ); + } + else + { + double mindistance = double.MaxValue; + 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) + { + mindistance = distance; + index = i; + } + } + Tasks[taskIndex + 1].SingleCopterInfos[copterID].TargetLat = recordLatLng[index].Lat; + Tasks[taskIndex + 1].SingleCopterInfos[copterID].TargetLng = recordLatLng[index].Lng; + distanceDic.Remove(kv.Key); + distanceDic.Remove(( taskIndex + 1) << 16 ^ index ); + } + + } + + } + public void OptimizeRoute() + { + Dictionary distanceDic = new Dictionary(); + Dictionary recordLatLng = new Dictionary(); + for (int i = 0; i < SelectedTask.SingleCopterInfos.Count; i++) + { + var curinfo = SelectedTask.SingleCopterInfos[i]; + recordLatLng.Add(i, new LatLng(curinfo.TargetLat, curinfo.TargetLng)); + for (int j = 0; j < Tasks[SelectedTaskIndex - 1].SingleCopterInfos.Count; j++) + { + var lastInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j]; + + double distance = GeographyUtils.CalcDistance(curinfo.TargetLat, curinfo.TargetLng, curinfo.TargetLng, + lastInfo.TargetLat, lastInfo.TargetLng, lastInfo.TargetLng); + int[] nums = new int[] {i,j}; + distanceDic.Add(nums, distance); + } + } + distanceDic = distanceDic.OrderBy(o=>o.Value).ToDictionary(p=>p.Key,o=>o.Value); + List movedCopters = new List(); + List usedPoints = new List(); + foreach (KeyValuePair kv in distanceDic) + { + int moveCopterNum = kv.Key[1]; + int destNum = kv.Key[0]; + if (movedCopters.Contains(moveCopterNum) || usedPoints.Contains(destNum)) + continue; + SelectedTask.SingleCopterInfos[moveCopterNum].TargetLat = recordLatLng[destNum].Lat; + SelectedTask.SingleCopterInfos[moveCopterNum].TargetLng = recordLatLng[destNum].Lng; + movedCopters.Add(moveCopterNum); + usedPoints.Add(destNum); + } + } + + public void ImportWaypoint(string tasksText) + { + dynamic jsonfile = JsonConvert.DeserializeObject(tasksText); + dynamic points = jsonfile.points; + for (int i = 0; i < SelectedTask.SingleCopterInfos.Count; i++) + { + MapManager _mapManager = Microsoft.Practices.ServiceLocation.ServiceLocator.Current.GetInstance(); + + var pointjson= points[i]; + System.Windows.Point point = new System.Windows.Point((int)pointjson.x, (int)pointjson.y); + Microsoft.Maps.MapControl.WPF.Location loc = _mapManager.MapView.map.ViewportPointToLocation(point); + SelectedTask.SingleCopterInfos[i].TargetLat = loc.Latitude; + SelectedTask.SingleCopterInfos[i].TargetLng = loc.Longitude; + } + } + // 导入任务 public void ImportTasks(string tasksText) { diff --git a/Plane.FormationCreator/ViewModels/ConnectViewModel.cs b/Plane.FormationCreator/ViewModels/ConnectViewModel.cs index 37a7caf..5fa9bee 100644 --- a/Plane.FormationCreator/ViewModels/ConnectViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ConnectViewModel.cs @@ -152,6 +152,32 @@ namespace Plane.FormationCreator.ViewModels } } + private ICommand _StateInquireCommand; + public ICommand StateInquireCommand + { + get + { + return _StateInquireCommand ?? (_StateInquireCommand = new RelayCommand(async () => + { + await WriteIdCommandAsync(0, 0x00); + } + )); + } + } + + private ICommand _CommDataAsync; + public ICommand CommDataAsync + { + get + { + return _CommDataAsync ?? (_CommDataAsync = new RelayCommand(async () => + { + await commModule.GenerateDataAsync((short)CopterNum); + } + )); + } + } + private ICommand _CommWriteMissionCommand; public ICommand CommWriteMissionCommand { @@ -300,14 +326,37 @@ namespace Plane.FormationCreator.ViewModels await commModule.GeneratePacketAsync((short)num, messageType); } - private async Task SendCommandAsync() { Protocols.MavComm.comm_set_mav_count mavCount = new Protocols.MavComm.comm_set_mav_count(); mavCount.mav_count = 200; - await commModule.GeneratePacketAsync(0, (byte)Protocols.MavComm.COMM_SET_MAV_COUNT, mavCount); + await commModule.GenerateDataAsync(0, (byte)Protocols.MavComm.COMM_SET_MAV_COUNT, mavCount); } + public async Task DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) + { + MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t(); + + req.target_system = 1; + req.target_component = 1; + + if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM) + { + req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL; + } + + req.command = (ushort)actionid; + + req.param1 = p1; + req.param2 = p2; + req.param3 = p3; + req.param4 = p4; + req.param5 = p5; + req.param6 = p6; + req.param7 = p7; + await Task.Delay(10); + return true; + } private async Task WriteCommandAsync(ICopter copter, List missions) { List mission_list = new List(); diff --git a/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs b/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs index d376051..d83e36f 100644 --- a/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs @@ -87,6 +87,8 @@ namespace Plane.FormationCreator.ViewModels } } + + private static readonly object locker = new object(); private ICommand _DetectionVoltage; public ICommand DetectionVoltage { @@ -94,26 +96,38 @@ namespace Plane.FormationCreator.ViewModels { return _DetectionVoltage ?? (_DetectionVoltage = new RelayCommand(async () => { + Message.Show("--------------开始检测电压--------------"); Dictionary dic_voltage = new Dictionary(); await Task.WhenAll(_copterManager.Copters.Select(async c => { float voltageSum = 0.0f; + string name = c.Name; + for (int i = 0; i < 5; i++) { + voltageSum += c.Voltage; await Task.Delay(1000).ConfigureAwait(false); } float voltageAverage = voltageSum / 5; - dic_voltage.Add(c.Name, voltageAverage); - })); - dic_voltage = dic_voltage.OrderByDescending(o => o.Value).ToDictionary(p => p.Key, o => o.Value); - Message.Show("--------------开始检测电压--------------"); - foreach (KeyValuePair kv in dic_voltage) - { - Message.Show(string.Format("{0} --> 5秒平均电压：{1}", kv.Key, kv.Value)); - await Task.Delay(5).ConfigureAwait(false); - } - Message.Show("--------------检测电压完成--------------"); + if (name != null && name != "") + { + lock(locker) + { + dic_voltage.Add(name, voltageAverage); + } + } + })).ConfigureAwait(false); + await Task.Run(async () => { + Dictionary dic_voltage_Order = dic_voltage.OrderByDescending(o => o.Value).ToDictionary(o => o.Key, o => o.Value); + + foreach (KeyValuePair kv in dic_voltage_Order) + { + Message.Show(string.Format("{0} --> 5秒平均电压：{1}", kv.Key, kv.Value)); + await Task.Delay(5).ConfigureAwait(false); + } + Message.Show(string.Format("----检测电压完成,检测总数：{0}----", dic_voltage.Count)); + }).ConfigureAwait(false); })); } } diff --git a/Plane.FormationCreator/ViewModels/CopterListViewModel.cs b/Plane.FormationCreator/ViewModels/CopterListViewModel.cs index e97405f..fe2ec93 100644 --- a/Plane.FormationCreator/ViewModels/CopterListViewModel.cs +++ b/Plane.FormationCreator/ViewModels/CopterListViewModel.cs @@ -88,6 +88,13 @@ namespace Plane.FormationCreator.ViewModels set { Set(nameof(IntervalNum), ref _IntervalNum, value); } } + private int _ContinuousNum; + public int ContinuousNum + { + get { return _ContinuousNum; } + set { Set(nameof(ContinuousNum), ref _ContinuousNum, value); } + } + private ICommand _IntervalSelectCoptersCommand; public ICommand IntervalSelectCoptersCommand { @@ -95,16 +102,21 @@ namespace Plane.FormationCreator.ViewModels { return _IntervalSelectCoptersCommand ?? (_IntervalSelectCoptersCommand = new RelayCommand(() => { - if (_copterManager.AcceptingControlCopters.Count() == 0 || IntervalNum == 0) + if (_copterManager.AcceptingControlCopters.Count() != 1 || IntervalNum == 0 || ContinuousNum == 0) return; - ICopter copter = _copterManager.AcceptingControlCopters.FirstOrDefault(); + ICopter copter = _copterManager.SelectedCopters.FirstOrDefault(); _copterManager.Select(null); int index = _copterManager.Copters.IndexOf(copter); _copterManager.shiftkeydown = true; - for (; index < _copterManager.Copters.Count; index += IntervalNum + 1) + for (; index < _copterManager.Copters.Count; index += IntervalNum) { - _copterManager.Select(_copterManager.Copters[index]); + for (int i = 0; i < ContinuousNum; i++) + { + if (index >= _copterManager.Copters.Count) break; + _copterManager.Select(_copterManager.Copters[index]); + index++; + } } _copterManager.shiftkeydown = false; @@ -122,9 +134,9 @@ namespace Plane.FormationCreator.ViewModels var center = _mapManager.Center; string id; - int colnum = 5; //自动生成列数=4 - float coldis = 3;//列相距5米 - float rowdis = 3;//行相距5米 + int colnum = 10; //自动生成列数=4 + float coldis = 5;//列相距5米 + float rowdis = 2.5f;//行相距5米 int currcol = 0; //当前列号 int currrow = 0; //当前行 Tuple colheadLatLng = new Tuple(0, 0); @@ -163,27 +175,15 @@ namespace Plane.FormationCreator.ViewModels _lastVirtualCopterLocation= new LatLng(targetLatLng.Item1, targetLatLng.Item2); } currcol++; - - - - - /* _lastVirtualCopterLocation = - _lastVirtualCopterLocation == null ? - new LatLng(center.Lat, center.Lng) : - new LatLng(_lastVirtualCopterLocation.Value.Lat, _lastVirtualCopterLocation.Value.Lng + 0.0001); - */ - - - - - - - - + /* _lastVirtualCopterLocation = + _lastVirtualCopterLocation == null ? + new LatLng(center.Lat, center.Lng) : + new LatLng(_lastVirtualCopterLocation.Value.Lat, _lastVirtualCopterLocation.Value.Lng + 0.0001); + */ diff --git a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs index 4edc64f..4083c5d 100644 --- a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs @@ -505,7 +505,39 @@ namespace Plane.FormationCreator.ViewModels })); } } + private ICommand _ImportWayPointCommand; + public ICommand ImportWayPointCommand + { + get + { + return _ImportWayPointCommand ?? (_ImportWayPointCommand = new RelayCommand(async => + { + var dialog = new OpenFileDialog + { + DefaultExt = "json", + Filter = "json文件|*.json" + }; + if (dialog.ShowDialog() == true) + { + var tasksText = File.ReadAllText(dialog.FileName); + _flightTaskManager.ImportWaypoint(tasksText); + + } + })); + } + } + private ICommand _OptimizeRouteCommand; + public ICommand OptimizeRouteCommand + { + get + { + return _OptimizeRouteCommand ?? (_OptimizeRouteCommand = new RelayCommand(async => + { + _flightTaskManager.OptimizeRoute2(); + })); + } + } private ICommand _LevelAlignmentCommand; public ICommand LevelAlignmentCommand @@ -1255,7 +1287,6 @@ public ICommand VerticlAlignmentCommand } if ((lng1 != 0) && (lng2 != 0)) { - distance = GeographyUtils.CalcDistance( lat1, lng1, alt1, lat2, lng2, alt2); diff --git a/Plane.FormationCreator/Views/ConnectWindow.xaml b/Plane.FormationCreator/Views/ConnectWindow.xaml index 60110bf..4627742 100644 --- a/Plane.FormationCreator/Views/ConnectWindow.xaml +++ b/Plane.FormationCreator/Views/ConnectWindow.xaml @@ -66,6 +66,7 @@ + @@ -108,7 +109,7 @@ IsProcessing="{Binding IsProcessing}" Command="{Binding Path=ConnectCommand}" CommandParameter="SerialPort" /> -