diff --git a/Plane.FormationCreator/App.xaml b/Plane.FormationCreator/App.xaml index bfd11e6..fdcfb47 100644 --- a/Plane.FormationCreator/App.xaml +++ b/Plane.FormationCreator/App.xaml @@ -75,9 +75,16 @@ + + + + + + + \ No newline at end of file diff --git a/Plane.FormationCreator/Converters/HeartbeatCountToBrushConverter.cs b/Plane.FormationCreator/Converters/HeartbeatCountToBrushConverter.cs index cc64b1d..81f7c99 100644 --- a/Plane.FormationCreator/Converters/HeartbeatCountToBrushConverter.cs +++ b/Plane.FormationCreator/Converters/HeartbeatCountToBrushConverter.cs @@ -1,44 +1,87 @@ -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; - - private static SolidColorBrush _FakeCopterBrush = Copter.DefaultFakeBrush; - - 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; - - if (values[1] is FakeCopter) - return _FakeCopterBrush; - - if (values[1] is PLCopter) - return _oneBrush; - - 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(); - } - } -} +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 _zeroBrush_fault = new SolidColorBrush(Color.FromRgb(160, 140, 0)); + + private static SolidColorBrush _oneBrush = Copter.DefaultBrush; + + private static SolidColorBrush _FakeCopterBrush = Copter.DefaultFakeBrush; + private static SolidColorBrush _PreCheckNopassBrush = Copter.YellowBrush; + private static SolidColorBrush _NoconnectedBrush = Copter.RedBrush; + + public object Convert(object[] values, Type targetType, object parameter, CultureInfo culture) + { + if (values[0] == DependencyProperty.UnsetValue) return _zeroBrush; + var heartbeatCount = (ulong)values[0]; + /* //绑定的是心跳，所以连接断开时不会调用这个转换器 + //如果是真飞机，并且断开了，直接显示红色 + if (values[1] is PLCopter) + { + var plcopter = values[1] as PLCopter; + if (!plcopter.CommModuleConnected) + return _NoconnectedBrush; + } + */ + + + + //用于显示收到心跳的闪烁 + if (heartbeatCount % 2 == 0) + { + //真飞机如果异常显示暗淡黄色 + if (values[1] is PLCopter) + { + var plcopter = values[1] as PLCopter; + if (plcopter.CopterPreCheckPass) + return _zeroBrush; + else return _zeroBrush_fault; + + } + else + return _zeroBrush; + + } + + + if (values[1] is FakeCopter) + { + var fkcopter = values[1] as FakeCopter; + if (fkcopter.CopterPreCheckPass) + return _FakeCopterBrush; + else return _PreCheckNopassBrush; + } + + if (values[1] is PLCopter) + { + var plcopter = values[1] as PLCopter; + if (plcopter.CopterPreCheckPass) + return _oneBrush; + else return _PreCheckNopassBrush; + + } + 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/Copter.cs b/Plane.FormationCreator/Formation/Copter.cs index b82209f..dd881a3 100644 --- a/Plane.FormationCreator/Formation/Copter.cs +++ b/Plane.FormationCreator/Formation/Copter.cs @@ -1,69 +1,75 @@ -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,false) - { - Brush = _brushes[NextBrushIndex()]; - - } - - public Copter(IConnection Connection, SynchronizationContext uiThreadContext, double lat, double lng) : base(Connection, uiThreadContext, false) - { - Brush = _brushes[NextBrushIndex()]; - RecordLat = lat; - RecordLng = lng; - Latitude = lat; - Longitude = lng; - //RaiseLocationChangedIfNeeded(); - } - - - internal static SolidColorBrush BlueBrush { get; } = new SolidColorBrush(Color.FromRgb(28, 151, 234)); - internal static SolidColorBrush RedBrush { get; } = new SolidColorBrush(Color.FromRgb(255, 100, 100)); - internal static SolidColorBrush GreenBrush { get; } = new SolidColorBrush(Color.FromRgb(100, 255, 100)); - internal static SolidColorBrush YellowBrush { get; } = new SolidColorBrush(Color.FromRgb(255, 215, 0)); - - - - 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; } = GreenBrush;// new SolidColorBrush(Color.FromRgb(50, 205, 50)); - //虚拟飞机列表默认颜色 - internal static SolidColorBrush DefaultFakeBrush { get; } = BlueBrush; - - - - - private SolidColorBrush _Brush; - public SolidColorBrush Brush - { - get { return _Brush ?? DefaultBrush; } - set { Set(nameof(Brush), ref _Brush, value); } - } - } -} +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,false) + { + Brush = _brushes[NextBrushIndex()]; + + } + + public Copter(IConnection Connection, SynchronizationContext uiThreadContext, double lat, double lng) : base(Connection, uiThreadContext, false) + { + Brush = _brushes[NextBrushIndex()]; + RecordLat = lat; + RecordLng = lng; + Latitude = lat; + Longitude = lng; + //RaiseLocationChangedIfNeeded(); + } + + + internal static SolidColorBrush BlueBrush { get; } = new SolidColorBrush(Color.FromRgb(28, 151, 234)); + internal static SolidColorBrush RedBrush { get; } = new SolidColorBrush(Color.FromRgb(255, 100, 100)); + internal static SolidColorBrush GreenBrush { get; } = new SolidColorBrush(Color.FromRgb(100, 255, 100)); + internal static SolidColorBrush YellowBrush { get; } = new SolidColorBrush(Color.FromRgb(255, 215, 0)); + + + + 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; } = GreenBrush;// new SolidColorBrush(Color.FromRgb(50, 205, 50)); + //虚拟飞机列表默认颜色 + internal static SolidColorBrush DefaultFakeBrush { get; } = BlueBrush; + + + + + private SolidColorBrush _Brush; + public SolidColorBrush Brush + { + get { + if (CopterPreCheckPass) + return _Brush ?? DefaultBrush; + else + return _Brush ?? YellowBrush; + + } + set { Set(nameof(Brush), ref _Brush, value); } + } + } +} diff --git a/Plane.FormationCreator/Formation/FlightTaskManager.cs b/Plane.FormationCreator/Formation/FlightTaskManager.cs index 820f907..37b857e 100644 --- a/Plane.FormationCreator/Formation/FlightTaskManager.cs +++ b/Plane.FormationCreator/Formation/FlightTaskManager.cs @@ -26,6 +26,7 @@ using Newtonsoft.Json.Linq; using GMap.NET.MapProviders; using GMap.NET; using static Plane.FormationCreator.CalculationLogLatDistance; +using Plane.CopterManagement; namespace Plane.FormationCreator.Formation { @@ -412,6 +413,13 @@ namespace Plane.FormationCreator.Formation get { return _TaskState; } private set { Set(nameof(TaskState), ref _TaskState, value); } } + //真实飞行任务状态 + private TasksStatus _TaskState_real = TasksStatus.Stop; + public TasksStatus TaskState_real + { + get { return _TaskState_real; } + private set { Set(nameof(TaskState_real), ref _TaskState_real, value);} + } public event EventHandler TaskAdded; public event EventHandler TaskDeled; @@ -601,6 +609,7 @@ namespace Plane.FormationCreator.Formation + /// /// 起始点作为参考，设置起始点后飞行写入的数据将为相对坐标 /// @@ -796,7 +805,7 @@ namespace Plane.FormationCreator.Formation Pause(); //等待暂停或2s超时(80*25ms) int k = 0; - while ((TaskState != TasksStatus.Paused) || (k > 80)) + while ((TaskState != TasksStatus.Paused) && (k < 80)) { await Task.Delay(25).ConfigureAwait(false); k++; @@ -2227,7 +2236,7 @@ namespace Plane.FormationCreator.Formation } //新开线程异步调用ABypassB并等待返回 - public async Task<(List>,bool)> ABypassBAsync(FlightRouteV2.Vector3[] aVecs, FlightRouteV2.Vector3[] bVecs) + public async Task<(List>,bool)> ABypassBAsync(FlightRouteV2.Vector3[] aVecs, FlightRouteV2.Vector3[] bVecs, List mappingId = null) { List> ret = null; bool isPasstmp = false; @@ -2235,7 +2244,7 @@ namespace Plane.FormationCreator.Formation var task = Task.Run(() => { cts = new CancellationTokenSource(); - ret = FlyVecFun.ABypassB(aVecs, bVecs, Routecallback, Cronograma, cts.Token, out isPasstmp); + ret = FlyVecFun.ABypassB(aVecs, bVecs, Routecallback, Cronograma, cts.Token, out isPasstmp, mappingId); }); try { @@ -2277,7 +2286,7 @@ namespace Plane.FormationCreator.Formation var task = Task.Run(() => { - ret = FlyVecFun.CollisionLayer(aVecs, bVecs, taskname_a, taskname_b, Routecallback); + ret = FlyVecFun.CollisionLayer(aVecs, bVecs, Routecallback); }); try { @@ -2408,7 +2417,7 @@ namespace Plane.FormationCreator.Formation /// /// 自动生成航线2D和3D都用这个-------V2版使用新的碰撞检测和绕行，用flybase.cs库，只用于新版固件-------------- /// - /// 3D计算 + /// 2D/3D计算 /// 是否改变线路的结束点顺序--返回起飞点航线不能交换 /// 3D是否自动插入拉开层，用于2D画面回起飞矩阵，拉开2D画面 public async Task OptimizeRouteMeterV2(bool Is3d = false, bool Ischange = true,bool Is3dstagger=false) @@ -2417,6 +2426,10 @@ namespace Plane.FormationCreator.Formation Dictionary prevTaskPoint = new Dictionary(); var stopWatch = new Stopwatch(); stopWatch.Start(); + //ID映射列表 + List mappingId = new List(); + + //设置随机数种子 FlyVecFun.RandomSeed = (int)DateTime.Now.Ticks; @@ -2435,6 +2448,8 @@ namespace Plane.FormationCreator.Formation var prevInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i]; Point3D prevLoc = new Point3D(prevInfo.X * 100, prevInfo.TargetAlt * 100, prevInfo.Y * 100); prevTaskPoint.Add(i, prevLoc); + //3D计算需要ID映射 + mappingId.Add(int.Parse(_copterManager.Copters[i].Id)); } string taskname_a = SelectedTaskIndex.ToString() + Tasks[SelectedTaskIndex - 1].TaskCnName; string taskname_b = (SelectedTaskIndex+1).ToString()+SelectedTask.TaskCnName; @@ -2473,7 +2488,9 @@ namespace Plane.FormationCreator.Formation if (Is3d) { bool isPass = false; - var result= await ABypassBAsync(aVecs, bVecs); + + + var result= await ABypassBAsync(aVecs, bVecs, mappingId); List> flyret = result.Item1; isPass= result.Item2; if ((flyret.Count == 0)&&(!isPass)) @@ -2483,6 +2500,12 @@ namespace Plane.FormationCreator.Formation { Message.Show($"任务{SelectedTaskIndex + 1}无法直接生成3D航线，尝试拉开画面"); cVecs = await NormalPullsync(aVecs, bVecs); + if (cVecs == null) + { + Message.Show($"任务不满足拉开条件，退出"); + return; + } + Message.Show($"任务{SelectedTaskIndex + 1}后插入拉开层..."); //插入错层航点 SelectTask(SelectedTaskIndex - 1); @@ -2498,7 +2521,7 @@ namespace Plane.FormationCreator.Formation SetTaskFlytime(SelectedTaskIndex); //选到最后一个航点 SelectTask(SelectedTaskIndex + 1); - var result1= await ABypassBAsync(cVecs, bVecs); + var result1= await ABypassBAsync(cVecs, bVecs, mappingId); flyret = result1.Item1; isPass = result1.Item2; } @@ -3260,9 +3283,64 @@ namespace Plane.FormationCreator.Formation } } + //任务飞行时长ms + private TimeSpan _taskflyTime ; + public TimeSpan taskflyTime + { + get { return _taskflyTime; } + set { Set(nameof(taskflyTime), ref _taskflyTime, value); } + } + + //真实任务飞行时长ms + private TimeSpan _taskflyTime_real; + public TimeSpan taskflyTime_real + { + get { return _taskflyTime_real; } + set { Set(nameof(taskflyTime_real), ref _taskflyTime_real, value); } + } + + private DateTime _taskStartTime; + private DateTime _taskStartTime_real; + + public DateTime TaskStartTime_real + { + get { return _taskStartTime_real; } + + } + public bool RealRunTask(DateTime taskStartTime) + { + if (TaskState_real == TasksStatus.Stop) + //根据传入时间设置真实起飞时间 + { + _taskStartTime_real = taskStartTime; + TaskState_real = TasksStatus.Running; + Message.Show($"{taskStartTime.ToString("HH:mm:ss")}：真实任务开始"); + return true; + }else + Message.Show($"任务已经开始"); + return false; + } + + public void RealResetTask() + { + TaskState_real = TasksStatus.Stop; + + } + + public bool SimRunTask(DateTime taskStartTime) + { + if (TaskState== TasksStatus.Stop) + //根据传入时间设置真实起飞时间 + { + _taskStartTime = taskStartTime; + Message.Show($"{taskStartTime.ToString("HH:mm:ss")}：默认任务开始"); + return true; + } else + Message.Show($"任务已经开始"); + return false; + } + - TimeSpan timeSpan; - public DateTime taskStartTime; public async Task RunTaskAsync() { if (Tasks.Count == 0) return; @@ -3290,7 +3368,12 @@ namespace Plane.FormationCreator.Formation if (CurrentRunningTaskIndex == 0) { - taskStartTime = DateTime.Now; + if (!SimRunTask(DateTime.Now)) + { + Alert.Show($"模拟任务已经开始，请先停止任务！"); + return; + } + Message.Show($"{DateTime.Now.ToString("HH:mm:ss")}：任务开始"); } @@ -3307,13 +3390,16 @@ namespace Plane.FormationCreator.Formation await RunAsync(); + taskflyTime = DateTime.Now - _taskStartTime; + if ((IsPaused ?? false) == false) { - timeSpan = DateTime.Now - taskStartTime; Message.Show($"{DateTime.Now.ToString("HH:mm:ss")}：任务结束"); - Message.Show($"总时长 = {timeSpan.Minutes}分{timeSpan.Seconds}秒"); - + Message.Show($"总时长 = {taskflyTime.Minutes}分{taskflyTime.Seconds}秒"); } + else + Message.Show($"任务暂停，总时长 = {taskflyTime.TotalSeconds}秒"); + } /// /// 开始运行模拟任务 @@ -3330,12 +3416,13 @@ namespace Plane.FormationCreator.Formation task.Status = FlightTaskStatus.Stop; } AppEx.Current.AppMode = AppMode.RunningTasks; - StartAvoidingCrash(); //开始碰撞检测 - + //告诉所有飞机开始任务--模拟飞机不计算起飞延迟，直接传0 foreach (var copter in _copterManager.Copters) await copter.MissionStartAsync(0, 0, 0, 0, 0); - TaskState = TasksStatus.Running; + TaskState = TasksStatus.Running; + StartAvoidingCrash(); //开始碰撞检测 + for (int i = CurrentRunningTaskIndex; i < Tasks.Count; i++) { var task = Tasks[i]; @@ -3474,6 +3561,7 @@ namespace Plane.FormationCreator.Formation { rettaskindex = i; taskflytime = rettime - starttime; + break; } starttime += flyedtasktime; } @@ -3495,7 +3583,7 @@ namespace Plane.FormationCreator.Formation curLoc = TaskFlyLoc(copterprvLoc, coptercurLoc, taskflytime, Tasks[rettaskindex].FlytoTime); } else //正在悬停 - curLoc = new PLLocation(curinfo.TargetLat, curinfo.TargetLng, 0); + curLoc = new PLLocation(curinfo.TargetLat, curinfo.TargetLng, curinfo.TargetAlt); curTaskPoint.Add(i, curLoc); if (curinfo.TargetLat > taskmaxlat) @@ -3557,15 +3645,22 @@ namespace Plane.FormationCreator.Formation - + /// + /// 开始碰撞检测 + /// private async void StartAvoidingCrash() { await Task.Factory.StartNew(AvoidCrashtoshow); await Task.Factory.StartNew(LoopToAvoidCrash); } + + /// + /// 将碰撞显示到UI + /// private async void AvoidCrashtoshow() { - while (IsPaused == false) + Message.Show("开始碰撞检测"); + while (TaskState != TasksStatus.Stop) //IsPaused == false { try { @@ -3585,10 +3680,14 @@ namespace Plane.FormationCreator.Formation await Task.Delay(100).ConfigureAwait(false); } } + Message.Show("开始碰撞检测停止"); } + /// + /// 循环检测碰撞 + /// private async void LoopToAvoidCrash() { - while (IsPaused == false) + while (TaskState != TasksStatus.Stop) { try { diff --git a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs index 057a0db..9429de0 100644 --- a/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs +++ b/Plane.FormationCreator/Formation/FlightTask_FlyTo.cs @@ -183,6 +183,10 @@ namespace Plane.FormationCreator.Formation await info.Copter.HoverAsync(); return; } + if (_flightTaskManager.IsEmergencyRet == true) + { + return; + } dtNow = DateTime.Now; ts = dtNow - dtLastTime; } diff --git a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs index 9648002..04e1a2e 100644 --- a/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs +++ b/Plane.FormationCreator/Formation/FlightTask_TakeOff.cs @@ -112,6 +112,8 @@ namespace Plane.FormationCreator.Formation info.TargetLng = info.Copter.Longitude; //info.Copter.takeOffTargetAltitude = takeOffAlt; FlightTask task = _flightTaskManager.CurrentRunningTask; + if (task==null) + return; float takeflytime = task.TakeOffTime - info.TakeOffWaitTime; //飞行目标，开始往上飞 await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt, takeflytime, 1); //秒 diff --git a/Plane.FormationCreator/ModifyParam.xaml.cs b/Plane.FormationCreator/ModifyParam.xaml.cs index 6146261..e48662d 100644 --- a/Plane.FormationCreator/ModifyParam.xaml.cs +++ b/Plane.FormationCreator/ModifyParam.xaml.cs @@ -1,56 +1,65 @@ -using Plane.FormationCreator.Util; -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; -using System.Windows; -using System.Windows.Controls; -using System.Windows.Data; -using System.Windows.Documents; -using System.Windows.Input; -using System.Windows.Media; -using System.Windows.Media.Imaging; -using System.Windows.Shapes; - -namespace Plane.FormationCreator -{ - /// - /// ModifyParam.xaml 的交互逻辑 - /// - public partial class ModifyParam : Window - { - public bool LoadParam = false; - public ModifyParam() - { - InitializeComponent(); -// if (!VersionControl.IsFullVersion) -// { -// hide_panel.Visibility = Visibility.Collapsed; -// textParamName.IsReadOnly = true; -// btnLoad.Visibility = Visibility.Collapsed; -// -// label.Visibility = Visibility.Collapsed; -// textParamName.Visibility = Visibility.Collapsed; -// } - } - - private void btnModify_Click(object sender, RoutedEventArgs e) - { - LoadParam = false; - this.DialogResult = true; - } - - private void Modify_Select(object sender, RoutedEventArgs e) - { - textParamName.Text = ((Button)sender).Tag.ToString(); - textParamName_cn.Text = ((Button)sender).Content.ToString(); - } - - private void btnLoad_Click(object sender, RoutedEventArgs e) - { - LoadParam = true; - this.DialogResult = true; - } - } -} +using Microsoft.Practices.ServiceLocation; +using Plane.FormationCreator.Util; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows; +using System.Windows.Controls; +using System.Windows.Data; +using System.Windows.Documents; +using System.Windows.Input; +using System.Windows.Media; +using System.Windows.Media.Imaging; +using System.Windows.Shapes; + +namespace Plane.FormationCreator +{ + /// + /// ModifyParam.xaml 的交互逻辑 + /// + public partial class ModifyParam : Window + { + public bool LoadParam = false; + public ModifyParam() + { + InitializeComponent(); +// if (!VersionControl.IsFullVersion) +// { +// hide_panel.Visibility = Visibility.Collapsed; +// textParamName.IsReadOnly = true; +// btnLoad.Visibility = Visibility.Collapsed; +// +// label.Visibility = Visibility.Collapsed; +// textParamName.Visibility = Visibility.Collapsed; +// } + } + + private void btnModify_Click(object sender, RoutedEventArgs e) + { + LoadParam = false; + this.DialogResult = true; + } + + private void Modify_Select(object sender, RoutedEventArgs e) + { + Formation.CopterManager _copterManager = ServiceLocator.Current.GetInstance(); + + String strParamName = ((Button)sender).Tag.ToString(); + if (strParamName == "FS_BATT_VOLTAGE") + { + if (_copterManager.FC_VER_NO >= 3) + strParamName = "BATT_LOW_VOLT"; + } + textParamName.Text = strParamName; + textParamName_cn.Text = ((Button)sender).Content.ToString(); + } + + private void btnLoad_Click(object sender, RoutedEventArgs e) + { + LoadParam = true; + this.DialogResult = true; + } + } +} diff --git a/Plane.FormationCreator/Plane.FormationCreator.csproj b/Plane.FormationCreator/Plane.FormationCreator.csproj index e1de434..8f5af26 100644 --- a/Plane.FormationCreator/Plane.FormationCreator.csproj +++ b/Plane.FormationCreator/Plane.FormationCreator.csproj @@ -166,6 +166,8 @@ + + diff --git a/Plane.FormationCreator/ViewModels/CalibrationViewModel.cs b/Plane.FormationCreator/ViewModels/CalibrationViewModel.cs index 36f38e9..926b281 100644 --- a/Plane.FormationCreator/ViewModels/CalibrationViewModel.cs +++ b/Plane.FormationCreator/ViewModels/CalibrationViewModel.cs @@ -1,222 +1,263 @@ -using GalaSoft.MvvmLight; -using GalaSoft.MvvmLight.Command; -using Microsoft.Practices.ServiceLocation; -using Plane.CommunicationManagement; -using Plane.CopterManagement; -using Plane.Copters; -using Plane.FormationCreator.Formation; -using Plane.Windows.Messages; -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; -using System.Windows.Input; - -namespace Plane.FormationCreator.ViewModels -{ - public class CalibrationViewModel : ViewModelBase - { - CommModuleManager commModule = CommModuleManager.Instance; - - private CopterManager _copterManager = ServiceLocator.Current.GetInstance(); - - private string _AccelerometerTips; - public string AccelerometerTips - { - get - { - switch (AccelerometerState) - { - case AccelerometerStates.Idle: - _AccelerometerTips = "点击“开始校准”后，开始校准加速计"; - break; - case AccelerometerStates.Front: - _AccelerometerTips = "亮紫色: 飞机水平放置(Front)"; - break; - case AccelerometerStates.Left: - _AccelerometerTips = "亮黄色：飞机左侧接触地面竖立放置(Left)"; - break; - case AccelerometerStates.Right: - _AccelerometerTips = "亮青色：飞机右侧接触地面竖立放置(Right)"; - break; - case AccelerometerStates.Down: - _AccelerometerTips = "紫色：飞机机头向下接触地面竖立放置(Down)"; - break; - case AccelerometerStates.Up: - _AccelerometerTips = "黄色：飞机机尾向下接触地面竖立放置(Up)"; - break; - case AccelerometerStates.Back: - _AccelerometerTips = "青色：飞机翻过来水平放置(Back)"; - break; - } - return _AccelerometerTips; - } - } - - private string _AccelerometerBtnText = "开始校准"; - public string AccelerometerBtnText - { - get - { - switch (AccelerometerState) - { - case AccelerometerStates.Idle: - _AccelerometerBtnText = "开始校准"; - break; - default: - _AccelerometerBtnText = "完成"; - break; - } - return _AccelerometerBtnText; - } - } - - //校准加速计的状态 - public enum AccelerometerStates - { - Idle = 0, - Front = 1, - Left = 2, - Right = 3, - Down = 4, - Up = 5, - Back = 6 - } - - private AccelerometerStates _AccelerometerState = AccelerometerStates.Idle; - public AccelerometerStates AccelerometerState - { - get { return _AccelerometerState; } - set - { - Set(nameof(AccelerometerState), ref _AccelerometerState, value); - RaisePropertyChanged(nameof(AccelerometerBtnText)); - RaisePropertyChanged(nameof(AccelerometerTips)); - } - } - - /// - /// 开始校准加速计 - /// - private ICommand _CalibrationAccelerometerStartCommand; - public ICommand CalibrationAccelerometerStartCommand - { - get - { - return _CalibrationAccelerometerStartCommand ?? (_CalibrationAccelerometerStartCommand = new RelayCommand(async () => - { - if (_copterManager.SelectedCopters.Count() == 0) return; - await commModule.DoStartPreflightCompassAsync(_copterManager.SelectedCopters); - })); - } - } - - /// - /// 校准加速计下一步 - /// - private ICommand _CalibrationAccelerometerNextCommand; - public ICommand CalibrationAccelerometerNextCommand - { - get - { - return _CalibrationAccelerometerNextCommand ?? (_CalibrationAccelerometerNextCommand = new RelayCommand(async () => - { - if (_copterManager.SelectedCopters.Count() == 0) return; - await commModule.DoNextPreflightCompassAsync(_copterManager.SelectedCopters); - })); - } - } - - private int _CompassPercent; - public int CompassPercent - { - get { return _CompassPercent; } - set - { - Set(nameof(CompassPercent), ref _CompassPercent, value); - } - } - - public bool IsCalibration { get; set; } - /// - /// 校准指南针 - /// - private ICommand _CalibrationCompassCommand; - public ICommand CalibrationCompassCommand - { - get - { - return _CalibrationCompassCommand ?? (_CalibrationCompassCommand = new RelayCommand(async () => - { - if (_copterManager.SelectedCopters.Count() == 0) return; - -// ICopter copter = _copterManager.SelectedCopters.FirstOrDefault(); -// -// short copterId = short.Parse(copter.Name); - Message.Show("开始校准指南针"); - - await commModule.DoCalibrationCompassAsync(_copterManager.SelectedCopters); - await Task.Delay(50).ConfigureAwait(false); - - /*支持多飞行机同时校准 不再判断单架的返回值 - - IsCalibration = true; - CompassPercent = 0; - int State = 0; //4：成功 5失败 todo 改为枚举 - - - while (IsCalibration && CompassPercent <= 100) - { - //两个255的时候表示 当前预留字节代表的意思是校准 - if (copter.Retain[2] == 255 && copter.Retain[3] == 255) - { - CompassPercent = copter.Retain[0]; - State = copter.Retain[1]; - if (State == 4 || State == 5) - break; - } - - await Task.Delay(100); - } - - switch (State) - { - case 4: - Alert.Show($"校准成功，请重新上电{copterId}号", "指南针"); - break; - case 5: - Alert.Show("校准失败", "指南针"); - break; - default: - break; - } - - IsCalibration = false; - */ - })); - } - } - - private ICommand _CancelCalibrationCompassCommand; - public ICommand CancelCalibrationCompassCommand - { - get - { - return _CancelCalibrationCompassCommand ?? (_CancelCalibrationCompassCommand = new RelayCommand(async () => - { - if (_copterManager.SelectedCopters.Count() == 1) return; - - ICopter copter = _copterManager.SelectedCopters.FirstOrDefault(); - - short copterId = short.Parse(copter.Name); - await commModule.DoCancelCalibrationCompassAsync(copterId); - IsCalibration = false; - Alert.Show("放弃校准", "指南针"); - })); - } - } - } - - -} +using GalaSoft.MvvmLight; +using GalaSoft.MvvmLight.Command; +using Microsoft.Practices.ServiceLocation; +using Plane.CommunicationManagement; +using Plane.CopterManagement; +using Plane.Copters; +using Plane.FormationCreator.Formation; +using Plane.Windows.Messages; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows.Input; + +namespace Plane.FormationCreator.ViewModels +{ + public class CalibrationViewModel : ViewModelBase + { + CommModuleManager commModule = CommModuleManager.Instance; + + private CopterManager _copterManager = ServiceLocator.Current.GetInstance(); + + private string _AccelerometerTips; + public string AccelerometerTips + { + get + { + switch (AccelerometerState) + { + case AccelerometerStates.Idle: + _AccelerometerTips = "点击“开始校准”后，开始校准加速计"; + break; + case AccelerometerStates.Front: + _AccelerometerTips = "亮紫色: 飞机水平放置(Front)"; + break; + case AccelerometerStates.Left: + _AccelerometerTips = "亮黄色：飞机左侧接触地面竖立放置(Left)"; + break; + case AccelerometerStates.Right: + _AccelerometerTips = "亮青色：飞机右侧接触地面竖立放置(Right)"; + break; + case AccelerometerStates.Down: + _AccelerometerTips = "紫色：飞机机头向下接触地面竖立放置(Down)"; + break; + case AccelerometerStates.Up: + _AccelerometerTips = "黄色：飞机机尾向下接触地面竖立放置(Up)"; + break; + case AccelerometerStates.Back: + _AccelerometerTips = "青色：飞机翻过来水平放置(Back)"; + break; + } + return _AccelerometerTips; + } + } + + private string _AccelerometerBtnText = "开始校准"; + public string AccelerometerBtnText + { + get + { + switch (AccelerometerState) + { + case AccelerometerStates.Idle: + _AccelerometerBtnText = "开始校准"; + break; + default: + _AccelerometerBtnText = "完成"; + break; + } + return _AccelerometerBtnText; + } + } + + //校准加速计的状态 + public enum AccelerometerStates + { + Idle = 0, + Front = 1, + Left = 2, + Right = 3, + Down = 4, + Up = 5, + Back = 6 + } + + private AccelerometerStates _AccelerometerState = AccelerometerStates.Idle; + public AccelerometerStates AccelerometerState + { + get { return _AccelerometerState; } + set + { + Set(nameof(AccelerometerState), ref _AccelerometerState, value); + RaisePropertyChanged(nameof(AccelerometerBtnText)); + RaisePropertyChanged(nameof(AccelerometerTips)); + } + } + + /// + /// 开始校准加速计 + /// + private ICommand _CalibrationAccelerometerStartCommand; + public ICommand CalibrationAccelerometerStartCommand + { + get + { + return _CalibrationAccelerometerStartCommand ?? (_CalibrationAccelerometerStartCommand = new RelayCommand(async () => + { + if (_copterManager.SelectedCopters.Count() == 0) return; + await commModule.DoStartPreflightCompassAsync(_copterManager.SelectedCopters); + })); + } + } + + /// + /// 校准加速计下一步 + /// + private ICommand _CalibrationAccelerometerNextCommand; + public ICommand CalibrationAccelerometerNextCommand + { + get + { + return _CalibrationAccelerometerNextCommand ?? (_CalibrationAccelerometerNextCommand = new RelayCommand(async () => + { + if (_copterManager.SelectedCopters.Count() == 0) return; + await commModule.DoNextPreflightCompassAsync(_copterManager.SelectedCopters); + })); + } + } + + private int _CompassPercent; + public int CompassPercent + { + get { return _CompassPercent; } + set + { + Set(nameof(CompassPercent), ref _CompassPercent, value); + } + } + + public bool IsCalibration { get; set; } + + + /// + /// 重启飞控 + /// + private ICommand _RestartFCCommand; + public ICommand RestartFCCommand + { + get + { + return _RestartFCCommand ?? (_RestartFCCommand = new RelayCommand(async () => + { + if (_copterManager.SelectedCopters.Count() == 0) return; + Message.Show("重启飞控"); + await commModule.DoRestartFCAsync(_copterManager.SelectedCopters); + await Task.Delay(50).ConfigureAwait(false); + })); + } + } + + + /// + /// 陀螺仪校准 + /// + private ICommand _CalibrationPreflightCommand; + public ICommand CalibrationPreflightCommand + { + get + { + return _CalibrationPreflightCommand ?? (_CalibrationPreflightCommand = new RelayCommand(async () => + { + if (_copterManager.SelectedCopters.Count() == 0) return; + Message.Show("开始校准陀螺仪"); + await commModule.DoCalibrationPreflightAsync(_copterManager.SelectedCopters); + await Task.Delay(50).ConfigureAwait(false); + })); + } + } + + + + /// + /// 校准指南针 + /// + private ICommand _CalibrationCompassCommand; + public ICommand CalibrationCompassCommand + { + get + { + return _CalibrationCompassCommand ?? (_CalibrationCompassCommand = new RelayCommand(async () => + { + if (_copterManager.SelectedCopters.Count() == 0) return; + +// ICopter copter = _copterManager.SelectedCopters.FirstOrDefault(); +// +// short copterId = short.Parse(copter.Name); + Message.Show("开始校准指南针"); + + await commModule.DoCalibrationCompassAsync(_copterManager.SelectedCopters); + await Task.Delay(50).ConfigureAwait(false); + + /*支持多飞行机同时校准 不再判断单架的返回值 + + IsCalibration = true; + CompassPercent = 0; + int State = 0; //4：成功 5失败 todo 改为枚举 + + + while (IsCalibration && CompassPercent <= 100) + { + //两个255的时候表示 当前预留字节代表的意思是校准 + if (copter.Retain[2] == 255 && copter.Retain[3] == 255) + { + CompassPercent = copter.Retain[0]; + State = copter.Retain[1]; + if (State == 4 || State == 5) + break; + } + + await Task.Delay(100); + } + + switch (State) + { + case 4: + Alert.Show($"校准成功，请重新上电{copterId}号", "指南针"); + break; + case 5: + Alert.Show("校准失败", "指南针"); + break; + default: + break; + } + + IsCalibration = false; + */ + })); + } + } + + private ICommand _CancelCalibrationCompassCommand; + public ICommand CancelCalibrationCompassCommand + { + get + { + return _CancelCalibrationCompassCommand ?? (_CancelCalibrationCompassCommand = new RelayCommand(async () => + { + if (_copterManager.SelectedCopters.Count() == 1) return; + + ICopter copter = _copterManager.SelectedCopters.FirstOrDefault(); + + short copterId = short.Parse(copter.Name); + await commModule.DoCancelCalibrationCompassAsync(copterId); + IsCalibration = false; + Alert.Show("放弃校准", "指南针"); + })); + } + } + } + + +} diff --git a/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs b/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs index 3dd44d9..fc78dbd 100644 --- a/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs @@ -394,7 +394,21 @@ namespace Plane.FormationCreator.ViewModels })); } } + + private ICommand _ResetRealMissionCommand; + public ICommand ResetRealMissionCommand + { + get + { + return _ResetRealMissionCommand ?? (_ResetRealMissionCommand = new RelayCommand(async () => + { + FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + _flightTaskManager.RealResetTask(); + AllowMissionStart = true; + })); + } + } private ICommand _EmergencyRetCommand; public ICommand EmergencyRetCommand { @@ -402,23 +416,45 @@ namespace Plane.FormationCreator.ViewModels { return _EmergencyRetCommand ?? (_EmergencyRetCommand = new RelayCommand(async () => { - if (Alert.Show("您确定要紧急返航吗？紧急返航可能导致飞行器碰撞！！！！", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning) - != MessageBoxResult.OK) - return; //计算当前图案中心点，起飞图案中心点，图案距离起飞图案中心点最近距离 FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + + if (_flightTaskManager.Tasks.Count==0) + { + Alert.Show("没有航点无法紧急返航！", "提示", MessageBoxButton.OK, MessageBoxImage.Warning); + return; + } + + if ((_flightTaskManager.TaskState_real == TasksStatus.Stop) + &&((_flightTaskManager.IsPaused ?? false) == false)) + { + Alert.Show("先暂停任务！", "提示", MessageBoxButton.OK, MessageBoxImage.Warning); + return; + } + //if (Alert.Show("您确定要紧急返航吗？紧急返航可能导致飞行器碰撞！！！！", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning) + // != MessageBoxResult.OK) + // return; + + //计算当前图案中心点，起飞图案中心点，图案距离起飞图案中心点最近距离 //计算无人机返航方向和最近的飞机距离 PLLocation takeoffcentloc; PLLocation taskcentloc; double mindistance; //单位米 bool overlapping; //图案否重叠 - int rettime= (int)(DateTime.Now - _flightTaskManager.taskStartTime).TotalSeconds; + int rettime=0; + if (_flightTaskManager.TaskState_real!= TasksStatus.Stop) + //计算实际飞行时间 + rettime = (int)(DateTime.UtcNow - _flightTaskManager.TaskStartTime_real).TotalSeconds; + else + if (_flightTaskManager.TaskState!= TasksStatus.Stop) + //计算模拟飞行时间 + rettime = (int)_flightTaskManager.taskflyTime.TotalSeconds; + + if (!_flightTaskManager.EmergencyRet(rettime,out takeoffcentloc,out taskcentloc, out mindistance,out overlapping)) { Alert.Show("紧急返航数据计算失败！", "提示"); return; } - rettime += 5; //加5秒延迟用于通讯重复，有些飞机通讯不成功多次发送才收到，比别的飞机慢 - //模拟测试 if (_flightTaskManager.TaskState != TasksStatus.Stop) await _flightTaskManager.sim_DoMissionEmergencyRetAsync(takeoffcentloc, taskcentloc, (float)mindistance, rettime, overlapping); @@ -544,7 +580,8 @@ namespace Plane.FormationCreator.ViewModels await Task.Delay(1000).ConfigureAwait(false); } await Task.Delay(100); - AllowMissionStart = true; + FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + AllowMissionStart = (_flightTaskManager.TaskState_real == TasksStatus.Stop); /* await Task.WhenAll(_copterManager.Copters.Select(async c => { @@ -1440,9 +1477,22 @@ namespace Plane.FormationCreator.ViewModels Alert.Show("作为参照的原点未设置，无法开始任务！", "提示"); return; } + + if (_flightTaskManager.Tasks.Count == 0 ) + { + if (Alert.Show("没有导入任务，紧急返航功能不能使用，继续吗？", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning) + != MessageBoxResult.OK) + return; + } //飞机用UTC时间，日志用本地时间记录 DateTime MissionTime = DateTime.UtcNow.AddSeconds(5); DateTime MissionTime_log = DateTime.Now.AddSeconds(5); + if (!_flightTaskManager.RealRunTask(MissionTime)) + { + Alert.Show("任务已经开始，请先重设任务！", "提示"); + return; + } + AllowMissionStart = false; Message.Show("任务开始:" + MissionTime_log.ToString()); //循环3次 发送起飞命令 避免通信问题 for (int i = 0; i < 20; i++) //20 diff --git a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs index 2d3d103..7c49e33 100644 --- a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs @@ -959,8 +959,8 @@ namespace Plane.FormationCreator.ViewModels return _OptimizeRouteCommandRet ?? (_OptimizeRouteCommandRet = new RelayCommand(async => { if (_copterManager.FC_VER_NO >= 3) - //3D计算，不改变ID，可以错层 - _flightTaskManager.OptimizeRouteMeter(false, false, true); //采用米计算逻辑，用3D生成，不改变ID，可拉开层(2D回起飞矩阵专用) + //2D计算，不改变ID，不用错层 + _flightTaskManager.OptimizeRouteMeter(false, false, false); //采用米计算逻辑，用3D生成，不改变ID，可拉开层(2D回起飞矩阵专用) else _flightTaskManager.OptimizeRouteMeter(false, false); //采用米计算逻辑，用2D生成，不改变ID，不拉开层 @@ -978,7 +978,7 @@ namespace Plane.FormationCreator.ViewModels { if (_copterManager.FC_VER_NO >= 3) //3D计算，不改变ID，可以错层 - _flightTaskManager.OptimizeRouteMeter(true, false, false); //采用米计算逻辑，用3D生成，不改变ID，不拉开层(3D回起飞矩阵专用) + _flightTaskManager.OptimizeRouteMeter(true, false, true); //采用米计算逻辑，用3D生成，不改变ID，拉开层(3D回起飞矩阵专用,结束必须是矩阵) else _flightTaskManager.OptimizeRouteMeter(false, false); //采用米计算逻辑，用2D生成，不改变ID，不拉开层 diff --git a/Plane.FormationCreator/Views/CalibrationWindow.xaml b/Plane.FormationCreator/Views/CalibrationWindow.xaml index c6d3afc..26c958e 100644 --- a/Plane.FormationCreator/Views/CalibrationWindow.xaml +++ b/Plane.FormationCreator/Views/CalibrationWindow.xaml @@ -1,40 +1,44 @@ - - - - - - - - -