commit 3c52409203c7f44c18fada4fa4d574db1320976a Author: pxzleo Date: Mon Feb 27 02:06:48 2017 +0800 原始导入 diff --git a/Plane.FormationCreator.sln b/Plane.FormationCreator.sln new file mode 100644 index 0000000..adcb603 --- /dev/null +++ b/Plane.FormationCreator.sln @@ -0,0 +1,74 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 14 +VisualStudioVersion = 14.0.25420.1 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Plane.FormationCreator", "Plane.FormationCreator\Plane.FormationCreator.csproj", "{61E2F31E-220A-4E3F-A64D-F7CDC2135008}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Plane.Windows", "..\Plane.Libraries\Plane.Windows\Plane.Windows.csproj", "{06848293-9B17-4068-9B35-44D0ED713CD4}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Plane.Reflection", "..\Plane.Libraries\Plane.Reflection\Plane.Reflection.csproj", "{98755514-C2E9-4ABE-8A25-007804577558}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Plane.Windows.Messages", "..\Plane.Libraries\Plane.Windows.Messages\Plane.Windows.Messages.csproj", "{413C18E2-235F-4E15-B5C1-633657DE5D7A}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Plane.Logging", "..\Plane.Libraries\Plane.Logging\Plane.Logging.csproj", "{9C2CAFDA-CF1D-4565-B797-398376FCD346}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Plane", "..\Plane.Libraries\Plane\Plane.csproj", "{6CCE2AEB-3B38-4C00-B32D-433A990AE2AD}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "PlaneGcsSdk_Private_NET46", "..\Plane.Sdk3\PlaneGcsSdk_Private_NET46\PlaneGcsSdk_Private_NET46.csproj", "{0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "PlaneGcsSdk.Contract_Private", "..\Plane.Sdk3\PlaneGcsSdk.Contract_Private\PlaneGcsSdk.Contract_Private.csproj", "{47141894-ECE3-48CA-8DCF-CA751BDA231E}" +EndProject +Project("{D954291E-2A0B-460D-934E-DC6B0785DB48}") = "PlaneGcsSdk_Shared", "..\Plane.Sdk3\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.shproj", "{2BE393DC-21A4-48B3-83FD-F21CBE8B038B}" +EndProject +Project("{D954291E-2A0B-460D-934E-DC6B0785DB48}") = "PlaneGcsSdk.Contract_Shared", "..\Plane.Sdk3\PlaneGcsSdk.Contract_Shared\PlaneGcsSdk.Contract_Shared.shproj", "{695733D7-99FF-4707-8C89-474E949CADCB}" +EndProject +Global + GlobalSection(SharedMSBuildProjectFiles) = preSolution + ..\Plane.Sdk3\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems*{0111eb6e-72e3-499c-a3ba-022f5bbc4caf}*SharedItemsImports = 4 + ..\Plane.Sdk3\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems*{2be393dc-21a4-48b3-83fd-f21cbe8b038b}*SharedItemsImports = 13 + ..\Plane.Sdk3\PlaneGcsSdk.Contract_Shared\PlaneGcsSdk.Contract_Shared.projitems*{47141894-ece3-48ca-8dcf-ca751bda231e}*SharedItemsImports = 4 + ..\Plane.Sdk3\PlaneGcsSdk.Contract_Shared\PlaneGcsSdk.Contract_Shared.projitems*{695733d7-99ff-4707-8c89-474e949cadcb}*SharedItemsImports = 13 + EndGlobalSection + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {61E2F31E-220A-4E3F-A64D-F7CDC2135008}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {61E2F31E-220A-4E3F-A64D-F7CDC2135008}.Debug|Any CPU.Build.0 = Debug|Any CPU + {61E2F31E-220A-4E3F-A64D-F7CDC2135008}.Release|Any CPU.ActiveCfg = Release|Any CPU + {61E2F31E-220A-4E3F-A64D-F7CDC2135008}.Release|Any CPU.Build.0 = Release|Any CPU + {06848293-9B17-4068-9B35-44D0ED713CD4}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {06848293-9B17-4068-9B35-44D0ED713CD4}.Debug|Any CPU.Build.0 = Debug|Any CPU + {06848293-9B17-4068-9B35-44D0ED713CD4}.Release|Any CPU.ActiveCfg = Release|Any CPU + {06848293-9B17-4068-9B35-44D0ED713CD4}.Release|Any CPU.Build.0 = Release|Any CPU + {98755514-C2E9-4ABE-8A25-007804577558}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {98755514-C2E9-4ABE-8A25-007804577558}.Debug|Any CPU.Build.0 = Debug|Any CPU + {98755514-C2E9-4ABE-8A25-007804577558}.Release|Any CPU.ActiveCfg = Release|Any CPU + {98755514-C2E9-4ABE-8A25-007804577558}.Release|Any CPU.Build.0 = Release|Any CPU + {413C18E2-235F-4E15-B5C1-633657DE5D7A}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {413C18E2-235F-4E15-B5C1-633657DE5D7A}.Debug|Any CPU.Build.0 = Debug|Any CPU + {413C18E2-235F-4E15-B5C1-633657DE5D7A}.Release|Any CPU.ActiveCfg = Release|Any CPU + {413C18E2-235F-4E15-B5C1-633657DE5D7A}.Release|Any CPU.Build.0 = Release|Any CPU + {9C2CAFDA-CF1D-4565-B797-398376FCD346}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {9C2CAFDA-CF1D-4565-B797-398376FCD346}.Debug|Any CPU.Build.0 = Debug|Any CPU + {9C2CAFDA-CF1D-4565-B797-398376FCD346}.Release|Any CPU.ActiveCfg = Release|Any CPU + {9C2CAFDA-CF1D-4565-B797-398376FCD346}.Release|Any CPU.Build.0 = Release|Any CPU + {6CCE2AEB-3B38-4C00-B32D-433A990AE2AD}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {6CCE2AEB-3B38-4C00-B32D-433A990AE2AD}.Debug|Any CPU.Build.0 = Debug|Any CPU + {6CCE2AEB-3B38-4C00-B32D-433A990AE2AD}.Release|Any CPU.ActiveCfg = Release|Any CPU + {6CCE2AEB-3B38-4C00-B32D-433A990AE2AD}.Release|Any CPU.Build.0 = Release|Any CPU + {0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}.Debug|Any CPU.Build.0 = Debug|Any CPU + {0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}.Release|Any CPU.ActiveCfg = Release|Any CPU + {0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}.Release|Any CPU.Build.0 = Release|Any CPU + {47141894-ECE3-48CA-8DCF-CA751BDA231E}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {47141894-ECE3-48CA-8DCF-CA751BDA231E}.Debug|Any CPU.Build.0 = Debug|Any CPU + {47141894-ECE3-48CA-8DCF-CA751BDA231E}.Release|Any CPU.ActiveCfg = Release|Any CPU + {47141894-ECE3-48CA-8DCF-CA751BDA231E}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/Plane.FormationCreator/App.config b/Plane.FormationCreator/App.config new file mode 100644 index 0000000..f5162f7 --- /dev/null +++ b/Plane.FormationCreator/App.config @@ -0,0 +1,26 @@ + + + + +
+ + + + + + + + + + + + + + + + + 192.168.1.50 + + + + diff --git a/Plane.FormationCreator/App.xaml b/Plane.FormationCreator/App.xaml new file mode 100644 index 0000000..ab593cc --- /dev/null +++ b/Plane.FormationCreator/App.xaml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Plane.FormationCreator/App.xaml.cs b/Plane.FormationCreator/App.xaml.cs new file mode 100644 index 0000000..d089e63 --- /dev/null +++ b/Plane.FormationCreator/App.xaml.cs @@ -0,0 +1,153 @@ +using Plane.Communication; +using Plane.Copters; +using Plane.FormationCreator.Formation; +using Plane.Logging; +using Plane.Windows; +using Plane.Windows.Messages; +using GalaSoft.MvvmLight.Ioc; +using Microsoft.Practices.ServiceLocation; +using System; +using System.Collections.Generic; +using System.Configuration; +using System.Data; +using System.Diagnostics; +using System.Globalization; +using System.Linq; +using System.Net; +using System.Reflection; +using System.Threading; +using System.Threading.Tasks; +using System.Windows; + +namespace Plane.FormationCreator +{ + /// + /// Interaction logic for App.xaml + /// + public partial class App : Application + { + public App() + { + Thread.CurrentThread.CurrentUICulture = CultureInfo.GetCultureInfo("en-US"); + + ServiceLocatorConfigurer.Instance.Configure(); + + _logger = ServiceLocator.Current.GetInstance(); + _copterManager = ServiceLocator.Current.GetInstance(); + _formationController = ServiceLocator.Current.GetInstance(); + + AppDomain.CurrentDomain.AssemblyResolve += (s, e) => + { + var simpleName = new AssemblyName(e.Name).Name; + if (simpleName.EndsWith(".resources")) + { + return null; + } + return LoadAssembly(simpleName); + }; + this.DispatcherUnhandledException += async (s, e) => + { + _logger.Log(e.Exception); + await _formationController.AllStop(); + TcpServerConnectionManager.Instance.StopListening(); + UdpServerConnectionManager.Instance.StopReceiving(); + }; + this.Exit += (s, e) => + { + TcpServerConnectionManager.Instance.StopListening(); + UdpServerConnectionManager.Instance.StopReceiving(); + }; + //new Test().Prepare().Run(); + } + + private ILogger _logger; + private CopterManager _copterManager; + private FormationController _formationController; + + private Assembly LoadAssembly(string simpleName) + { + String resourceName = this.GetType().Namespace + ".AssemblyLoadingAndReflection." + simpleName + ".dll"; + using (var stream = Assembly.GetExecutingAssembly().GetManifestResourceStream(resourceName)) + { + Byte[] assemblyData = new Byte[stream.Length]; + stream.Read(assemblyData, 0, assemblyData.Length); + return Assembly.Load(assemblyData); + } + } + + protected override void OnStartup(StartupEventArgs e) + { + base.OnStartup(e); + + var md = Resources.MergedDictionaries; + md.Add(new ResourceDictionary { Source = new Uri("pack://application:,,,/MahApps.Metro;component/Styles/Controls.xaml") }); + md.Add(new ResourceDictionary { Source = new Uri("pack://application:,,,/MahApps.Metro;component/Styles/Fonts.xaml") }); + md.Add(new ResourceDictionary { Source = new Uri("pack://application:,,,/MahApps.Metro;component/Styles/Colors.xaml") }); + md.Add(new ResourceDictionary { Source = new Uri("pack://application:,,,/MahApps.Metro;component/Styles/VS/Colors.xaml") }); + md.Add(new ResourceDictionary { Source = new Uri("pack://application:,,,/MahApps.Metro;component/Styles/VS/Styles.xaml") }); + + md.Add(new ResourceDictionary { Source = new Uri("pack://application:,,,/Plane.Windows.Messages;component/Styles.xaml") }); + + md.Add(new ResourceDictionary { Source = new Uri("pack://application:,,,/Styles/Colors.xaml") }); + md.Add(new ResourceDictionary { Source = new Uri("pack://application:,,,/Styles.xaml") }); +/* + new UpdateChecker("http://dl.Plane.com/tools/ver.php?app=FormationCreator", _logger) + .CheckAsync(ver => + { + var currentVersion = this.GetType().Assembly.GetName().Version; + if (currentVersion < ver + && Alert.Show("检测到新版本，请下载。", "更新提示", MessageBoxButton.YesNo) == MessageBoxResult.Yes) + { + Process.Start("http://dl.Plane.com/tools/FormationCreatorSetup.exe"); + this.Shutdown(); + } + }); +*/ + MainWindow = new MainWindow(); + MainWindow.Show(); + + TcpServerConnectionManager.Instance.ConnectionEstablished += ConnectionManager_ConnectionEstablished; + if (!TcpServerConnectionManager.Instance.StartListening()) + { + Alert.Show("网络连接不正常，无法启动监听。"); + } + UdpServerConnectionManager.Instance.ExceptionThrown += (sender, e1) => + { + _logger.Log(e1.Exception); + }; + UdpServerConnectionManager.Instance.ConnectionEstablished += ConnectionManager_ConnectionEstablished; + UdpServerConnectionManager.Instance.StartReceiving(); + } + + private async Task AddOrUpdateCopter(string ip, IConnection Connection) + { + var copters = _copterManager.Copters; + var copterStatus = _copterManager.CopterStatus; + + var copter = copters.FirstOrDefault(c => c.Id == ip); + if (copter == null) + { + await Dispatcher.BeginInvoke(new Action(() => + { + copter = new Copter(Connection, SynchronizationContext.Current) + { + Id = ip, + Name = ip.Substring(ip.LastIndexOf('.') + 1) + }; + copters.Add(copter); + copterStatus.Add(false); + })); + } + else + { + copter.Connection = Connection; + } + await copter.ConnectAsync().ConfigureAwait(false); + } + + private async void ConnectionManager_ConnectionEstablished(object sender, ConnectionEstablishedEventArgs e) + { + await AddOrUpdateCopter(e.RemoteAddress, e.Connection); + } + } +} diff --git a/Plane.FormationCreator/AppConfig.cs b/Plane.FormationCreator/AppConfig.cs new file mode 100644 index 0000000..8311b30 --- /dev/null +++ b/Plane.FormationCreator/AppConfig.cs @@ -0,0 +1,105 @@ +using Plane.FormationCreator.Formation; +using Newtonsoft.Json; +using System; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator +{ + class AppConfig + { + public AppConfig(MapManager mapMannager) + { + _mapManager = mapMannager; + + _dirPath = Path.Combine( + Environment.GetFolderPath(Environment.SpecialFolder.LocalApplicationData), + "Plane", + AppDomain.CurrentDomain.FriendlyName, + "Config" + ); + _filePath = Path.Combine( + _dirPath, + "Config.json" + ); + + string json; + try + { + json = File.ReadAllText(_filePath); + } + catch + { + json = "{}"; + } + _config = JsonConvert.DeserializeObject(json); + + App.Current.Exit += (sender, e) => + { + this.Center = _mapManager.Center; + this.ZoomLevel = _mapManager.MapView.map.ZoomLevel; + this.Save(); + }; + } + + string _dirPath; + string _filePath; + + dynamic _config; + + MapManager _mapManager; + + public LatLng Center + { + get + { + try + { + var text = (string)_config.Center; + var fields = text.Split(','); + return new LatLng(double.Parse(fields[0]), double.Parse(fields[1])); + } + catch + { + return new LatLng(40.160491667, 116.23426389); + } + } + set + { + _config.Center = $"{value.Lat},{value.Lng}"; + } + } + + public double ZoomLevel + { + get + { + try + { + var text = (string)_config.ZoomLevel; + return double.Parse(text); + } + catch + { + return 19; + } + } + set + { + _config.ZoomLevel = value; + } + } + + public void Save() + { + if (!Directory.Exists(_dirPath)) + { + Directory.CreateDirectory(_dirPath); + } + File.WriteAllText(_filePath, JsonConvert.SerializeObject(_config)); + } + } +} diff --git a/Plane.FormationCreator/AppEx.cs b/Plane.FormationCreator/AppEx.cs new file mode 100644 index 0000000..5dea263 --- /dev/null +++ b/Plane.FormationCreator/AppEx.cs @@ -0,0 +1,53 @@ +using Plane.FormationCreator.Formation; +using GalaSoft.MvvmLight; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator +{ + public class AppEx : ObservableObject + { + public static AppEx Current { get; } = new AppEx(); + + private AppEx() { } + + private bool _IsInFastMode = true; + public bool IsInFastMode + { + get { return _IsInFastMode; } + set { Set(nameof(IsInFastMode), ref _IsInFastMode, value); } + } + + private AppMode _AppMode = AppMode.ModifyingTask; + public AppMode AppMode + { + get { return _AppMode; } + set + { + bool changed = _AppMode != value; + var oldItem = _AppMode; + Set(nameof(AppMode), ref _AppMode, value); + if (changed) + { + AppModeChanged?.Invoke(this, new PropertyChangedEventArgs + { + OldItem = oldItem, + NewItem = value + }); + } + } + } + + private bool _ShowModifyTaskView = true; + public bool ShowModifyTaskView + { + get { return _ShowModifyTaskView; } + set { Set(nameof(ShowModifyTaskView), ref _ShowModifyTaskView, value); } + } + + public event EventHandler> AppModeChanged; + } +} diff --git a/Plane.FormationCreator/ClassDiagram1.cd b/Plane.FormationCreator/ClassDiagram1.cd new file mode 100644 index 0000000..7b89419 --- /dev/null +++ b/Plane.FormationCreator/ClassDiagram1.cd @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/Plane.FormationCreator/Converters/AppModeToVisibilityConverter.cs b/Plane.FormationCreator/Converters/AppModeToVisibilityConverter.cs new file mode 100644 index 0000000..ff69e33 --- /dev/null +++ b/Plane.FormationCreator/Converters/AppModeToVisibilityConverter.cs @@ -0,0 +1,40 @@ +using Plane.FormationCreator.Formation; +using Plane.FormationCreator.Views; +using System; +using System.Collections.Generic; +using System.Globalization; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows; +using System.Windows.Data; + +namespace Plane.FormationCreator.Converters +{ + public class AppModeToVisibilityConverter : IValueConverter + { + static string[] _viewsForControl = { "ControlPanelView_Formation", "SwitchToPreparedForRunningTasksModeButton" }; + static string[] _viewsForTasks = { nameof(ModifyTaskView), nameof(TaskBarView), "SwitchToControllingCoptersModeButton" }; + public object Convert(object value, Type targetType, object parameter, CultureInfo culture) + { + var mode = (AppMode)value; + var viewName = (string)parameter; + switch (mode) + { + case AppMode.ControllingCopters: + return _viewsForControl.Contains(viewName) ? Visibility.Visible : Visibility.Collapsed; + case AppMode.ModifyingTask: + case AppMode.PreparedForRunningTasks: + case AppMode.RunningTasks: + return _viewsForTasks.Contains(viewName) ? Visibility.Visible : Visibility.Collapsed; + default: + return Visibility.Visible; + } + } + + public object ConvertBack(object value, Type targetType, object parameter, CultureInfo culture) + { + throw new NotImplementedException(); + } + } +} diff --git a/Plane.FormationCreator/Converters/FlightTaskIsSelectedToEffectConverter.cs b/Plane.FormationCreator/Converters/FlightTaskIsSelectedToEffectConverter.cs new file mode 100644 index 0000000..aca993b --- /dev/null +++ b/Plane.FormationCreator/Converters/FlightTaskIsSelectedToEffectConverter.cs @@ -0,0 +1,36 @@ +using Plane.FormationCreator.Formation; +using Plane.FormationCreator.Views; +using System; +using System.Collections.Generic; +using System.Globalization; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows; +using System.Windows.Data; +using System.Windows.Media; +using System.Windows.Media.Effects; + +namespace Plane.FormationCreator.Converters +{ + public class FlightTaskIsSelectedToEffectConverter : IValueConverter + { + static DropShadowEffect _effect = new DropShadowEffect + { + Color = Colors.LightGray, + // Color = Colors.White, + Direction = 90, + BlurRadius = 10 + }; + public object Convert(object value, Type targetType, object parameter, CultureInfo culture) + { + var selected = (bool)value; + return selected ? _effect : null; + } + + public object ConvertBack(object value, Type targetType, object parameter, CultureInfo culture) + { + throw new NotImplementedException(); + } + } +} diff --git a/Plane.FormationCreator/Converters/FlightTaskStatusToFillConverter.cs b/Plane.FormationCreator/Converters/FlightTaskStatusToFillConverter.cs new file mode 100644 index 0000000..75e537b --- /dev/null +++ b/Plane.FormationCreator/Converters/FlightTaskStatusToFillConverter.cs @@ -0,0 +1,30 @@ +using Plane.FormationCreator.Formation; +using Plane.FormationCreator.Views; +using System; +using System.Collections.Generic; +using System.Globalization; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows; +using System.Windows.Data; +using System.Windows.Media; + +namespace Plane.FormationCreator.Converters +{ + public class FlightTaskStatusToFillConverter : IValueConverter + { + static SolidColorBrush _normalFill = new SolidColorBrush(Colors.LightGray); + static SolidColorBrush _runningFill = new SolidColorBrush(Colors.OrangeRed); + public object Convert(object value, Type targetType, object parameter, CultureInfo culture) + { + var status = (FlightTaskStatus)value; + return status == FlightTaskStatus.Running ? _runningFill : _normalFill; + } + + public object ConvertBack(object value, Type targetType, object parameter, CultureInfo culture) + { + throw new NotImplementedException(); + } + } +} diff --git a/Plane.FormationCreator/Converters/HeartbeatCountToBrushConverter.cs b/Plane.FormationCreator/Converters/HeartbeatCountToBrushConverter.cs new file mode 100644 index 0000000..253662f --- /dev/null +++ b/Plane.FormationCreator/Converters/HeartbeatCountToBrushConverter.cs @@ -0,0 +1,34 @@ +using Plane.Copters; +using Plane.FormationCreator.Formation; +using System; +using System.Collections.Generic; +using System.Globalization; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows; +using System.Windows.Data; +using System.Windows.Media; + +namespace Plane.FormationCreator.Converters +{ + public class HeartbeatCountToBrushConverter : IMultiValueConverter + { + private static SolidColorBrush _zeroBrush = new SolidColorBrush(Color.FromArgb(125, 125, 125, 125)); + private static SolidColorBrush _oneBrush = Copter.DefaultBrush; + + public object Convert(object[] values, Type targetType, object parameter, CultureInfo culture) + { + if (values[0] == DependencyProperty.UnsetValue) return _zeroBrush; + var heartbeatCount = (ulong)values[0]; + if (heartbeatCount % 2 == 0) return _zeroBrush; + var copter = values[1] as Copter; + return copter == null ? _oneBrush : copter.Brush; + } + + public object[] ConvertBack(object value, Type[] targetTypes, object parameter, CultureInfo culture) + { + throw new NotSupportedException(); + } + } +} diff --git a/Plane.FormationCreator/Formation/AppMode.cs b/Plane.FormationCreator/Formation/AppMode.cs new file mode 100644 index 0000000..c56c3bf --- /dev/null +++ b/Plane.FormationCreator/Formation/AppMode.cs @@ -0,0 +1,30 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public enum AppMode + { + ControllingCopters, + ModifyingTask, + PreparedForRunningTasks, + RunningTasks, + MeasuringDistance + } + + public static class AppModeExtensions + { + public static bool IsForControl(this AppMode mode) + { + return mode == AppMode.ControllingCopters; + } + + public static bool IsForTasks(this AppMode mode) + { + return mode != AppMode.ControllingCopters; + } + } +} diff --git a/Plane.FormationCreator/Formation/Copter.cs b/Plane.FormationCreator/Formation/Copter.cs new file mode 100644 index 0000000..8ee4b29 --- /dev/null +++ b/Plane.FormationCreator/Formation/Copter.cs @@ -0,0 +1,44 @@ +using Plane.Copters; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Communication; +using System.Threading; +using System.Windows.Media; + +namespace Plane.FormationCreator.Formation +{ + class Copter : PLCopter + { + public Copter(IConnection Connection, SynchronizationContext uiThreadContext) : base(Connection, uiThreadContext) + { + Brush = _brushes[NextBrushIndex()]; + } + + static SolidColorBrush[] _brushes = new[] + { + new SolidColorBrush(Color.FromArgb(180, 255, 0, 0)), + new SolidColorBrush(Color.FromArgb(180, 235, 97, 0)), + new SolidColorBrush(Color.FromArgb(180, 255, 255, 0)), + new SolidColorBrush(Color.FromArgb(180, 0, 255, 0)), + new SolidColorBrush(Color.FromArgb(180, 0, 198, 255)), + new SolidColorBrush(Color.FromArgb(180, 0, 122, 204)), + new SolidColorBrush(Color.FromArgb(180, 174, 0, 255)) + }; + static int _brushIndex = 0; + static int NextBrushIndex() + { + return _brushIndex++ % _brushes.Length; + } + + internal static SolidColorBrush DefaultBrush { get; } = new SolidColorBrush(Color.FromRgb(28, 151, 234)); + private SolidColorBrush _Brush; + public SolidColorBrush Brush + { + get { return _Brush ?? DefaultBrush; } + set { Set(nameof(Brush), ref _Brush, value); } + } + } +} diff --git a/Plane.FormationCreator/Formation/CopterManager.cs b/Plane.FormationCreator/Formation/CopterManager.cs new file mode 100644 index 0000000..2313776 --- /dev/null +++ b/Plane.FormationCreator/Formation/CopterManager.cs @@ -0,0 +1,85 @@ +using Plane.Copters; +using GalaSoft.MvvmLight; +using System; +using System.Collections; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public class CopterManager : ObservableObject + { + public CopterManager() + { + AppEx.Current.AppModeChanged += (sender, e) => + { + RaisePropertyChanged(nameof(Copters)); + }; + App.Current.Exit += (sender, e) => + { + Task.WhenAll(Copters.Select(c => c.DisconnectAsync())); + }; + } + + + public ObservableCollection Copters { get; } = new ObservableCollection(); + + public ArrayList CopterStatus = new ArrayList(); + //public ObservableCollection Copters + //{ + // get + // { + // switch (AppEx.Instance.CurrentMode) + // { + // case AppMode.ControllingCopters: + // default: + // return CoptersForControlling; + // case AppMode.PreparedForRunningTasks: + // case AppMode.ModifyingTask: + // return CoptersForModifyingTask; + // case AppMode.RunningTasks: + // throw new NotImplementedException(); + // } + // } + //} + + + public IEnumerable SelectedCopters { get { return _selectedCoptersGetter().Cast(); } } + + /// + /// 注意：为避免多线程操作出问题，每次使用此属性时都会新建一个 List！ + /// + public IEnumerable AcceptingControlCopters { get { return SelectedCopters.ToList(); } } + + private Func _selectedCoptersGetter; + + private Action _selectCopterAction; + + public void SetSelectionDelegates(Func selectedCoptersGetter, Action selectCopterAction) + { + _selectedCoptersGetter = selectedCoptersGetter; + _selectCopterAction = selectCopterAction; + } + + public event EventHandler SelectedCoptersChanged; + + public void RaiseSelectedCoptersChanged(IEnumerable addedCopters, IEnumerable removedCopters) + { + SelectedCoptersChanged?.Invoke(this, new SelectedCoptersChangedEventArgs { AddedCopters = addedCopters, RemovedCopters = removedCopters }); + } + + public void Select(ICopter copter) + { + _selectCopterAction(copter); + } + } + + public class SelectedCoptersChangedEventArgs : EventArgs + { + public IEnumerable AddedCopters { get; set; } + public IEnumerable RemovedCopters { get; set; } + } +} diff --git a/Plane.FormationCreator/Formation/CopterStatus.cs b/Plane.FormationCreator/Formation/CopterStatus.cs new file mode 100644 index 0000000..8e579fd --- /dev/null +++ b/Plane.FormationCreator/Formation/CopterStatus.cs @@ -0,0 +1,61 @@ +using GalaSoft.MvvmLight; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public class CopterStatus : ObservableObject + { + private float _Lng; + public float Lng + { + get { return _Lng == 0 ? 116.23426389F : _Lng; } + set { Set(nameof(Lng), ref _Lng, value); } + } + + private float _Lat; + public float Lat + { + get { return _Lat == 0 ? 40.160491667F : _Lat; } + set { Set(nameof(Lat), ref _Lat, value); } + } + + private float _Height; + public float Height + { + get { return _Height; } + set { Set(nameof(Height), ref _Height, value); } + } + + private float _Heading; + public float Heading + { + get { return _Heading; } + set { Set(nameof(Heading), ref _Heading, value); } + } + + private float _Pitch; + public float Pitch + { + get { return _Pitch; } + set { Set(nameof(Pitch), ref _Pitch, value); } + } + + private float _Roll; + public float Roll + { + get { return _Roll; } + set { Set(nameof(Roll), ref _Roll, value); } + } + + private DateTime _Time; + public DateTime Time + { + get { return _Time; } + set { Set(nameof(Time), ref _Time, value); } + } + } +} diff --git a/Plane.FormationCreator/Formation/Extensions.cs b/Plane.FormationCreator/Formation/Extensions.cs new file mode 100644 index 0000000..53e8350 --- /dev/null +++ b/Plane.FormationCreator/Formation/Extensions.cs @@ -0,0 +1,77 @@ +using Plane.Copters; +using Plane.Geography; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + static class Extensions + { + public static LatLng? GetCenter(this IEnumerable copters) + { + int count = 0; + double sumLat = 0; + double sumLng = 0; + foreach (var copter in copters) + { + sumLat += copter.Latitude; + sumLng += copter.Longitude; + ++count; + } + if (count == 0) + { + return null; + } + return new LatLng + { + Lat = sumLat / count, + Lng = sumLng / count + }; + } + + public static double InPlaneDirectionToDeg(this ICopter copter, double targetLat, double targetLng) + { + return GeographyUtils.RadToDeg(GeographyUtils.CalcDirection2D(copter.Latitude, copter.Longitude, targetLat, targetLng)); + } + + public static double DistanceTo(this ICopter copter, double targetLat, double targetLng, float targetAlt) + { + return GeographyUtils.CalcDistance(copter.Latitude, copter.Longitude, copter.Altitude, targetLat, targetLng, targetAlt); + } + + public static double DistanceTo(this ICopter copter, ICopter copter2) + { + return GeographyUtils.CalcDistance(copter.Latitude, copter.Longitude, copter.Altitude, copter2.Latitude, copter2.Longitude, copter2.Altitude); + } + + public static double InPlaneDistanceTo(this ICopter copter, double targetLat, double targetLng) + { + 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) < 2; + // return copter.DistanceTo(targetLat, targetLng, targetAlt) < 1.5; + // return copter.DistanceTo(targetLat, targetLng, targetAlt) < 2; // added by ZJF + } + + 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) + { + return copter.DistanceTo(copter2) < 2; + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTask.cs b/Plane.FormationCreator/Formation/FlightTask.cs new file mode 100644 index 0000000..779cb86 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTask.cs @@ -0,0 +1,125 @@ +using Plane.Copters; +using Plane.Logging; +using GalaSoft.MvvmLight; +using Microsoft.Practices.ServiceLocation; +using System; +using System.Collections.Generic; +using System.Collections.Specialized; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTask : ObservableObject + { + public FlightTask(FlightTaskType type) + { + TaskType = type; + } + + private FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + + private ILogger _logger = ServiceLocator.Current.GetInstance(); + + private FlightTaskStatus _Status = FlightTaskStatus.Created; + public FlightTaskStatus Status + { + get { return _Status; } + set { Set(nameof(Status), ref _Status, value); } + } + + private FlightTaskType _TaskType; + public FlightTaskType TaskType + { + get { return _TaskType; } + set + { + if (_TaskType != value) + { + RaisePropertyChanged(nameof(TaskTypeIndex)); + } + Set(nameof(TaskType), ref _TaskType, value); + } + } + + public int TaskTypeIndex + { + get { return (int)TaskType; } + set + { + TaskType = (FlightTaskType)value; + if ( (value == (int)FlightTaskType.Loiter) || (value == (int)FlightTaskType.Circle) ) + { + VerticalLift = true; + } + } + } + + private bool _IsSelected; + public bool IsSelected + { + get { return _IsSelected; } + set { Set(nameof(IsSelected), ref _IsSelected, value); } + } + + // 右键单击任务，用于隐藏任务图标, added by ZJF + private bool _IsRightSelected; + public bool IsRightSelected + { + get { return _IsRightSelected; } + set { Set(nameof(IsRightSelected), ref _IsRightSelected, value); } + } + + public List SingleCopterInfos { get; set; } = new List(); + + private FlightTaskSingleCopterInfo _ModifyingSingleCopterInfo; + public FlightTaskSingleCopterInfo ModifyingSingleCopterInfo + { + get { return _ModifyingSingleCopterInfo; } + set { Set(nameof(ModifyingSingleCopterInfo), ref _ModifyingSingleCopterInfo, value); } + } + + public async Task RunAsync() + { + switch (TaskType) + { + case FlightTaskType.FlyTo: + await RunFlyToTaskAsync().ConfigureAwait(false); + break; + case FlightTaskType.Turn: + await RunTurnTaskAsync().ConfigureAwait(false); + break; + case FlightTaskType.Circle: + await RunCircleTaskAsync().ConfigureAwait(false); + break; + case FlightTaskType.SimpleCircle: + await RunSimpleCircleTaskAsync().ConfigureAwait(false); + break; + case FlightTaskType.Loiter: + await RunLoiterTimeTaskAsync().ConfigureAwait(false); + break; + case FlightTaskType.TakeOff: + await RunTakeOffTaskAsync().ConfigureAwait(false); + break; + case FlightTaskType.ReturnToLand: // Added by ZJF + await RunReturnToLandTaskAsync().ConfigureAwait(false); + break; + default: + throw new InvalidOperationException(); + } + } + } + + public enum FlightTaskType + { + FlyTo = 0, + Turn = 1, + Circle = 2, + Loiter = 3, // 加入悬停时间选项 + // LEDAction = 4, + ReturnToLand = 4, // added by ZJF. 20160315 + SimpleCircle = 5, + TakeOff = 100 + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskManager.cs b/Plane.FormationCreator/Formation/FlightTaskManager.cs new file mode 100644 index 0000000..07b8ea3 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTaskManager.cs @@ -0,0 +1,830 @@ +using Plane.Collections; +using Plane.Copters; +using Plane.Geography; +using Plane.Windows.Messages; +using GalaSoft.MvvmLight; +using Newtonsoft.Json; +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public class FlightTaskManager : ObservableObject + { + public FlightTaskManager(CopterManager copterManager) + { + _copterManager = copterManager; + + //AddTakeOffTask(_copterManager.Copters); + + _copterManager.Copters.CollectionChanged += (sender, e) => + { + // TODO: 林俊清, 20150724, 需要改为正确的做法（清除旧飞机的任务，补充新飞机的任务）。 + AddTakeOffTask(e.NewItems?.Cast()); + }; + + _copterManager.SelectedCoptersChanged += (sender, e) => + { + // TODO: 林俊清, 20150803, 处理选中多个飞行器的情况。 + if (_copterManager.SelectedCopters.Count() > 1) + { + return; + } + var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); + + foreach (var task in Tasks) + { + foreach (var info in task.SingleCopterInfos) + { + if (info.Copter == selectedCopter) + { + task.ModifyingSingleCopterInfo = info; + break; + } + } + } + }; + + TaskAdded += (sender, e) => + { + // TODO: 林俊清, 20150803, 处理选中多个飞行器的情况。 + if (_copterManager.SelectedCopters.Count() > 1) + { + return; + } + var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); + + var task = e.AddedTask; + foreach (var info in task.SingleCopterInfos) + { + if (info.Copter == selectedCopter) + { + task.ModifyingSingleCopterInfo = info; + break; + } + } + }; + } + + private CopterManager _copterManager; + + private double _OriginLat; + public double OriginLat + { + get { return _OriginLat; } + set { Set(nameof(OriginLat), ref _OriginLat, value); } + } + + private double _OriginLng; + public double OriginLng + { + get { return _OriginLng; } + set { Set(nameof(OriginLng), ref _OriginLng, value); } + } + + private void AddTakeOffTask(IEnumerable copters) + { + if (copters == null || !copters.Any()) return; + + bool takeOffTaskExisted = Tasks.Count >= 1; + FlightTask takeOffTask; + if (takeOffTaskExisted) + { + takeOffTask = Tasks[0]; + } + else + { + takeOffTask = new FlightTask(FlightTaskType.TakeOff); + Tasks.Add(takeOffTask); + TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { AddedTask = takeOffTask }); + } + foreach (var copter in copters) + { + takeOffTask.SingleCopterInfos.Add(FlightTaskSingleCopterInfo.CreateForTakeOffTask(copter, targetAlt: 10)); + } + } + + public ObservableCollection Tasks { get; } = new ObservableCollection(); + + private FlightTask _SelectedTask; + public FlightTask SelectedTask + { + get { return _SelectedTask; } + set + { + if (_SelectedTask != value) + { + if (_SelectedTask != null) _SelectedTask.IsSelected = false; + if (value != null) value.IsSelected = true; + } + Set(nameof(SelectedTask), ref _SelectedTask, value); + } + } + + private int _SelectedTaskIndex; + public int SelectedTaskIndex + { + get { return _SelectedTaskIndex; } + set { Set(nameof(SelectedTaskIndex), ref _SelectedTaskIndex, value); } + } + + // 右键单击任务，用于隐藏任务图标, added by ZJF + private int _RightSelectedTaskIndex; + public int RightSelectedTaskIndex + { + get { return _RightSelectedTaskIndex; } + set { Set(nameof(RightSelectedTaskIndex), ref _RightSelectedTaskIndex, value); } + } + + private FlightTask _CurrentRunningTask; + public FlightTask CurrentRunningTask + { + get { return _CurrentRunningTask; } + private set { Set(nameof(CurrentRunningTask), ref _CurrentRunningTask, value); } + } + + private int _CurrentRunningTaskIndex; + public int CurrentRunningTaskIndex + { + get { return _CurrentRunningTaskIndex; } + private set { Set(nameof(CurrentRunningTaskIndex), ref _CurrentRunningTaskIndex, value); } + } + + public event EventHandler TaskAdded; + + public event EventHandler SingleCopterInfoChanged; + + public void RaiseSingleCopterTaskChanged(FlightTaskSingleCopterInfo singleCopterInfo) + { + SingleCopterInfoChanged?.Invoke(this, new SingleCopterInfoChangedEventArgs(singleCopterInfo)); + } + + public event EventHandler TasksCleared; + + public void AddTask() + { + //if (AppEx.Instance.CurrentMode == AppMode.PreparedForRunningTasks) + { + var copters = _copterManager.Copters; + if (!copters.Any()) return; + AppEx.Current.AppMode = AppMode.ModifyingTask; + var lastTask = Tasks.LastOrDefault(); + var nullableCenter = copters.GetCenter(); + if (nullableCenter == null) return; + var center = nullableCenter.Value; + var newTask = new FlightTask(FlightTaskType.FlyTo); + foreach (var copter in copters) + { + var lastSingleCopterInfo = lastTask.SingleCopterInfos.Find(info => info.Copter == copter); + var direction = GeographyUtils.CalcDirection2D(center.Lat, center.Lng, lastSingleCopterInfo.TargetLat, lastSingleCopterInfo.TargetLng); + var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(lastSingleCopterInfo.TargetLat, lastSingleCopterInfo.TargetLng, (float)GeographyUtils.RadToDeg(direction), 5); + var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(copter, targetLatLng.Item1, targetLatLng.Item2, lastSingleCopterInfo.TargetAlt); + newSingleCopterInfo.TargetHeading = lastSingleCopterInfo.TargetHeading; + newSingleCopterInfo.CenterDirectionDeg = lastSingleCopterInfo.TargetHeading; + newTask.SingleCopterInfos.Add(newSingleCopterInfo); + } + Tasks.Add(newTask); + TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask }); + SelectedTask = newTask; + SelectedTaskIndex = Tasks.Count - 1; + } + } + + public void ClearTasks() + { + ResetTasks(); + this.Tasks.Clear(); + SelectedTask = null; + SelectedTaskIndex = 0; + TasksCleared?.Invoke(this, EventArgs.Empty); + AddTakeOffTask(_copterManager.Copters); + } + + public void ResetTasks() + { + Pause(); + CurrentRunningTaskIndex = 0; + if (CurrentRunningTask != null) + { + CurrentRunningTask.Status = FlightTaskStatus.Saved; + CurrentRunningTask = null; + } + + for (int i = 0; i < Tasks.Count; i++) + { + // 将起飞的阶段标志位都置位0 + if (Tasks[i].TaskType == FlightTaskType.TakeOff) + { + var infos = Tasks[i].SingleCopterInfos; + for (int j = 0; j < infos.Count; j++) + { + var info = infos[j]; + info.takeOffStage = 0; + } + } + + // 将降落的阶段标志位都置位0 + if (Tasks[i].TaskType == FlightTaskType.ReturnToLand) + { + var infos = Tasks[i].SingleCopterInfos; + for (int j = 0; j < infos.Count; j++) + { + var info = infos[j]; + info.RTLStage = 0; + } + } + } + } + + public void RestoreFlyToTask(bool staggerRoutes, dynamic singleCopterInfos) + { + var copters = _copterManager.Copters; + if (!copters.Any()) return; + AppEx.Current.AppMode = AppMode.ModifyingTask; + var lastTask = Tasks.LastOrDefault(); + var nullableCenter = copters.GetCenter(); + if (nullableCenter == null) return; + var center = nullableCenter.Value; + var newTask = new FlightTask(FlightTaskType.FlyTo) { StaggerRoutes = staggerRoutes }; + // TODO: 林俊清, 20150801, 处理实际飞行器数目与记录中数目不一致的情况。 + for (int i = 0; i < copters.Count && i < singleCopterInfos.Count; i++) + { + var copter = copters[i]; + var singleCopterInfoObj = singleCopterInfos[i]; + var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt); + newTask.SingleCopterInfos.Add(newSingleCopterInfo); + } + Tasks.Add(newTask); + TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask }); + SelectedTask = newTask; + SelectedTaskIndex = Tasks.Count - 1; + } + + private void RestoreTurnTask(dynamic singleCopterInfos) + { + var copters = _copterManager.Copters; + if (!copters.Any()) return; + AppEx.Current.AppMode = AppMode.ModifyingTask; + var lastTask = Tasks.LastOrDefault(); + var newTask = new FlightTask(FlightTaskType.Turn); + // TODO: 林俊清, 20150806, 处理实际飞行器数目与记录中数目不一致的情况。 + for (int i = 0; i < copters.Count && i < singleCopterInfos.Count; i++) + { + var copter = copters[i]; + var singleCopterInfoObj = singleCopterInfos[i]; + var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForTurnTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (short)singleCopterInfoObj.targetHeading); + newTask.SingleCopterInfos.Add(newSingleCopterInfo); + } + Tasks.Add(newTask); + TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask }); + SelectedTask = newTask; + SelectedTaskIndex = Tasks.Count - 1; + } + + private void RestoreCircleTask(dynamic singleCopterInfos) + { + var copters = _copterManager.Copters; + if (!copters.Any()) return; + AppEx.Current.AppMode = AppMode.ModifyingTask; + var lastTask = Tasks.LastOrDefault(); + var newTask = new FlightTask(FlightTaskType.Circle); + // TODO: 林俊清, 20150806, 处理实际飞行器数目与记录中数目不一致的情况。 + for (int i = 0; i < copters.Count && i < singleCopterInfos.Count; i++) + { + var copter = copters[i]; + var singleCopterInfoObj = singleCopterInfos[i]; + var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForCircleTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (short)singleCopterInfoObj.centerDirectionDeg, (int)singleCopterInfoObj.radius, (int)singleCopterInfoObj.rate, (int)singleCopterInfoObj.turns, (ushort)singleCopterInfoObj.channel3); + newTask.SingleCopterInfos.Add(newSingleCopterInfo); + } + Tasks.Add(newTask); + TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask }); + SelectedTask = newTask; + SelectedTaskIndex = Tasks.Count - 1; + } + + private void RestoreSimpleCircleTask(dynamic singleCopterInfos) + { + var copters = _copterManager.Copters; + if (!copters.Any()) return; + AppEx.Current.AppMode = AppMode.ModifyingTask; + var lastTask = Tasks.LastOrDefault(); + var newTask = new FlightTask(FlightTaskType.SimpleCircle); + // TODO: 林俊清, 20150806, 处理实际飞行器数目与记录中数目不一致的情况。 + for (int i = 0; i < copters.Count && i < singleCopterInfos.Count; i++) + { + var copter = copters[i]; + var singleCopterInfoObj = singleCopterInfos[i]; + var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForSimpleCircleTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (int)singleCopterInfoObj.rate, (int)singleCopterInfoObj.turns, (ushort)singleCopterInfoObj.channel3); + newTask.SingleCopterInfos.Add(newSingleCopterInfo); + } + Tasks.Add(newTask); + TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = newTask }); + SelectedTask = newTask; + SelectedTaskIndex = Tasks.Count - 1; + } + + // added by ZJF + private void RestoreReturnToLandTask(IEnumerable copters) + { + if (copters == null || !copters.Any()) return; + AppEx.Current.AppMode = AppMode.ModifyingTask; + var lastTask = Tasks.LastOrDefault(); + + var RTLTask = new FlightTask(FlightTaskType.ReturnToLand); + foreach (var copter in copters) + { + var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForReturnToLandTask(copter, targetAlt:10); + RTLTask.SingleCopterInfos.Add(newSingleCopterInfo); + } + Tasks.Add(RTLTask); + TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = RTLTask }); + SelectedTask = RTLTask; + SelectedTaskIndex = Tasks.Count - 1; + } + + // added by ZJF + private void RestoreLoiterTimeTask(float LoiteTimeTmp, bool flashCheck, float flashCheckPeriod, + bool oneByOneCheck, float oneByOneCheckPeriod, string flashNameArray, string flashIndexArray, + dynamic singleCopterInfos) + { + var copters = _copterManager.Copters; + if (copters == null || !copters.Any()) return; + AppEx.Current.AppMode = AppMode.ModifyingTask; + var lastTask = Tasks.LastOrDefault(); + + var loiterTimeTask = new FlightTask(FlightTaskType.Loiter) + { + LoiterTimeAttr = LoiteTimeTmp + }; + loiterTimeTask.flashAttr = flashCheck; + loiterTimeTask.flashPeriodAttr = flashCheckPeriod; + loiterTimeTask.oneByOneAttr = oneByOneCheck; + loiterTimeTask.oneByOnePeriodAttr = oneByOneCheckPeriod; + loiterTimeTask.flashCopterNameArray = flashNameArray; + loiterTimeTask.flashCopterIndexArray = flashIndexArray; + // foreach (var copter in copters) + for (int i=0; i < copters.Count && i < singleCopterInfos.Count; i++) + { + var copter = copters[i]; + var singleCopterInfoObj = singleCopterInfos[i]; + var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForLoiterTimeTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt); + loiterTimeTask.SingleCopterInfos.Add(newSingleCopterInfo); + } + Tasks.Add(loiterTimeTask); + TaskAdded?.Invoke(this, new FlightTaskAddedEventArgs { LastTask = lastTask, AddedTask = loiterTimeTask }); + SelectedTask = loiterTimeTask; + SelectedTaskIndex = Tasks.Count - 1; + } + + public string ExportTasks() + { + // For reference. + //var tasks = new object[] + //{ + // new + // { + // type = FlightTaskType.TakeOff, + // }, + // new + // { + // type = FlightTaskType.FlyTo, + // staggerRoutes = false, + // singleCopterInfos = new object[] { + // new { + // latOffset = 0.0001, + // lngOffset = 0.0001, + // targetAlt = 10 + // }, + // new { + // latOffset = 0.0003, + // lngOffset = 0.0003, + // targetAlt = 15 + // } + // } + // }, + // new + // { + // type = FlightTaskType.Turn, + // singleCopterInfos = new object[] { + // new { + // latOffset = 0.0001, + // lngOffset = 0.0001, + // targetAlt = 10, + // targetHeading = 90 + // }, + // new { + // latOffset = 0.0001, + // lngOffset = 0.0001, + // targetAlt = 10, + // targetHeading = 270 + // } + // } + // }, + // new + // { + // type = FlightTaskType.Circle, + // singleCopterInfos = new object[] { + // new { + // latOffset = 0.0001, + // lngOffset = 0.0001, + // targetAlt = 10, + // centerDirectionDeg = 90, + // radius = 1000, + // rate = 20, + // turns = 1, + // channel3 = 1500 + // }, + // new { + // latOffset = 0.0003, + // lngOffset = 0.0003, + // targetAlt = 15, + // centerDirectionDeg = 270, + // radius = 1000, + // rate = 20, + // turns = 1, + // channel3 = 1500 + // } + // } + // }, + // new + // { + // type = FlightTaskType.SimpleCircle, + // singleCopterInfos = new object[] { + // new { + // latOffset = 0.0001, + // lngOffset = 0.0001, + // targetAlt = 10, + // rate = 20, + // turns = 1, + // channel3 = 1500 + // }, + // new { + // latOffset = 0.0003, + // lngOffset = 0.0003, + // targetAlt = 15, + // rate = 20, + // turns = 1, + // channel3 = 1500 + // } + // } + // } + //}; + var tasks = Tasks.Select(task => + { + var type = task.TaskType; + switch (type) + { + case FlightTaskType.TakeOff: + return new { type = type }; + case FlightTaskType.FlyTo: + return new + { + type = type, + staggerRoutes = task.StaggerRoutes, + singleCopterInfos = task.SingleCopterInfos.Select(info => + { + var offset = info.LatLngOffset; + return new + { + latOffset = offset.Lat, + lngOffset = offset.Lng, + targetAlt = info.TargetAlt + }; + }) + }; + case FlightTaskType.Turn: + return new + { + type = type, + singleCopterInfos = task.SingleCopterInfos.Select(info => + { + var offset = info.LatLngOffset; + return new + { + latOffset = offset.Lat, + lngOffset = offset.Lng, + targetAlt = info.TargetAlt, + targetHeading = info.TargetHeading + }; + }) + }; + case FlightTaskType.Loiter: // 导出悬停任务 + return new + { + type = type, + loiterTimeAttr = task.LoiterTimeAttr, + flashCheck = task.flashAttr, + flashCheckPeriod = task.flashPeriodAttr, + oneByOneCheck = task.oneByOneAttr, + oneByOneCheckPeriod = task.oneByOnePeriodAttr, + flashNameArray = task.flashCopterNameArray, + flashIndexArray = task.flashCopterIndexArray, + singleCopterInfos = task.SingleCopterInfos.Select(info => + { + var offset = info.LatLngOffset; + return new + { + latOffset = offset.Lat, + lngOffset = offset.Lng, + targetAlt = info.TargetAlt + }; + }) + }; + case FlightTaskType.Circle: + return new + { + type = type, + singleCopterInfos = task.SingleCopterInfos.Select(info => + { + var offset = info.LatLngOffset; + return new + { + latOffset = offset.Lat, + lngOffset = offset.Lng, + targetAlt = info.TargetAlt, + centerDirectionDeg = info.CenterDirectionDeg, + radius = info.Radius, + rate = info.Rate, + turns = info.Turns, + channel3 = info.Channel3 + }; + }) + }; + case FlightTaskType.SimpleCircle: + return new + { + type = type, + singleCopterInfos = task.SingleCopterInfos.Select(info => + { + var offset = info.LatLngOffset; + return new + { + latOffset = offset.Lat, + lngOffset = offset.Lng, + targetAlt = info.TargetAlt, + rate = info.Rate, + turns = info.Turns, + channel3 = info.Channel3 + }; + }) + }; + case FlightTaskType.ReturnToLand: // added by ZJF + return new + { + type = type + }; + default: + throw new NotImplementedException(type + " task export not implemented."); + } + }); + return JsonConvert.SerializeObject(tasks); + } + + public void ImportTasks(string tasksText) + { + dynamic tasks = JsonConvert.DeserializeObject(tasksText); + var copters = _copterManager.Copters; + foreach (var task in tasks) + { + switch ((FlightTaskType)task.type) + { + case FlightTaskType.TakeOff: + // AddTakeOffTask(copters); // added by ZJF + break; + case FlightTaskType.FlyTo: + RestoreFlyToTask((bool)task.staggerRoutes, task.singleCopterInfos); + break; + case FlightTaskType.Turn: + RestoreTurnTask(task.singleCopterInfos); + break; + case FlightTaskType.Circle: + RestoreCircleTask(task.singleCopterInfos); + break; + case FlightTaskType.Loiter: + RestoreLoiterTimeTask( (float)task.loiterTimeAttr, (bool)task.flashCheck, (float)task.flashCheckPeriod, + (bool)task.oneByOneCheck, (float)task.oneByOneCheckPeriod, (string)task.flashNameArray, + (string)task.flashIndexArray, task.singleCopterInfos); + break; + case FlightTaskType.SimpleCircle: + RestoreSimpleCircleTask(task.singleCopterInfos); + break; + case FlightTaskType.ReturnToLand: + RestoreReturnToLandTask(copters); + break; + } + } + } + + public void Select(int taskIndex, ICopter copter) + { + this.SelectedTaskIndex = taskIndex; + this.SelectedTask = Tasks[taskIndex]; + + // 在悬停任务时，左键waypoint为选中 + // 在悬停任务模式，且LED控制被选中时 + if ((this.SelectedTask.TaskType == FlightTaskType.Loiter) && (this.SelectedTask.flashAttr || this.SelectedTask.oneByOneAttr)) + { + string copterNameStr = this.SelectedTask.flashCopterNameArray; + + string[] copterArray = copterNameStr.Split(','); + bool flag = false; + for (int i = 0; i < copterArray.Length; i++) + { + if (copterArray[i].Equals("")) + continue; + if (copterArray[i].Equals(copter.Name)) + { + flag = true; + break; + } + } + if (!flag) + { + if (copterArray[0].Equals("")) + { + this.SelectedTask.flashCopterNameArray += copter.Name; + SelectedTask.flashCopterIndexArray += Name2Index(copter.Name); + } + else + { + this.SelectedTask.flashCopterNameArray += "," + copter.Name; + SelectedTask.flashCopterIndexArray += "," + Name2Index(copter.Name); + } + + } + } + + } + + /** + * 在悬停任务时，右键waypoint为取消选中 + */ + public void RightSelect(int taskIndex, ICopter copter) + { + this.SelectedTaskIndex = taskIndex; + this.SelectedTask = Tasks[taskIndex]; + + // 在悬停任务模式，且LED控制被选中时 + if ((this.SelectedTask.TaskType == FlightTaskType.Loiter) && (this.SelectedTask.flashAttr || this.SelectedTask.oneByOneAttr)) + { + this.SelectedTask.flashCopterNameArray = DeleteSelectedName(this.SelectedTask.flashCopterNameArray, copter.Name); + this.SelectedTask.flashCopterIndexArray = DeleteSelectedName(this.SelectedTask.flashCopterIndexArray, Name2Index(copter.Name).ToString()); + } + } + + private int Name2Index(string name) // 获取指定copter名字对应的序号 + { + int index = -1; + for (int i = 0; i < SelectedTask.SingleCopterInfos.Count; i++) + { + if (name.Equals(SelectedTask.SingleCopterInfos[i].Copter.Name)) + { + return i; + } + } + return index; + } + + private string DeleteSelectedName( string nameArray, string name ) // 删除字符串中的指定项 + { + string copterStr = nameArray; + if (copterStr.Equals("")) + return ""; + string[] copterNameArray = copterStr.Split(','); + copterNameArray = copterNameArray.Where(str => !str.Equals(name)).ToArray(); + + copterStr = ""; + for (int i = 0; i < copterNameArray.Length; i++) + { + copterStr = copterStr + copterNameArray[i]; + if (i < (copterNameArray.Length - 1)) + { + copterStr = copterStr + ","; + } + } + return copterStr; + } + + public void Select(FlightTask flightTask) + { + this.SelectedTaskIndex = Tasks.IndexOf(flightTask); + this.SelectedTask = flightTask; + } + + // 右键选中任务 + public void RightSelect(FlightTask flightTask) + { + int RightSelectedTaskIndexTmp = Tasks.IndexOf(flightTask); + if (this.RightSelectedTaskIndex == RightSelectedTaskIndexTmp) + { + this.RightSelectedTaskIndex = -RightSelectedTaskIndexTmp; + } + else + { + this.RightSelectedTaskIndex = RightSelectedTaskIndexTmp; + } + } + + #region Run and pause. + + private bool? _IsPaused; + public bool? IsPaused + { + get { return _IsPaused; } + private set { Set(nameof(IsPaused), ref _IsPaused, value); } + } + + public async Task RunAsync() + { + IsPaused = false; + AppEx.Current.AppMode = AppMode.RunningTasks; + StartAvoidingCrash(); + for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++) + { + var task = Tasks[i]; + task.Status = FlightTaskStatus.Running; + CurrentRunningTask = task; + CurrentRunningTaskIndex = i; + await task.RunAsync().ConfigureAwait(false); + // 1. 被暂停时，中断 RunAsync。继续运行时将把此时运行了一半的 CurrentRunningTask 重新运行一遍。 + if (IsPaused == true) return; + + task.Status = FlightTaskStatus.Saved; + } + + // 2. 正常结束时，重置 CurrentRunningTask、CurrentRunningTaskIndex 和 IsPaused。 + CurrentRunningTask = null; + CurrentRunningTaskIndex = 0; + IsPaused = null; + } + + public void Pause() + { + IsPaused = true; + } + + #endregion Run and pause. + + private async void StartAvoidingCrash() + { + await Task.Factory.StartNew(LoopToAvoidCrash); + } + + private async void LoopToAvoidCrash() + { + while (IsPaused == false) + { + try + { + // 在目前的起飞、降落逻辑，如果起飞或最后一个任务是返航降落，不要检查两架飞机距离过近. added by ZJF + bool RTLFlag = ( CurrentRunningTaskIndex == Tasks.Count()-1 ) && ( CurrentRunningTask.TaskType == FlightTaskType.ReturnToLand ); + // bool RTLFlag = false; + if ( (CurrentRunningTaskIndex != 0) && !RTLFlag) + { + + foreach (var copter in _copterManager.Copters) + { + foreach (var anotherCopter in _copterManager.Copters) + { + if (copter != anotherCopter && copter.IsTooCloseTo(anotherCopter)) + { + Pause(); + Alert.Show($"{copter.Name} 与 {anotherCopter.Name} 距离过近，已中止任务。"); + return; + } + } + } + + } + } + catch // 林俊清, 20151102, 通常是“集合已修改”异常。 + { + } + finally + { + await Task.Delay(100).ConfigureAwait(false); + } + } + } + } + + public class FlightTaskAddedEventArgs : EventArgs + { + public FlightTask LastTask { get; set; } + public FlightTask AddedTask { get; set; } + } + + public class SingleCopterInfoChangedEventArgs : EventArgs + { + public SingleCopterInfoChangedEventArgs(FlightTaskSingleCopterInfo changedSingleCopterInfo) + { + this.ChangedSingleCopterInfo = changedSingleCopterInfo; + } + + public FlightTaskSingleCopterInfo ChangedSingleCopterInfo { get; set; } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo.cs new file mode 100644 index 0000000..3766bd7 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo.cs @@ -0,0 +1,72 @@ +using Plane.Copters; +using GalaSoft.MvvmLight; +using Microsoft.Practices.ServiceLocation; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTaskSingleCopterInfo : ObservableObject + { + public FlightTaskSingleCopterInfo(ICopter copter) + { + this.PropertyChanged += (sender, e) => + { + switch (e.PropertyName) + { + case nameof(TargetLat): + case nameof(TargetLng): + case nameof(TargetAlt): + _flightTaskManager.RaiseSingleCopterTaskChanged(this); + break; + default: + break; + } + }; + + this.Copter = copter; + } + + private CopterManager _copterManager = ServiceLocator.Current.GetInstance(); + private FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + + public ICopter Copter { get; set; } + + private double? _TargetLat; + public double TargetLat { get { return _TargetLat ?? Copter.Latitude; } set { Set(nameof(TargetLat), ref _TargetLat, value); } } + + private double? _TargetLng; + public double TargetLng { get { return _TargetLng ?? Copter.Longitude; } set { Set(nameof(TargetLng), ref _TargetLng, value); } } + + private float? _TargetAlt; + public float TargetAlt { get { return _TargetAlt ?? Copter.Altitude; } set { Set(nameof(TargetAlt), ref _TargetAlt, value); } } + + public LatLng LatLngOffset + { + get + { + var origin = GetOrigin(); + return new LatLng(TargetLat - origin.Lat, TargetLng - origin.Lng); + } + set + { + var origin = GetOrigin(); + TargetLat = origin.Lat + value.Lat; + TargetLng = origin.Lng + value.Lng; + } + } + + public LatLng GetOrigin() + { + return _copterManager.Copters.GetCenter() ?? LatLng.Empty; + } + + public bool isFinished; + public int takeOffStage; // 判断起飞处于哪个阶段. (0 -> 1 ->2, 对应起飞到10m -> 水平飞行 -> 垂直飞行) + public int RTLStage; // 判断降落返航处于哪个阶段. (0->1->2, 对应降落到10m -> 水平飞行 -> 垂直降落) + public int FlyToStage; // 判断FlyTo处于哪个阶段 + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Circle.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Circle.cs new file mode 100644 index 0000000..cd3975b --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Circle.cs @@ -0,0 +1,68 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Copters; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTaskSingleCopterInfo + { + public static FlightTaskSingleCopterInfo CreateForCircleTask(ICopter copter, LatLng latLngOffset, float targetAlt, short centerDirectionDeg, int radius, int rate, int turns, ushort channel3) + { + var info = new FlightTaskSingleCopterInfo(copter) + { + LatLngOffset = latLngOffset, + TargetAlt = targetAlt, + CenterDirectionDeg = centerDirectionDeg, + Radius = radius, + Rate = rate, + Turns = turns, + Channel3 = channel3 + }; + return info; + } + + private short _CenterDirectionDeg; + public short CenterDirectionDeg + { + get { return _CenterDirectionDeg; } + set { Set(nameof(CenterDirectionDeg), ref _CenterDirectionDeg, value); } + } + + private int _Radius = 1000; + /// + /// 圆的半径，单位为厘米。 + /// + public int Radius + { + get { return _Radius; } + set { Set(nameof(Radius), ref _Radius, value); } + } + + private int _Rate = 20; + /// + /// 转圈的角速度，单位为 deg/s，范围为 -90 到 90。 + /// + public int Rate + { + get { return _Rate; } + set { Set(nameof(Rate), ref _Rate, value); } + } + + private int _Turns = 1; + public int Turns + { + get { return _Turns; } + set { Set(nameof(Turns), ref _Turns, value); } + } + + private ushort _Channel3 = 1500; + public ushort Channel3 + { + get { return _Channel3; } + set { Set(nameof(Channel3), ref _Channel3, value); } + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_FlyTo.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_FlyTo.cs new file mode 100644 index 0000000..592afac --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_FlyTo.cs @@ -0,0 +1,33 @@ +using Plane.Copters; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTaskSingleCopterInfo + { + public static FlightTaskSingleCopterInfo CreateForFlyToTask(ICopter copter, double targetLat, double targetLng, float targetAlt) + { + var info = new FlightTaskSingleCopterInfo(copter) + { + TargetLat = targetLat, + TargetLng = targetLng, + TargetAlt = targetAlt + }; + return info; + } + + public static FlightTaskSingleCopterInfo CreateForFlyToTask(ICopter copter, LatLng latLngOffset, float targetAlt) + { + var info = new FlightTaskSingleCopterInfo(copter) + { + LatLngOffset = latLngOffset, + TargetAlt = targetAlt + }; + return info; + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_LoiterTime.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_LoiterTime.cs new file mode 100644 index 0000000..d59e131 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_LoiterTime.cs @@ -0,0 +1,22 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Copters; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTaskSingleCopterInfo + { + public static FlightTaskSingleCopterInfo CreateForLoiterTimeTask(ICopter copter, LatLng latLngOffset, float targetAlt) + { + var info = new FlightTaskSingleCopterInfo(copter) + { + LatLngOffset = latLngOffset, + TargetAlt = targetAlt + }; + return info; + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_ReturnToLand.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_ReturnToLand.cs new file mode 100644 index 0000000..9b8ac11 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_ReturnToLand.cs @@ -0,0 +1,22 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Copters; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTaskSingleCopterInfo + { + // This file is added by ZJF + public static FlightTaskSingleCopterInfo CreateForReturnToLandTask(ICopter copter, float targetAlt) + { + var info = new FlightTaskSingleCopterInfo(copter) + { + TargetAlt = targetAlt + }; + return info; + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_SimpleCircle.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_SimpleCircle.cs new file mode 100644 index 0000000..dec9406 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_SimpleCircle.cs @@ -0,0 +1,25 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Copters; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTaskSingleCopterInfo + { + public static FlightTaskSingleCopterInfo CreateForSimpleCircleTask(ICopter copter, LatLng latLngOffset, float targetAlt, int rate, int turns, ushort channel3) + { + var info = new FlightTaskSingleCopterInfo(copter) + { + LatLngOffset = latLngOffset, + TargetAlt = targetAlt, + Rate = rate, + Turns = turns, + Channel3 = channel3 + }; + return info; + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_TakeOff.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_TakeOff.cs new file mode 100644 index 0000000..e0ebda9 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_TakeOff.cs @@ -0,0 +1,21 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Copters; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTaskSingleCopterInfo + { + public static FlightTaskSingleCopterInfo CreateForTakeOffTask(ICopter copter, float targetAlt) + { + var info = new FlightTaskSingleCopterInfo(copter) + { + TargetAlt = targetAlt + }; + return info; + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Turn.cs b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Turn.cs new file mode 100644 index 0000000..e95472e --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_Turn.cs @@ -0,0 +1,30 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Copters; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTaskSingleCopterInfo + { + public static FlightTaskSingleCopterInfo CreateForTurnTask(ICopter copter, LatLng latLngOffset, float targetAlt, short targetHeading) + { + var info = new FlightTaskSingleCopterInfo(copter) + { + LatLngOffset = latLngOffset, + TargetAlt = targetAlt, + TargetHeading = targetHeading + }; + return info; + } + + private short _TargetHeading; + public short TargetHeading + { + get { return _TargetHeading; } + set { Set(nameof(TargetHeading), ref _TargetHeading, value); } + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskStatus.cs b/Plane.FormationCreator/Formation/FlightTaskStatus.cs new file mode 100644 index 0000000..cf8541e --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTaskStatus.cs @@ -0,0 +1,6 @@ +public enum FlightTaskStatus +{ + Created, + Saved, + Running +} \ No newline at end of file diff --git a/Plane.FormationCreator/Formation/FlightTask_Circle.cs b/Plane.FormationCreator/Formation/FlightTask_Circle.cs new file mode 100644 index 0000000..b0a352e --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTask_Circle.cs @@ -0,0 +1,137 @@ +using Plane.Copters; +using Plane.Geography; +using Plane.Logging; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTask + { + public async Task RunCircleTaskAsync() + { + // 林俊清, 20150922, 首先一起调好方向。 + await Task.WhenAll( + SingleCopterInfos.Select(info => + { + int copterIndex = SingleCopterInfos.IndexOf(info); + // 当该飞机被标记时，跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + return Task.Run(()=> { }); + info.TargetHeading = info.CenterDirectionDeg; + return TurnTaskFlySingleCopterAsync(info); + }) + ).ConfigureAwait(false); + + await Task.WhenAll( + SingleCopterInfos.Select(info => CircleTaskFlySingleCopterAsync(info)) + ).ConfigureAwait(false); + } + + private async Task CircleTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) + { + // 林俊清, 20150907, 目前不对 Circle 进行仿真。 + if (info.Copter is FakeCopter) + { + await Task.Delay(1000); + return; + } + + int copterIndex = SingleCopterInfos.IndexOf(info); + // 当该飞机被标记时，跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + return; + + await info.Copter.SetParamAsync("CIRCLE_RADIUS", info.Radius); + + await info.Copter.SetParamAsync("CIRCLE_RATE", info.Rate); + + var center = GeographyUtils.CalcLatLngSomeMetersAway2D(info.Copter.Latitude, info.Copter.Longitude, info.CenterDirectionDeg, 10); + + await info.Copter.CircleAsync(); + + while (info.Copter.State != Copters.CopterState.Circling) + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(25).ConfigureAwait(false); + } + + await info.Copter.SetChannelsAsync(ch3: info.Channel3); + + for (int i = info.Turns; i > 0; i--) + { + //// 等待一段时间，让飞行器飞出一段距离。 + //for (int i = 0; i < 3000 / 25; i++) + //{ + // if (_flightTaskManager.IsPaused == true) + // { + // info.Copter.Hover(); + // return; + // } + // await Task.Delay(25).ConfigureAwait(false); + //} + + //var log = new StringBuilder(1024); + //var lastLogTime = DateTime.Now; + bool step1 = false; + while (true) + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + var angle = info.Radius == 0 ? NormalizeAngleDeg(info.Copter.Heading - info.CenterDirectionDeg) : GetNormalizedAngleDeg(info, center); + //log.AppendLine($"{angle} {step1}"); + if (!step1) + { + if (angle > 100 && angle < 330) + { + step1 = true; + //log.AppendLine($"step1 is true now."); + } + } + else if (angle > 340 || angle < 5) + { + //log.AppendLine("break"); + break; + } + //if (lastLogTime.AddSeconds(10) < DateTime.Now) + //{ + // Logger.Instance.Log(log.ToString()); + // log.Clear(); + // lastLogTime = DateTime.Now; + //} + await Task.Delay(25).ConfigureAwait(false); + } + //Logger.Instance.Log(log.ToString()); + } + + await info.Copter.HoverAsync(); + } + + private static double GetNormalizedAngleDeg(FlightTaskSingleCopterInfo info, Tuple center) + { + var angle = info.Copter.InPlaneDirectionToDeg(center.Item1, center.Item2) - info.CenterDirectionDeg; + return NormalizeAngleDeg(angle); + } + + private static double NormalizeAngleDeg(double angle) + { + while (true) + { + if (angle < 0) angle += 360; + else if (angle >= 360) angle -= 360; + else break; + } + return angle; + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs new file mode 100644 index 0000000..b4bbe4b --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs @@ -0,0 +1,738 @@ +using Plane.Copters; +using Plane.Geography; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Windows.Messages; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTask + { + private bool _StaggerRoutes = true; + public bool StaggerRoutes + { + get { return _StaggerRoutes; } + set { Set(nameof(StaggerRoutes), ref _StaggerRoutes, value); } + } + + private bool _VerticalLift = false; + public bool VerticalLift // 垂直升降标志位，后面需要加入即使拖动地图上的飞机，也不会变化经纬度. added by ZJF + { + get { return _VerticalLift; } + set + { + if (value) + { + int currentIndex = _flightTaskManager.SelectedTaskIndex; + var currentCopterInfos = _flightTaskManager.SelectedTask.SingleCopterInfos; + var previousCopterInfos = _flightTaskManager.Tasks[currentIndex - 1].SingleCopterInfos; + for (int i = 0; i < currentCopterInfos.Count; i++) + { + currentCopterInfos[i].TargetLat = previousCopterInfos[i].TargetLat; + currentCopterInfos[i].TargetLng = previousCopterInfos[i].TargetLng; + } + } + + Set(nameof(VerticalLift), ref _VerticalLift, value); + } + } + + 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 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 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() // 全部飞到指定航点 + { + + 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 Task.WhenAll(tasks).ConfigureAwait(false); + + } + else + { + await Task.WhenAll( + SingleCopterInfos.Select(info => FlyToTaskFlySingleCopterAsync(info)) + ).ConfigureAwait(false); + } + } + + /* + 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; + int copterIndex = SingleCopterInfos.IndexOf(info); + + if ((bool)_copterManager.CopterStatus[copterIndex]) + return; + + 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) // 只发送最终目标点 + { + 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 Task.Delay(10).ConfigureAwait(false); + } + pointStage++; + + 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); + + + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + if (ts.TotalMilliseconds > 500) // 每200ms发送一遍指点坐标 + { + 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; + } + dtLastTime = dtNow; + } + + } + // await info.Copter.HoverAsync(); + if (taskIndex == _flightTaskManager.Tasks.Count() - 1) + { + await info.Copter.HoverAsync(); + } + } + + + private static bool CheckCrossing(List infos, int currentIndex) + { + var info = infos[currentIndex]; + for (int i = 0; i < currentIndex; i++) + { + var nextInfo = infos[i]; + if (GeographyUtils.CheckCrossing2D(info.Copter.Latitude, info.Copter.Longitude, info.TargetLat, info.TargetLng, + nextInfo.Copter.Latitude, nextInfo.Copter.Longitude, nextInfo.TargetLat, nextInfo.TargetLng)) + { + return true; + } + } + return false; + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs b/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs new file mode 100644 index 0000000..15e0ba5 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTask_LoiterTime.cs @@ -0,0 +1,474 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTask + { + private float _LoiterTimeAttr = 0.0f; + public float LoiterTimeAttr + { + get { return _LoiterTimeAttr; } + set + { + Set(nameof(LoiterTimeAttr), ref _LoiterTimeAttr, value); + } + } + + // LED灯闪烁 + private bool _flashAttr = false; + public bool flashAttr + { + get { return _flashAttr; } + set + { + Set(nameof(flashAttr), ref _flashAttr, value); + } + } + + // LED闪烁次数 + private float _flashPeriodAttr = 1.0f; + public float flashPeriodAttr + { + get { return _flashPeriodAttr; } + set + { + Set(nameof(flashPeriodAttr), ref _flashPeriodAttr, value); + } + } + + // 闪烁的飞机名字编号 + private string _flashCopterNameArray = ""; + public string flashCopterNameArray + { + get { return _flashCopterNameArray; } + set + { + Set(nameof(flashCopterNameArray), ref _flashCopterNameArray, value); + } + } + + // 闪烁的飞机序号编号 + private string _flashCopterIndexArray = ""; + public string flashCopterIndexArray + { + get { return _flashCopterIndexArray; } + set + { + Set(nameof(flashCopterIndexArray), ref _flashCopterIndexArray, value); + } + } + + // LED走马灯 + private bool _oneByOneAttr = false; + public bool oneByOneAttr + { + get { return _oneByOneAttr; } + set + { + Set(nameof(oneByOneAttr), ref _oneByOneAttr, value); + } + } + + // LED走马灯次数 + private float _oneByOnePeriodAttr = 1.0f; + public float oneByOnePeriodAttr + { + get { return _oneByOnePeriodAttr; } + set + { + Set(nameof(oneByOnePeriodAttr), ref _oneByOnePeriodAttr, value); + } + } + + private int[] columnFirst = {0, 1, 2, 3, 4 }; + private int[] columnSecond = { 5, 6, 7, 8, 9 }; + private int[] columnThird = { 10, 11, 12, 13, 14 }; + private int[] columnFourth = { 15, 16, 17, 18, 19 }; + private int[] columnFive = { 20, 21, 22, 23, 24 }; + private int[] columnSix = { 25, 26, 27, 28, 29 }; + + private int[] rowFirst = { 0, 5, 10, 15, 20, 25 }; + private int[] rowSecond = { 1, 6, 11, 16, 21, 26 }; + private int[] rowThird = { 2, 7, 12, 17, 22, 27 }; + private int[] rowFourth = { 3, 8, 13, 18, 23, 28 }; + private int[] rowFive = { 4, 9, 14, 19, 24, 29 }; + + private int[] rowFirstForEight = { 0, 1, 2, 3 }; + private int[] rowSecondForEight = { 4, 5, 6, 7 }; + + private int[] doubleColumnFirst = { 0, 1, 2, 3, 4, 25, 26, 27, 28, 29 }; + private int[] doubleColumnSecond = { 5, 6, 7, 8, 9, 20, 21, 22, 23, 24 }; + private int[] doubleColumnThird = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19 }; + + private int[] circleFirst = { 0, 1, 2, 3, 4, 9, 14, 19, 24, 29, 28, 27, 26, 25, 20, 15, 10, 5 }; + private int[] circleSecond = { 6, 7, 8, 13, 18, 23, 22, 21, 16, 11 }; + private int[] circleThird = { 12, 17 }; + + private int[] lineFirst = { 4 }; + private int[] lineSecond = { 3, 9 }; + private int[] lineThird = { 2, 8, 14 }; + private int[] lineFourth = { 1, 7, 13, 19 }; + private int[] lineFive = { 0, 6, 12, 18, 24 }; + private int[] lineSix = { 5, 11, 17, 23, 29 }; + private int[] lineSeven = { 10, 16, 22, 28 }; + private int[] lineEight = { 15, 21, 27 }; + private int[] lineNine = { 20, 26 }; + private int[] lineTen = { 25 }; + + public async Task RunLoiterTimeTaskAsync() + { + // 判断flashPeriodAttr, oneByOnePeriodAttr的值，执行不同的闪烁模式 + // 当flashPeriodAttr小于10，正常闪烁，即没有预制闪烁模式 + // 当flashPeriodAttr大于等于10，小于20，执行预制闪烁模式 + // 当flashPeriodAttr大于等于20， 改变飞控闪烁模式，以oneByOnePeriodAttr作为周期值 + + if ((flashPeriodAttr >= 10.0f) && (flashPeriodAttr < 20.0f)) + { + await LEDFlashPlanAsync(); + return; + } + + if (flashPeriodAttr >= 20.0f) + { + await LEDFlashParaModifyPlanAsync(); + return; + } + + + var infos = SingleCopterInfos; + if (flashAttr) // LED闪烁显示效果 + { + if (flashCopterIndexArray.Equals("")) + return; + string[] copterArray = flashCopterIndexArray.Split(','); + + var tasks_selected = new Task[copterArray.Length]; + + // LED灯全灭 + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false))); + await Task.Delay(1000).ConfigureAwait(false); + + // 选中的飞机LED灯全亮 + for (int i = 0; i < copterArray.Length; i++) + { + int index = int.Parse(copterArray[i]); + var info = infos[index]; + + tasks_selected[i] = await Task.Factory.StartNew(async () => + { + var internalInfo = info; + await LEDFlashTaskFlySingleCopterAsync(internalInfo, true); + }); + } + await Task.WhenAll(tasks_selected).ConfigureAwait(false); + await Task.Delay(3000).ConfigureAwait(false); + + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true))); + await Task.Delay(1000).ConfigureAwait(false); + + + } + else if (oneByOneAttr) // LED走马灯显示效果 + { + if (flashCopterNameArray.Equals("")) + return; + string[] copterArray = flashCopterIndexArray.Split(','); + + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false))); + await Task.Delay(1000).ConfigureAwait(false); + + // LED灯一个接一个全亮 + for (int i = 0; i < copterArray.Length; i++) + { + int index = int.Parse(copterArray[i]); + var info = infos[index]; + await LEDFlashTaskFlySingleCopterAsync(info, true); + await Task.Delay(200).ConfigureAwait(false); + } + + await Task.Delay(1000).ConfigureAwait(false); + + // LED灯全亮 + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true))); + await Task.Delay(1000).ConfigureAwait(false); + + + } + else // 没有LED显示效果 + { + await Task.Delay((int)(LoiterTimeAttr * 1000)).ConfigureAwait(false); + } + + } + + private async Task LEDFlashParaModifyPlanAsync() + { + /* + await LEDFlashParaModifyAsync(rowFirstForEight); + await Task.Delay(500).ConfigureAwait(false); + + await LEDFlashParaModifyAsync(rowSecondForEight); + await Task.Delay(500).ConfigureAwait(false); + */ + + int copterCount = SingleCopterInfos.Count(); + int[] intArray = new int[copterCount]; + for (int ii=0; ii + { + var internalInfo = info; + await LEDParaModifySingleCopterAsync(internalInfo, oneByOnePeriodAttr); + }); + } + await Task.WhenAll(tasks_selected).ConfigureAwait(false); + } + + private async Task LEDParaModifySingleCopterAsync(FlightTaskSingleCopterInfo info, float count) + { + var copter = info.Copter; + + var tasks = new Task[2]; + tasks[0] = Task.Run(async () => + { + try + { + await copter.SetParamAsync("NOTI_GPSLED", count, 5000); + } + catch (TimeoutException ex) + { + _logger.Log($"NOTI_GPSLED 超时, {ex.Message}, CopterId: {copter.Id}。"); + } + }); + tasks[1] = Task.Run(async () => + { + try + { + await copter.SetParamAsync("NOTI_ARMLED", count, 5000); + } + catch (TimeoutException ex) + { + _logger.Log($"NOTI_ARMLED 超时, {ex.Message}, CopterId: {copter.Id}。"); + } + }); + await Task.WhenAll(tasks).ConfigureAwait(false); + } + + /** + * isOn为true，打开灯；isOn为false，关闭灯 + */ + private async Task LEDFlashTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info, bool isOn) + { + var copter = info.Copter; + // float gpsLed = await c.GetParamAsync("NOTI_GPSLED"); + float ledControl = 0.0f; + if (isOn) + { + ledControl = 1.0f; + } + var tasks = new Task[2]; + tasks[0] = Task.Run(async () => + { + try + { + await copter.SetParamAsync("NOTI_GPSLED", ledControl, 5000); + } + catch (TimeoutException ex) + { + _logger.Log($"NOTI_GPSLED 超时, {ex.Message}, CopterId: {copter.Id}。"); + } + }); + tasks[1] = Task.Run(async () => + { + try + { + await copter.SetParamAsync("NOTI_ARMLED", ledControl, 5000); + } + catch (TimeoutException ex) + { + _logger.Log($"NOTI_ARMLED 超时, {ex.Message}, CopterId: {copter.Id}。"); + } + }); + await Task.WhenAll(tasks).ConfigureAwait(false); + } + + private async Task LEDFlashPlanAsync() + { + + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false))); + await Task.Delay(500).ConfigureAwait(false); + + await LEDColumnFlashAsync(); + await Task.Delay(500).ConfigureAwait(false); + + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false))); + await Task.Delay(500).ConfigureAwait(false); + + await LEDDoubleColumnFlashAsync(); + await Task.Delay(500).ConfigureAwait(false); + + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false))); + await Task.Delay(500).ConfigureAwait(false); + + await LEDLineFlashAsync(); + await Task.Delay(500).ConfigureAwait(false); + + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false))); + await Task.Delay(500).ConfigureAwait(false); + + await LEDCircleFlashAsync(); + await Task.Delay(500).ConfigureAwait(false); + + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false))); + await Task.Delay(200).ConfigureAwait(false); + + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true))); + await Task.Delay(200).ConfigureAwait(false); + + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false))); + await Task.Delay(200).ConfigureAwait(false); + + await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true))); + await Task.Delay(200).ConfigureAwait(false); + } + + // 一排一排闪烁 + private async Task LEDColumnFlashAsync() + { + await LEDArrayFlashAsync(columnFirst); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(columnSecond); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(columnThird); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(columnFourth); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(columnFive); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(columnSix); + await Task.Delay(200).ConfigureAwait(false); + } + + // 以圆圈收缩形式显示 + private async Task LEDCircleFlashAsync() + { + + await LEDArrayFlashAsync(circleFirst); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(circleSecond); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(circleThird); + await Task.Delay(200).ConfigureAwait(false); + } + + // 两列同时闪烁 + private async Task LEDDoubleColumnFlashAsync() + { + + await LEDArrayFlashAsync(doubleColumnFirst); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(doubleColumnSecond); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(doubleColumnThird); + await Task.Delay(200).ConfigureAwait(false); + } + + // 斜线方式闪烁 + private async Task LEDLineFlashAsync() + { + + await LEDArrayFlashAsync(lineFirst); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(lineSecond); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(lineThird); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(lineFourth); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(lineFive); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(lineSix); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(lineSeven); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(lineEight); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(lineNine); + await Task.Delay(200).ConfigureAwait(false); + + await LEDArrayFlashAsync(lineTen); + await Task.Delay(200).ConfigureAwait(false); + } + + private async Task LEDArrayFlashAsync(int[] LEDArray) + { + var infos = SingleCopterInfos; + + var tasks_selected = new Task[LEDArray.Length]; + for (int i = 0; i < LEDArray.Length; i++) + { + var info = infos[LEDArray[i]]; + + tasks_selected[i] = await Task.Factory.StartNew(async () => + { + var internalInfo = info; + await LEDFlashTaskFlySingleCopterAsync(internalInfo, true); + }); + } + await Task.WhenAll(tasks_selected).ConfigureAwait(false); + } + + } +} diff --git a/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs b/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs new file mode 100644 index 0000000..1d51279 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTask_ReturnToLand.cs @@ -0,0 +1,209 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTask + { + private float _RTLAlt = 15.0f; + public float RTLAlt + { + get { return _RTLAlt; } + set { Set(nameof(RTLAlt), ref _RTLAlt, value); } + } + + public async Task RunReturnToLandTaskAsync() + { + int TaskCount = _flightTaskManager.Tasks.Count(); + if (TaskCount > 1) + { + var infos = SingleCopterInfos; + // var infos = SingleCopterInfos.OrderByDescending(i => i.TargetAlt).ToList(); + var tasks = new Task[infos.Count]; + var tasksTmp = new Task[infos.Count]; + for (int i = 0; i < infos.Count; i++) + { + var info = infos[i]; + + // var copter = info.Copter; + if (info.RTLStage <= 1) // 当前阶段小于等于1时进入 + { + await ReturnToLandTaskFlySingleCopterAsync(info); + } + + tasksTmp[i] = ReturnToLandSecondTaskAsync(info); + + } + await Task.WhenAll(tasksTmp).ConfigureAwait(false); + if (_flightTaskManager.IsPaused == false) + { + for (int i = 0; i < infos.Count; i++) + { + var info = infos[i]; + info.RTLStage = 0; + } + } + } + + } + + private async Task ReturnToLandTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) + { + float takeOffAlt = 15.0f; + int TaskCount = _flightTaskManager.Tasks.Count(); + + int copterIndex = SingleCopterInfos.IndexOf(info); + var copter = info.Copter; + var copterPreviousTask = _flightTaskManager.Tasks[TaskCount-2].SingleCopterInfos[copterIndex]; // 倒数第二步的目标位置 + + // 当该飞机被标记时，跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + return; + + DateTime dtNow = DateTime.Now; + DateTime dtLastTime = DateTime.Now; + TimeSpan ts = dtNow - dtLastTime; + // 第一阶段：垂直飞行 + if (info.RTLStage == 0) + { + for (int j = 0; j < 3; j++) + { + await info.Copter.FlyToAsync(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt); + } + + while (!info.Copter.ArrivedTarget(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt)) + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(25).ConfigureAwait(false); + + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + if (ts.TotalMilliseconds > 1000) + { + for (int j = 0; j < 2; j++) + { + await info.Copter.FlyToAsync(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt); + } + dtLastTime = dtNow; + } + } + + info.RTLStage++; + } + + dtNow = DateTime.Now; + dtLastTime = DateTime.Now; + ts = dtNow - dtLastTime; + + var copterFirstTask = _flightTaskManager.Tasks[0].SingleCopterInfos[copterIndex]; // 第一步记录的家位置 + // 第二阶段：水平飞行 + if (info.RTLStage == 1) + { + for (int j = 0; j < 3; j++) + { + await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt); + } + + while (!info.Copter.ArrivedTarget(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt)) + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(25).ConfigureAwait(false); + + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + if (ts.TotalMilliseconds > 1000) + { + for (int j = 0; j < 2; j++) + { + await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt); + } + dtLastTime = dtNow; + } + } + + info.RTLStage++; + } + + } + + private async Task ReturnToLandSecondTaskAsync(FlightTaskSingleCopterInfo info) + { + // await Task.Run(async () => + // { + int copterIndex = SingleCopterInfos.IndexOf(info); + + // 当该飞机被标记时，跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + return; + + DateTime dtNow = DateTime.Now; + DateTime dtLastTime = DateTime.Now; + TimeSpan ts = dtNow - dtLastTime; + + float landAlt = 4.0f; // 飞机降落到4m再切land模式 + + var copterFirstTask = _flightTaskManager.Tasks[0].SingleCopterInfos[copterIndex]; // 第一步记录的家位置 + // 第三阶段 + if (info.RTLStage == 2) + { + for (int j = 0; j < 3; j++) + { + await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, landAlt); + } + + while (!info.Copter.ArrivedTarget(copterFirstTask.TargetLat, copterFirstTask.TargetLng, landAlt)) + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(25).ConfigureAwait(false); + + dtNow = DateTime.Now; + ts = dtNow - dtLastTime; + if (ts.TotalMilliseconds > 1000) + { + for (int j = 0; j < 2; j++) + { + await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, landAlt); + } + dtLastTime = dtNow; + } + } + + info.RTLStage++; + } + + // 切到land模式 + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + var copter = info.Copter; + if (info.RTLStage == 3) // 下降过程 + { + for (int j = 0; j < 5; j++) // added by ZJF + { + // await copter.ReturnToLaunchAsync(); + await copter.LandAsync(); // 修改为降落模式 + await Task.Delay(10); + } + } + + // }); + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTask_SimpleCircle.cs b/Plane.FormationCreator/Formation/FlightTask_SimpleCircle.cs new file mode 100644 index 0000000..db34e16 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTask_SimpleCircle.cs @@ -0,0 +1,24 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTask + { + public async Task RunSimpleCircleTaskAsync() + { + var center = SingleCopterInfos.Select(info => info.Copter).GetCenter().Value; + + // 林俊清, 20150921, 自动计算圆心方向和半径，然后直接调用普通画圈的方法。 + foreach (var info in SingleCopterInfos) + { + info.CenterDirectionDeg = (short)info.Copter.InPlaneDirectionToDeg(center.Lat, center.Lng); + info.Radius = (int)(info.Copter.InPlaneDistanceTo(center.Lat, center.Lng) * 100); + } + await RunCircleTaskAsync().ConfigureAwait(false); + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs new file mode 100644 index 0000000..0d2ba85 --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs @@ -0,0 +1,299 @@ +using Plane.Copters; +using Microsoft.Practices.ServiceLocation; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Plane.Geography; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTask + { + public async Task RunTakeOffTaskAsync() + { + // float takeOffAlt = 15; + int TaskCount = _flightTaskManager.Tasks.Count(); + if (TaskCount > 1) + { + var infos = SingleCopterInfos; + // var tasks = new Task[infos.Count]; + var tasksTmp = new Task[infos.Count]; + for (int i = 0; i < infos.Count; i++) + { + var info = infos[i]; + + if (info.takeOffStage == 0) // 第一阶段：起飞到10m + { + await TakeOffTaskFlySingleCopterAsync(info); + //if (_flightTaskManager.IsPaused == false) + //{ + // info.takeOffStage++; + //} + } + + tasksTmp[i] = TakeOffSecondTaskAsync(info); // 第二和第三阶段 + } + await Task.WhenAll(tasksTmp).ConfigureAwait(false); + if (_flightTaskManager.IsPaused == false) + { + for (int i = 0; i < infos.Count; i++) + { + var info = infos[i]; + info.takeOffStage = 0; + } + } + + } + } + + // 几架飞机同时起飞，参数为takeOffCount + public async Task RunTakeOffTask2Async() + { + int TaskCount = _flightTaskManager.Tasks.Count(); + if (TaskCount > 1) + { + var infos = SingleCopterInfos; + + int takeOffCount = 5; + int copterCount = infos.Count; + int integerPart = copterCount / takeOffCount; + int residualPart = copterCount % takeOffCount; + + // var tasks = new Task[infos.Count]; + var tasksTmp = new Task[infos.Count]; + for (int i = 0; i < integerPart; i++) + { + var tasksTakeOff = new Task[takeOffCount]; + for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++) + { + var info = infos[j]; + + int indexTmp = j - takeOffCount * i; + if (info.takeOffStage == 0) // 第一阶段：起飞到10m + { + tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info); + } + else + { + tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); }); + } + } + await Task.WhenAll(tasksTakeOff).ConfigureAwait(false); + + for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++) + { + var info = infos[j]; + tasksTmp[j] = TakeOffSecondTaskAsync(info); // 第二和第三阶段 + } + } + + // 余数架飞机同时起飞 + if (residualPart > 0) + { + var tasksTakeOff = new Task[residualPart]; + for (int j = integerPart * takeOffCount; j < takeOffCount * integerPart + residualPart; j++) + { + var info = infos[j]; + + int indexTmp = j - takeOffCount * integerPart; + if (info.takeOffStage == 0) // 第一阶段：起飞到10m + { + tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info); + } + else + { + tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); }); + } + } + await Task.WhenAll(tasksTakeOff).ConfigureAwait(false); + + for (int j = integerPart * takeOffCount; j < takeOffCount * integerPart + residualPart; j++) + { + var info = infos[j]; + tasksTmp[j] = TakeOffSecondTaskAsync(info); // 第二和第三阶段 + } + } + await Task.WhenAll(tasksTmp).ConfigureAwait(false); + if (_flightTaskManager.IsPaused == false) + { + for (int i = 0; i < infos.Count; i++) + { + var info = infos[i]; + info.takeOffStage = 0; + } + } + + } + } + + private CopterManager _copterManager = ServiceLocator.Current.GetInstance(); + + private async Task TakeOffTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) + { + int copterIndex = SingleCopterInfos.IndexOf(info); + var copter = info.Copter; + var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex]; + + if ((bool)_copterManager.CopterStatus[copterIndex]) + return; + + if (_flightTaskManager.IsPaused == true) + { + return; + } + await copter.UnlockAsync(); + for (int i = 0; !copter.IsUnlocked; i++) + { + //if (_flightTaskManager.IsPaused == true) + //{ + // return; + //} + if (i % (1000 / 25) == 1000 / 25 - 1) + { + await copter.UnlockAsync(); // 每 1000 毫秒重试一次。 + } + await Task.Delay(25).ConfigureAwait(false); + } + + // 为了返航，记录家的位置, 应该放在起飞命令 + info.TargetLat = info.Copter.Latitude; + info.TargetLng = info.Copter.Longitude; + + for (int i = 0; i < 5; i++) // added by ZJF + { + await copter.TakeOffAsync(); + await Task.Delay(50).ConfigureAwait(false); + } + + // while (copter.Altitude < 4 || copter.State == Copters.CopterState.TakingOff) + while (copter.Altitude < 5) // 修改起飞逻辑，当高度达到5米时，下一架开始起飞 + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(100).ConfigureAwait(false); + } + +/* //先不要这个控制看能否正常工作 + if (copter.Altitude > 8) + { + await info.Copter.GuidAsync(); + return; + } +*/ + } + + private async Task TakeOffSecondTaskAsync(FlightTaskSingleCopterInfo info) + { + float takeOffAlt = 15; + int copterIndex = SingleCopterInfos.IndexOf(info); + var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex]; + + // 当该飞机被标记时，跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + return; + // await Task.Run(async () => + // { + // 修改起飞逻辑，当高度达到5米时，下一架开始起飞 + if (info.takeOffStage == 0) + { + while (info.Copter.Altitude < 9) + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(100).ConfigureAwait(false); + } + if (_flightTaskManager.IsPaused == false) + { + info.takeOffStage++; + } + } + + DateTime dtNow = DateTime.Now; + DateTime dtLastTime = DateTime.Now; + TimeSpan ts = dtNow - dtLastTime; + + // 第二阶段：水平飞行 + if (info.takeOffStage == 1) + { + await info.Copter.GuidAsync(); + for (int j = 0; j < 3; j++) + { + await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt); + await Task.Delay(10).ConfigureAwait(false); + } + + while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt)) + { + 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 j = 0; j < 2; j++) + { + await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt); + await Task.Delay(10).ConfigureAwait(false); + } + dtLastTime = dtNow; + } + } + if (_flightTaskManager.IsPaused == false) + { + info.takeOffStage++; + } + } + + dtNow = DateTime.Now; + dtLastTime = DateTime.Now; + ts = dtNow - dtLastTime; + // 第三阶段：垂直飞行 + if (info.takeOffStage == 2) + { + for (int j = 0; j < 3; j++) + { + await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt); + await Task.Delay(10).ConfigureAwait(false); + } + + while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.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 j = 0; j < 2; j++) + { + await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt); + await Task.Delay(10).ConfigureAwait(false); + } + dtLastTime = dtNow; + } + } + } + + // }); + } + + } +} diff --git a/Plane.FormationCreator/Formation/FlightTask_Turn.cs b/Plane.FormationCreator/Formation/FlightTask_Turn.cs new file mode 100644 index 0000000..6f428de --- /dev/null +++ b/Plane.FormationCreator/Formation/FlightTask_Turn.cs @@ -0,0 +1,41 @@ +using Plane.Copters; +using Plane.Logging; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public partial class FlightTask + { + public async Task RunTurnTaskAsync() + { + await Task.WhenAll( + SingleCopterInfos.Select(info => TurnTaskFlySingleCopterAsync(info)) + ).ConfigureAwait(false); + } + + private async Task TurnTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info) + { + int copterIndex = SingleCopterInfos.IndexOf(info); + // 当该飞机被标记时，跳过飞行任务 + if ((bool)_copterManager.CopterStatus[copterIndex]) + return; + + await info.Copter.SetMobileControlAsync(yaw: info.TargetHeading); + while (!info.Copter.ArrivedHeading(info.TargetHeading)) + { + if (_flightTaskManager.IsPaused == true) + { + await info.Copter.HoverAsync(); + return; + } + await Task.Delay(25).ConfigureAwait(false); + } + + await info.Copter.HoverAsync(); + } + } +} diff --git a/Plane.FormationCreator/Formation/FormationController.cs b/Plane.FormationCreator/Formation/FormationController.cs new file mode 100644 index 0000000..bd9e060 --- /dev/null +++ b/Plane.FormationCreator/Formation/FormationController.cs @@ -0,0 +1,734 @@ +using Plane.Communication; +using Plane.Copters; +using Plane.Logging; +using Newtonsoft.Json; +using System; +using System.Collections.Generic; +using System.Collections.Concurrent; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Threading; +using System.Diagnostics; +using static Plane.Copters.Constants; +using Plane.Geography; + +namespace Plane.FormationCreator.Formation +{ + public class FormationController + { + public FormationController(ILogger logger, CopterManager copterManager) + { + _logger = logger; + _copterManager = copterManager; + + foreach (var copter in _copterManager.Copters) + { + _guides.Add(copter, null); + } + _copterManager.Copters.CollectionChanged += (sender, e) => + { + if (e.OldItems != null) + { + foreach (ICopter copter in e.OldItems) + { + if (_guides.ContainsKey(copter)) + { + _guides.Remove(copter); + } + } + } + if (e.NewItems != null) + { + foreach (ICopter copter in e.NewItems) + { + if (!_guides.ContainsKey(copter)) + { + _guides.Add(copter, null); + } + } + } + }; + //var thread = new Thread(() => + //{ + // while (true) + // { + // Control(); + // Thread.Sleep(50); + // } + //}) + //{ + // IsBackground = true + //}; + //thread.Start(); + var timer = new System.Timers.Timer(INTERVAL); + timer.Elapsed += (sender, e) => Control(); + timer.Start(); + } + + private const int INTERVAL = 50; + + private bool _shouldStop; + private Dictionary _guides = new Dictionary(); + + private ILogger _logger; + private CopterManager _copterManager; + + #region 检查是否有飞行器在某状态的方法 + + private bool AnyCopterWithGuide() + { + try + { + return _guides.Any(item => item.Value != null); + } + catch (InvalidOperationException ex) // 集合已修改。 + { + _logger.Log(ex.ToString(), Category.Info, Priority.None); + return true; // 安全起见返回 true，让调用者再次检查。 + } + } + + private bool AnyCopterWithGuide(IEnumerable copters) + { + try + { + return copters.Any(c => _guides[c] != null); + } + catch (InvalidCastException ex) // 集合已修改。 + { + _logger.Log(ex.ToString(), Category.Info, Priority.None); + return true; // 安全起见返回 true，让调用者再次检查。 + } + } + + private async Task EnsureAllTasksFinishedAsync() + { + await Task.Run(async () => + { + while (AnyCopterWithGuide()) + { + await Task.Delay(INTERVAL).ConfigureAwait(false); + } + }).ConfigureAwait(false); + } + + private async Task EnsureCopterTasksFinishedAsync(IEnumerable copters) + { + await Task.Run(async () => + { + while (AnyCopterWithGuide(copters)) + { + await Task.Delay(INTERVAL).ConfigureAwait(false); + } + }).ConfigureAwait(false); + } + + private async Task EnsureTaskFinishedAsync(ICopter copter) + { + while (_guides[copter] != null) + { + await Task.Delay(INTERVAL).ConfigureAwait(false); + } + } + + #endregion 检查是否有飞行器在某状态的方法 + + private void Control() + { + try + { + var copters = _guides.Keys.ToList(); + + Parallel.ForEach(copters, (copter, loopState) => + { + try + { + if (_shouldStop) + { + loopState.Stop(); + return; + } + var guide = _guides[copter]; + if (guide == null) + { + return; + } + if (!guide.IsLoop && guide.FinishPredicate?.Invoke() == true) + { + var finishAction = guide?.FinishAction; + _guides[copter] = null; + copter.HoverAsync().Wait(); + finishAction?.Invoke(); + } + else + { + guide.UpdateAction?.Invoke(); + } + } + catch (Exception ex) + { + _logger.Log(ex); + } + }); + + if (_shouldStop) + { + Parallel.ForEach(copters, copter => + { + try + { + var finishAction = _guides[copter]?.FinishAction; + _guides[copter] = null; + copter.HoverAsync().Wait(); + finishAction?.Invoke(); + } + catch (Exception ex) + { + _logger.Log(ex); + } + }); + } + } + catch (Exception ex) + { + _logger.Log(ex); + } + } + + public async Task AllStop() + { + Parallel.ForEach(_copterManager.Copters, copter => copter.HoverAsync()); + + if (!_shouldStop && AnyCopterWithGuide()) + { + _shouldStop = true; + while (AnyCopterWithGuide()) + { + await Task.Delay(INTERVAL).ConfigureAwait(false); + } + _shouldStop = false; + } + + Parallel.ForEach(_copterManager.Copters, copter => copter.HoverAsync().Wait()); + } + + public async Task TurnAsync(IEnumerable copters, double targetLat, double targetLng) + { + await AllStop(); + + foreach (var copter in copters) + { + var guide = !_guides.ContainsKey(copter) || _guides[copter] == null ? (_guides[copter] = new Guide()) : _guides[copter]; + var targetHeading = (float)GeographyUtils.RadToDeg(GeographyUtils.CalcDirection2D(copter.Latitude, copter.Longitude, targetLat, targetLng)); + guide.FinishPredicate = () => Math.Abs(copter.Heading - targetHeading) < 1; + guide.UpdateAction = () => + { + copter.MoveToIdealState(new CopterState + { + Heading = targetHeading + }); + }; + } + + await EnsureAllTasksFinishedAsync().ConfigureAwait(false); + } + + public async Task FlyToLatLngAsync(ICopter copter, double targetLat, double targetLng) + { + const double DT = INTERVAL / 1000.0; + const double DECELERATE_DISTANCE = 15; + double idealVel = 0; + double idealMovedDistance = 0; + var startLat = copter.Latitude; + var startLng = copter.Longitude; + var startTargetDistance = GeographyUtils.CalcDistance2D(startLat, startLng, targetLat, targetLng); + var idealMoveDirectionRadians = (float)GeographyUtils.CalcDirection2D(startLat, startLng, targetLat, targetLng); + var idealMoveDirectionDegrees = (float)GeographyUtils.RadToDeg(idealMoveDirectionRadians); + + var guide = !_guides.ContainsKey(copter) || _guides[copter] == null ? (_guides[copter] = new Guide()) : _guides[copter]; + guide.FinishPredicate = () => GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, targetLat, targetLng) <= 0.5; + var log = new List(); + guide.UpdateAction = () => + { + if (idealVel < MAX_VEL) + { + idealVel += MAX_ACCEL * DT; + if (idealVel > MAX_VEL) idealVel = MAX_VEL; + } + var distance = GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, targetLat, targetLng); + if (distance < DECELERATE_DISTANCE) idealVel = MAX_VEL * distance / DECELERATE_DISTANCE; + + double idealLat, idealLng; + if (idealMovedDistance < startTargetDistance) + { + idealMovedDistance += idealVel * DT; + if (idealMovedDistance > startTargetDistance) + { + idealLat = targetLat; + idealLng = targetLng; + } + else + { + var idealLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(startLat, startLng, idealMoveDirectionDegrees, (float)idealMovedDistance); + idealLat = idealLatLng.Item1; + idealLng = idealLatLng.Item2; + } + } + else + { + idealLat = targetLat; + idealLng = targetLng; + } + + copter.MoveToIdealState(new CopterState + { + Latitude = idealLat, + Longitude = idealLng, + Heading = idealMoveDirectionDegrees, + VelocityDirection = idealMoveDirectionRadians, + Velocity = idealVel + }); + + log.Add(new + { + time = DateTime.Now, + targetLat = idealLat, + targetLng = idealLng, + lat = copter.Latitude, + lng = copter.Longitude, + alt = copter.Altitude + }); + }; + guide.FinishAction = () => _logger.Log(JsonConvert.SerializeObject(log)); + + await EnsureAllTasksFinishedAsync().ConfigureAwait(false); + } + + public async Task FlyToLatLngAsync(IEnumerable copters, double targetLat, double targetLng) + { + await AllStop(); + + if (!copters.Any()) return; + + var centerLat = copters.Average(c => c.Latitude); + var centerLng = copters.Average(c => c.Longitude); + var latDelta = targetLat - centerLat; + var lngDelta = targetLng - centerLng; + + foreach (var copter in copters) + { + var copterTargetLat = copter.Latitude + latDelta; + var copterTargetLng = copter.Longitude + lngDelta; + var guide = !_guides.ContainsKey(copter) || _guides[copter] == null ? (_guides[copter] = new Guide()) : _guides[copter]; + guide.FinishPredicate = () => GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, copterTargetLat, copterTargetLng) <= 0.5; + guide.UpdateAction = () => + { + copter.MoveToIdealState(new CopterState + { + Latitude = copterTargetLat, + Longitude = copterTargetLng + }); + }; + } + + await EnsureAllTasksFinishedAsync().ConfigureAwait(false); + } + + public async Task FlyToAltitudeAsync(ICopter copter, float targetAlt) + { + var guide = _guides[copter] ?? (_guides[copter] = new Guide()); + guide.FinishPredicate = () => Math.Abs(copter.Altitude - targetAlt) <= 0.5; + guide.UpdateAction = () => + { + copter.MoveToIdealState(new CopterState + { + Altitude = targetAlt + }); + }; + + await EnsureTaskFinishedAsync(copter).ConfigureAwait(false); + } + + public async Task FlyToAltitudeAsync(IEnumerable copters, float targetAlt) + { + await AllStop(); + + foreach (var copter in copters) + { + if (!_guides.ContainsKey(copter)) + { + _guides.Add(copter, null); + } + var guide = _guides[copter] ?? (_guides[copter] = new Guide()); + guide.FinishPredicate = () => Math.Abs(copter.Altitude - targetAlt) <= 0.5; + guide.UpdateAction = () => + { + copter.MoveToIdealState(new CopterState + { + Altitude = targetAlt + }); + }; + } + + await EnsureAllTasksFinishedAsync().ConfigureAwait(false); + } + + public async Task FlyInCircleAsync(IEnumerable copters, float centerDirection, float radius, bool clockwise = true, bool loop = false, double? velLimit = null) + { + await AllStop(); + + var velMax = velLimit ?? 2.0; + var velMin = -velMax; + var angVelMax = velMax / radius; + var angVelMin = velMin / radius; + var angAccel = (clockwise ? MAX_ACCEL : -MAX_ACCEL) / radius; + + Parallel.ForEach(copters, copter => + { + var guide = _guides[copter] ?? (_guides[copter] = new Guide()); + guide.IsLoop = loop; + + var centerLat = copter.Latitude + radius * Math.Cos(GeographyUtils.DegToRad(centerDirection)) * GeographyUtils.METERS_TO_LAT_SPAN; + var centerLng = copter.Longitude + radius * Math.Sin(GeographyUtils.DegToRad(centerDirection)) * GeographyUtils.CalcMetersToLngSpan(copter.Latitude); + + var startingPointLat = copter.Latitude; + var startingPointLng = copter.Longitude; + + var angVel = 0.0; + const double DT = INTERVAL / 1000.0; + var angleTotal = 0.0; + var startAngle = GeographyUtils.CalcDirection2D(centerLat, centerLng, copter.Latitude, copter.Longitude); + + guide.FinishPredicate = () => angleTotal >= Math.PI * 2; + + var log = new List(); + + #region guide.UpdateAction + + guide.UpdateAction = () => + { + if (clockwise) + { + if (angVel < angVelMax) + { + angVel += angAccel * DT; + MathUtils.Constrain(ref angVel, 0, angVelMax); + } + } + else + { + if (angVel > angVelMin) + { + angVel += angAccel * DT; + MathUtils.Constrain(ref angVel, angVelMin, 0); + } + } + + var angleChange = angVel * DT; + angleTotal += angleChange; + var targetAngle = startAngle + angleTotal; + var targetLat = centerLat + Math.Cos(targetAngle) * radius * GeographyUtils.METERS_TO_LAT_SPAN; + var targetLng = centerLng + Math.Sin(targetAngle) * radius * GeographyUtils.CalcMetersToLngSpan(copter.Latitude); + + copter.MoveToIdealState(new CopterState + { + Latitude = targetLat, + Longitude = targetLng, + Heading = (float)GeographyUtils.RadToDeg(GeographyUtils.CalcDirection2D(targetLat, targetLng, centerLat, centerLng)), + VelocityDirection = GeographyUtils.CalcDirection2D(centerLat, centerLng, targetLat, targetLng) + + Math.PI / 2, + Velocity = angVel * radius + }); + + log.Add(new + { + time = DateTime.Now, + targetLat, + targetLng, + lat = copter.Latitude, + lng = copter.Longitude, + alt = copter.Altitude, + angAccel, + angVel, + angleChange, + angleTotal + }); + }; + + #endregion guide.UpdateAction + + guide.FinishAction = () => + { + _logger.Log(JsonConvert.SerializeObject(log)); + }; + }); + + await EnsureAllTasksFinishedAsync().ConfigureAwait(false); + } + + public async Task FlyAroundCenterOfCoptersAsync(IEnumerable copters, bool clockwise = true, bool loop = false, double? velLimit = null) + { + await AllStop(); + + var nullableCenter = copters.GetCenter(); + if (nullableCenter == null) return; + + var center = nullableCenter.Value; + + var radiuses = copters.ToDictionary(c => c, c => GeographyUtils.CalcDistance2D(c.Latitude, c.Longitude, center.Lat, center.Lng)); + var velMax = velLimit ?? 2.0; + var velMin = -velMax; + var angVelMax = velMax / radiuses.Values.Max(); + + Parallel.ForEach(copters, copter => + { + var guide = _guides[copter] ?? (_guides[copter] = new Guide()); + guide.IsLoop = loop; + + var startingPointLat = copter.Latitude; + var startingPointLng = copter.Longitude; + + var radius = radiuses[copter]; + + var angVelMin = velMin / radius; + var angAccel = (clockwise ? 10 : -10) / radius; + var angVel = 0.0; + const double DT = INTERVAL / 1000.0; + var angleTotal = 0.0; + var startAngle = GeographyUtils.CalcDirection2D(center.Lat, center.Lng, copter.Latitude, copter.Longitude); + + guide.FinishPredicate = () => angleTotal >= Math.PI * 2; + + var log = new List(); + + #region guide.UpdateAction + + guide.UpdateAction = () => + { + if (clockwise) + { + if (angVel < angVelMax) + { + angVel += angAccel * DT; + MathUtils.Constrain(ref angVel, 0, angVelMax); + } + } + else + { + if (angVel > angVelMin) + { + angVel += angAccel * DT; + MathUtils.Constrain(ref angVel, angVelMin, 0); + } + } + + var angleChange = angVel * DT; + angleTotal += angleChange; + var targetAngle = startAngle + angleTotal; + var targetLat = center.Lat + Math.Cos(targetAngle) * radius * GeographyUtils.METERS_TO_LAT_SPAN; + var targetLng = center.Lng + Math.Sin(targetAngle) * radius * GeographyUtils.CalcMetersToLngSpan(copter.Latitude); + + copter.MoveToIdealState(new CopterState + { + Latitude = targetLat, + Longitude = targetLng, + Heading = (float)GeographyUtils.RadToDeg(GeographyUtils.CalcDirection2D(targetLat, targetLng, center.Lat, center.Lng)), + VelocityDirection = GeographyUtils.CalcDirection2D(center.Lat, center.Lng, targetLat, targetLng) + + Math.PI / 2, + Velocity = angVel * radius + }); + + log.Add(new + { + time = DateTime.Now, + targetLat, + targetLng, + lat = copter.Latitude, + lng = copter.Longitude, + alt = copter.Altitude, + angAccel, + angVel, + angleChange, + angleTotal + }); + }; + + #endregion guide.UpdateAction + + guide.FinishAction = () => + { + _logger.Log(JsonConvert.SerializeObject(log)); + }; + }); + + await EnsureAllTasksFinishedAsync().ConfigureAwait(false); + } + + public Task FlyInRectangleAsync(IEnumerable copters, float startDirection, float sideLength1, float sideLength2, bool clockwise = true, bool loop = false) + { + throw new NotImplementedException(); + //await AllStop(); + + //Parallel.ForEach(copters, copter => + //{ + // const int pointCount = 4; + // var points = new LatLng[pointCount]; + // // 把起始点也加进去是为了循环飞行。 + // points[0] = new LatLng { Lat = copter.Latitude, Lng = copter.Longitude }; + + // var directionRad = startDirection * GeographyUtils.DegreesToRad; + // float distance; + // LatLng lastPoint; + + // for (int i = 1; i < pointCount; ++i) + // { + // distance = i % 2 != 0 ? sideLength1 : sideLength2; + // lastPoint = points[i - 1]; + // points[i] = new LatLng + // { + // Lat = lastPoint.Lat + distance * Math.Cos(directionRad) * GeographyUtils.MetersToLocalLat, + // Lng = lastPoint.Lng + distance * Math.Sin(directionRad) * GeographyUtils.GetMetersToLocalLon(lastPoint.Lat) + // }; + // if (clockwise) + // { + // directionRad += Math.PI / 2; + // } + // else + // { + // directionRad -= Math.PI / 2; + // } + // } + // var target = _guides[copter] ?? new Guide(); + // target.LatLngs = points; + // target.IsLoop = loop; + // target.CurrentIndex = 0; + // _guides[copter] = target; + + // _logger.Log(string.Join(Environment.NewLine, points.Select(p => $"{p.Lat} {p.Lng}")), Category.Debug, Priority.None); + //}); + + //await Task.Run(() => + //{ + // while (AnyCopterFlyingToLatLng()) + // { + // Thread.Sleep(100); + // } + //}); + } + + public async Task FlyToVerticalLineAndMakeCircle(IEnumerable copters, float centerDirection, float radius, bool clockwise = true, bool loop = false, double? verLimit = null) + { + await AllStop().ConfigureAwait(false); + + if (!copters.Any()) return; + + // 1. 飞到不同的高度。 + + int i = 0; + var tasks = copters.Select(copter => + { + var targetAlt = 40 + 5 * i++; + return FlyToAltitudeAsync(copter, targetAlt); + }); + await Task.WhenAll(tasks).ConfigureAwait(false); + + // 2. 飞到相同经纬度。 + + var targetLat = copters.Average(c => c.Latitude); + var targetLng = copters.Average(c => c.Longitude); + tasks = copters.Select(copter => + { + return FlyToLatLngAsync(copter, targetLat, targetLng); + }); + await Task.WhenAll(tasks).ConfigureAwait(false); + + // 3. 画圈。 + + await FlyInCircleAsync(copters, centerDirection, radius, clockwise, loop, verLimit); + } + } + + class Guide + { + public bool IsLoop { get; set; } + public Func FinishPredicate { get; set; } + public Action FinishAction { get; set; } + public Action UpdateAction { get; set; } + } + + struct CopterState + { + public double? Latitude { get; set; } + public double? Longitude { get; set; } + public float? Altitude { get; set; } + public float? Heading { get; set; } + public double VelocityDirection { get; set; } + public double Velocity { get; set; } + } + + static class ICopterExtensions + { + const double DECELERATE_DISTANCE = 5; + + internal static void MoveToIdealState(this ICopter copter, CopterState idealState) + { + ushort channel1, channel2; + CalcChannel1And2(copter, idealState, out channel1, out channel2); + + ushort channel3 = CalcChannel3(copter, idealState); + + //copter.SetChannels(new ChannelBag + //{ + // Channel1 = channel1, + // Channel2 = channel2, + // Channel3 = channel3 + //}); + copter.SetMobileControlAsync(ch1: channel1, ch2: channel2, ch3: channel3, yaw: idealState.Heading); + } + + private static void CalcChannel1And2(ICopter copter, CopterState idealState, out ushort channel1, out ushort channel2) + { + if (idealState.Latitude == null || idealState.Longitude == null) + { + channel1 = channel2 = HOVER_CHANNEL; + return; + } + + var distance = GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, idealState.Latitude.Value, idealState.Longitude.Value); + + var moveDirection = GeographyUtils.CalcDirection2D(copter.Latitude, copter.Longitude, idealState.Latitude.Value, idealState.Longitude.Value) + - copter.Heading.DegToRad(); + var coefficient = distance >= DECELERATE_DISTANCE ? 1.0 : distance / DECELERATE_DISTANCE; + var channelDelta1 = MAX_CHANNEL_DELTA * coefficient; + var ch1Delta1 = channelDelta1 * Math.Sin(moveDirection); + var ch2Delta1 = -channelDelta1 * Math.Cos(moveDirection); + + var channelDelta2 = MAX_CHANNEL_DELTA * idealState.Velocity / MAX_VEL; + var idealVelDirectionRelativeToHeading = idealState.VelocityDirection - copter.Heading.DegToRad(); + var ch1Delta2 = channelDelta2 * Math.Sin(idealVelDirectionRelativeToHeading); + var ch2Delta2 = -channelDelta2 * Math.Cos(idealVelDirectionRelativeToHeading); + + var ch1Delta = ch1Delta1 + ch1Delta2; + var ch2Delta = ch2Delta1 + ch2Delta2; + MathUtils.Constrain(ref ch1Delta, -MAX_CHANNEL_DELTA, MAX_CHANNEL_DELTA); + MathUtils.Constrain(ref ch2Delta, -MAX_CHANNEL_DELTA, MAX_CHANNEL_DELTA); + + channel1 = (ushort)(ch1Delta > 0 ? MAX_HOVER_CHANNEL + ch1Delta : MIN_HOVER_CHANNEL + ch1Delta); + channel2 = (ushort)(ch2Delta > 0 ? MAX_HOVER_CHANNEL + ch2Delta : MIN_HOVER_CHANNEL + ch2Delta); + } + + private static ushort CalcChannel3(ICopter copter, CopterState idealState) + { + if (idealState.Altitude == null) + { + return HOVER_CHANNEL; + } + + var distance = idealState.Altitude - copter.Altitude; + var coefficient = distance >= DECELERATE_DISTANCE ? 1.0 : distance / DECELERATE_DISTANCE; + var ch3Delta = MAX_CHANNEL_DELTA * coefficient; + return (ushort)(ch3Delta > 0 ? MAX_HOVER_CHANNEL + ch3Delta : MIN_HOVER_CHANNEL + ch3Delta); + } + } +} diff --git a/Plane.FormationCreator/Formation/LatLng.cs b/Plane.FormationCreator/Formation/LatLng.cs new file mode 100644 index 0000000..34a8dfa --- /dev/null +++ b/Plane.FormationCreator/Formation/LatLng.cs @@ -0,0 +1,24 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + [DebuggerDisplay("Lat={Lat}, Lng={Lng}")] + public struct LatLng + { + public LatLng(double lat, double lng) + { + this.Lat = lat; + this.Lng = lng; + } + + public static LatLng Empty { get { return new LatLng(-1, -1); } } + + public double Lat { get; set; } + public double Lng { get; set; } + } +} diff --git a/Plane.FormationCreator/Formation/MapManager.cs b/Plane.FormationCreator/Formation/MapManager.cs new file mode 100644 index 0000000..0c9040d --- /dev/null +++ b/Plane.FormationCreator/Formation/MapManager.cs @@ -0,0 +1,29 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public class MapManager + { + public LatLng Center + { + get { return _centerGetter.Invoke(); } + } + + public Views.MapView MapView { get; set; } + + private Func _centerGetter; + public void SetCenterGetter(Func centerGetter) + { + _centerGetter = centerGetter; + } + + public void ClearCopters() + { + this.MapView.ClearCopters(); + } + } +} diff --git a/Plane.FormationCreator/Formation/MapServer.cs b/Plane.FormationCreator/Formation/MapServer.cs new file mode 100644 index 0000000..a9dbd79 --- /dev/null +++ b/Plane.FormationCreator/Formation/MapServer.cs @@ -0,0 +1,138 @@ +using Plane.Logging; +using GalaSoft.MvvmLight.Ioc; +using Microsoft.Practices.ServiceLocation; +using System; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Net; +using System.Text; + +namespace Plane.FormationCreator.Formation +{ + public class MapServer : IDisposable + { + private HttpListener _listner = new HttpListener(); + private Dictionary _resources; + private int _port; + + private static ILogger _logger = ServiceLocator.Current.GetInstance(); + + public static MapServer Create(Dictionary resources) + { + const int ORIGINAL_PORT = 10240; + int port = ORIGINAL_PORT; + var r = new Random(); + MapServer _earthResourcesServer = null; + for (int i = 0; ; ++i) + { + _earthResourcesServer = new MapServer(resources, port); + try + { + _earthResourcesServer.Start(); + _earthResourcesServer._port = port; + if (port != ORIGINAL_PORT) + { + using (var reader = new StreamReader(resources["js/earth.js"])) + { + var ms = new MemoryStream(); + var writer = new StreamWriter(ms); + writer.Write(reader.ReadToEnd().Replace(ORIGINAL_PORT.ToString(), port.ToString())); + writer.Flush(); + resources["js/earth.js"] = ms; // The old one will be closed when the reader is closed. + } + } + return _earthResourcesServer; + } + catch (HttpListenerException ex) + { + _logger.Log(ex); + if (i >= 100) + { + // Alert.Show("找不到空闲端口，建议重启电脑后再试。", "Message", MessageBoxButton.OK); + App.Current.Shutdown(); + } + port = r.Next(ORIGINAL_PORT + 1, 65536); + } + } + } + + private MapServer(Dictionary resources, int port) + { + _resources = resources; + _listner.Prefixes.Add("http://localhost:" + port + "/"); + } + + private string _HomePageUrl; + public string HomePageUrl { get { return _HomePageUrl ?? (_HomePageUrl = $"http://localhost:{_port}/Cesium-1.9/Apps/HelloWorld.html"); } } + + public void Start() + { + _listner.Start(); + _listner.BeginGetContext(AfterGetContext, null); + } + + private void AfterGetContext(IAsyncResult ar) + { + if (!_listner.IsListening) return; + var context = _listner.EndGetContext(ar); + _listner.BeginGetContext(AfterGetContext, null); + + var response = context.Response; + string key = context.Request.Url.LocalPath.TrimStart('/').ToLower(); + response.ContentType = + key.EndsWith(".html") + ? "text/html" + : key.EndsWith(".css") + ? "text/css" + : key.EndsWith(".js") + ? "application/javascript" + : key.EndsWith(".gltf") + ? "application/json" + : key.EndsWith(".jpg") + ? "image/jpeg" + : key.EndsWith(".png") + ? "image/png" + : "text/plain"; + try + { + if (!_resources.ContainsKey(key)) + { + response.StatusCode = 404; + response.StatusDescription = "Not Found"; + } + else + { + _resources[key].Position = 0; + _resources[key].CopyTo(response.OutputStream); + response.OutputStream.Flush(); + } + } + catch (Exception ex) + { + _logger.Log(ex); + response.StatusCode = 500; + response.StatusDescription = "Internal Server Error"; + } + finally + { + response.Close(); + } + } + + public void Stop() + { + _listner.Stop(); + } + + public void Close() + { + _listner.Close(); + } + + void IDisposable.Dispose() + { + Close(); + } + } +} diff --git a/Plane.FormationCreator/Formation/PropertyChangedEventArgs.cs b/Plane.FormationCreator/Formation/PropertyChangedEventArgs.cs new file mode 100644 index 0000000..b913fa0 --- /dev/null +++ b/Plane.FormationCreator/Formation/PropertyChangedEventArgs.cs @@ -0,0 +1,14 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator.Formation +{ + public class PropertyChangedEventArgs : EventArgs + { + public T OldItem { get; set; } + public T NewItem { get; set; } + } +} diff --git a/Plane.FormationCreator/MainWindow.xaml b/Plane.FormationCreator/MainWindow.xaml new file mode 100644 index 0000000..e9d6da2 --- /dev/null +++ b/Plane.FormationCreator/MainWindow.xaml @@ -0,0 +1,132 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +