1741 lines
71 KiB
C#
1741 lines
71 KiB
C#
using Plane.Copters;
|
||
using Plane.FormationCreator.Formation;
|
||
using Plane.Geography;
|
||
using Plane.Logging;
|
||
using Plane.Windows.Messages;
|
||
using GalaSoft.MvvmLight;
|
||
using GalaSoft.MvvmLight.CommandWpf;
|
||
using Microsoft.Practices.ServiceLocation;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
using System.Windows;
|
||
using System.Windows.Input;
|
||
using Plane.Communication;
|
||
using Microsoft.Win32;
|
||
using Plane.Util;
|
||
using System.Windows.Media;
|
||
using Plane.CommunicationManagement;
|
||
using Plane.FormationCreator.Views;
|
||
|
||
namespace Plane.FormationCreator.ViewModels
|
||
{
|
||
public class ControlPanelViewModel : ViewModelBase
|
||
{
|
||
// serialport
|
||
internal static IConnection Rtkport ;
|
||
private static System.Threading.Thread thrtk;
|
||
private static bool trkthreadrun = false;
|
||
|
||
private static bool rtcmthreadrun = false;
|
||
|
||
|
||
|
||
public ControlPanelViewModel(FormationController formationController, CopterManager copterManager)
|
||
{
|
||
_formationController = formationController;
|
||
_copterManager = copterManager;
|
||
}
|
||
|
||
private FormationController _formationController;
|
||
private CopterManager _copterManager;
|
||
private CommModuleManager _commModuleManager = CommModuleManager.Instance;
|
||
|
||
private ILogger _logger = ServiceLocator.Current.GetInstance<ILogger>();
|
||
|
||
private float _AltP = 0.5f;
|
||
public float AltP
|
||
{
|
||
get { return _AltP; }
|
||
set { Set(nameof(AltP), ref _AltP, value); }
|
||
}
|
||
|
||
|
||
private string _RTKState = "RTK未发送";
|
||
public string RTKState
|
||
{
|
||
get { return _RTKState; }
|
||
set { Set(nameof(RTKState), ref _RTKState, value); }
|
||
}
|
||
|
||
private string _RTKbtntxt = "发送RTK";
|
||
public string RTKbtntxt
|
||
{
|
||
get { return _RTKbtntxt; }
|
||
set { Set(nameof(RTKbtntxt), ref _RTKbtntxt, value); }
|
||
}
|
||
|
||
private string _NTRIPbtntxt = "发送RTCM";
|
||
public string NTRIPbtntxt
|
||
{
|
||
get { return _NTRIPbtntxt; }
|
||
set { Set(nameof(NTRIPbtntxt), ref _NTRIPbtntxt, value); }
|
||
}
|
||
|
||
private ICommand _LockAllCommand;
|
||
public ICommand LockAllCommand
|
||
{
|
||
get
|
||
{
|
||
return _LockAllCommand ?? (_LockAllCommand = new RelayCommand(async () =>
|
||
{
|
||
// await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync()));
|
||
if (Alert.Show("您确定要上锁吗？所有飞行器将无视转速，立即强制停止运转！！！！", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning)
|
||
== MessageBoxResult.OK)
|
||
{
|
||
if (Alert.Show("再次确认上锁？请确认所有飞行器都在地面，否则将自由落体", "再次警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning)
|
||
== MessageBoxResult.OK)
|
||
{
|
||
await _commModuleManager.LockAsync();
|
||
/*
|
||
await Task.WhenAll(_copterManager.Copters.Select(async c =>
|
||
{
|
||
await c.LockAsync();
|
||
}));
|
||
*/
|
||
}
|
||
}
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
private static readonly object locker = new object();
|
||
private ICommand _DetectionVoltage;
|
||
public ICommand DetectionVoltage
|
||
{
|
||
get
|
||
{
|
||
return _DetectionVoltage ?? (_DetectionVoltage = new RelayCommand(async () =>
|
||
{
|
||
Message.Show("--------------开始检测电压--------------");
|
||
Dictionary<string, float> dic_voltage = new Dictionary<string, float>();
|
||
await Task.WhenAll(_copterManager.Copters.Select(async c =>
|
||
{
|
||
float voltageSum = 0.0f;
|
||
string name = c.Name;
|
||
|
||
for (int i = 0; i < 5; i++)
|
||
{
|
||
|
||
voltageSum += c.Voltage;
|
||
await Task.Delay(1000).ConfigureAwait(false);
|
||
}
|
||
float voltageAverage = voltageSum / 5;
|
||
if (name != null && name != "")
|
||
{
|
||
lock(locker)
|
||
{
|
||
dic_voltage.Add(name, voltageAverage);
|
||
}
|
||
}
|
||
})).ConfigureAwait(false);
|
||
await Task.Run(async () => {
|
||
Dictionary<string, float> dic_voltage_Order = dic_voltage.OrderByDescending(o => o.Value).ToDictionary(o => o.Key, o => o.Value);
|
||
|
||
foreach (KeyValuePair<string, float> kv in dic_voltage_Order)
|
||
{
|
||
Message.Show(string.Format("{0} --> 5秒平均电压：{1}", kv.Key, kv.Value));
|
||
await Task.Delay(5).ConfigureAwait(false);
|
||
}
|
||
Message.Show(string.Format("----检测电压完成,检测总数：{0}----", dic_voltage.Count));
|
||
}).ConfigureAwait(false);
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _DetectionMissionData;
|
||
public ICommand DetectionMissionData
|
||
{
|
||
get
|
||
{
|
||
return _DetectionMissionData ?? (_DetectionMissionData = new RelayCommand(async () =>
|
||
{
|
||
var missionState = _commModuleManager.MissionWriteState.Where(i => i.Value.IsFailed);
|
||
if (missionState.Count() == 0)
|
||
Message.Show("航点全部写入成功");
|
||
else
|
||
{
|
||
Message.Show("----统计失败航点写入失败----");
|
||
foreach (KeyValuePair<int, CommWriteMissinState> item in missionState)
|
||
{
|
||
CommWriteMissinState state = item.Value;
|
||
string msg = $"{item.Key}:飞机状态:{state.StateReturn}, 错误码:{state.ErrorCode}, 传输:{state.SendAchieved}, 写入:{state.WriteSucceed} ";
|
||
Message.Show(msg);
|
||
await Task.Delay(10);
|
||
}
|
||
}
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _DetectionReturnData;
|
||
public ICommand DetectionReturnData
|
||
{
|
||
get
|
||
{
|
||
return _DetectionReturnData ?? (_DetectionReturnData = new RelayCommand(async () =>
|
||
{
|
||
Dictionary<int, List<string>> dic_returnData = new Dictionary<int, List<string>>();
|
||
await Task.WhenAll(_copterManager.Copters.Select(async c =>
|
||
{
|
||
|
||
string name = c.Name;
|
||
int return_data = BitConverter.ToInt32(c.Retain, 0);
|
||
|
||
lock (locker)
|
||
{
|
||
List<string> copterList;
|
||
if (!dic_returnData.ContainsKey(return_data))
|
||
{
|
||
copterList = new List<string>();
|
||
dic_returnData.Add(return_data, copterList);
|
||
}
|
||
else
|
||
copterList = dic_returnData[return_data];
|
||
|
||
copterList.Add(name);
|
||
}
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
})).ConfigureAwait(false);
|
||
|
||
await Task.Run(async () =>
|
||
{
|
||
foreach (KeyValuePair<int, List<string>> kv in dic_returnData)
|
||
{
|
||
byte[] test = BitConverter.GetBytes(kv.Key);
|
||
Array.Reverse(test);
|
||
string data = string.Join(".", test);
|
||
string copters = string.Join(",", kv.Value.ToArray());
|
||
Message.Show(string.Format("返回数据{0}：{1}", data, copters));
|
||
await Task.Delay(5).ConfigureAwait(false);
|
||
}
|
||
Message.Show("----统计完成----");
|
||
}).ConfigureAwait(false);
|
||
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _ReportGPSTypeCommand;
|
||
public ICommand ReportGPSTypeCommand
|
||
{
|
||
get
|
||
{
|
||
return _ReportGPSTypeCommand ?? (_ReportGPSTypeCommand = new RelayCommand(async()=>
|
||
{
|
||
if (_copterManager.Copters.Count > 0)
|
||
{
|
||
Dictionary<string, List<string>> GPSTypes = new Dictionary<string, List<string>>();
|
||
foreach (var copter in _copterManager.Copters)
|
||
{
|
||
string name = copter.Name;
|
||
if (!GPSTypes.Keys.Contains(copter.GpsFixType.ToString()))
|
||
{
|
||
List<string> copterNames = new List<string>();
|
||
copterNames.Add(name);
|
||
GPSTypes.Add(copter.GpsFixType.ToString(), copterNames);
|
||
}
|
||
else
|
||
GPSTypes[copter.GpsFixType.ToString()].Add(name);
|
||
}
|
||
|
||
foreach (var item in GPSTypes)
|
||
{
|
||
Message.Show($"{item.Key}:{string.Join(",", item.Value)}");
|
||
}
|
||
}
|
||
await Task.Delay(10);
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
|
||
private ICommand _DetectionCommModuleVersion;
|
||
public ICommand DetectionCommModuleVersion
|
||
{
|
||
get
|
||
{
|
||
return _DetectionCommModuleVersion ?? (_DetectionCommModuleVersion = new RelayCommand(async () =>
|
||
{
|
||
Dictionary<int, List<string>> dic_version = new Dictionary<int, List<string>>();
|
||
await Task.WhenAll(_copterManager.Copters.Select(async c =>
|
||
{
|
||
string name = c.Name;
|
||
int version = c.CommModuleVersion;
|
||
|
||
lock (locker)
|
||
{
|
||
List<string> copterList;
|
||
if (!dic_version.ContainsKey(version))
|
||
{
|
||
copterList = new List<string>();
|
||
dic_version.Add(version, copterList);
|
||
}
|
||
else
|
||
copterList = dic_version[version];
|
||
|
||
copterList.Add(name);
|
||
}
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
})).ConfigureAwait(false);
|
||
|
||
await Task.Run(async () =>
|
||
{
|
||
foreach (KeyValuePair<int, List<string>> kv in dic_version)
|
||
{
|
||
string copters = string.Join(",", kv.Value.ToArray());
|
||
Message.Show(string.Format("通信模块版本{0}：{1}", kv.Key, copters));
|
||
await Task.Delay(5).ConfigureAwait(false);
|
||
}
|
||
Message.Show("----统计完成----");
|
||
}).ConfigureAwait(false);
|
||
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _AllLandCommand;
|
||
public ICommand AllLandCommand
|
||
{
|
||
get
|
||
{
|
||
return _AllLandCommand ?? (_AllLandCommand = new RelayCommand(async () =>
|
||
{
|
||
await _commModuleManager.LandAsync();
|
||
/*
|
||
await Task.WhenAll(_copterManager.Copters.Select(async c =>
|
||
{
|
||
await c.LandAsync();
|
||
}));
|
||
*/
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _LEDFlickerCommand;
|
||
public ICommand LEDFlickerCommand
|
||
{
|
||
get
|
||
{
|
||
return _LEDFlickerCommand ?? (_LEDFlickerCommand = new RelayCommand(() =>
|
||
{
|
||
_commModuleManager.LED_FlickerAsync(_copterManager.AcceptingControlCopters);
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _MotorTestCommand;
|
||
public ICommand MotorTestCommand
|
||
{
|
||
get
|
||
{
|
||
return _MotorTestCommand ?? (_MotorTestCommand = new RelayCommand(async () =>
|
||
{
|
||
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
{
|
||
await _commModuleManager.MotorTestAsync(_copterManager.AcceptingControlCopters);
|
||
}
|
||
|
||
/*
|
||
|
||
await Task.WhenAll(_copterManager.Copters.Select(async c =>
|
||
{
|
||
for (int i = 1; i <= 4; i++)
|
||
{
|
||
await c.MotorTestAsync(i).ConfigureAwait(false);
|
||
await Task.Delay(10);
|
||
}
|
||
|
||
})).ConfigureAwait(false);
|
||
*/
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _GuidAsyncCommand;
|
||
public ICommand GuidAsyncCommand
|
||
{
|
||
get
|
||
{
|
||
return _GuidAsyncCommand ?? (_GuidAsyncCommand = new RelayCommand(async () =>
|
||
{
|
||
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
await _commModuleManager.TakeOffAsync(1, _copterManager.AcceptingControlCopters);
|
||
/*
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async c =>
|
||
{
|
||
await c.GuidAsync();
|
||
await Task.Delay(100).ConfigureAwait(false);
|
||
await c.TakeOffAsync(1);
|
||
}));
|
||
*/
|
||
}));
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 是否允许开始任务，全部解锁会在3秒内循环发送3次，该过程中不允许开始任务
|
||
/// </summary>
|
||
private bool _AllowMissionStart = true;
|
||
public bool AllowMissionStart
|
||
{
|
||
get { return _AllowMissionStart; }
|
||
set { Set(nameof(AllowMissionStart), ref _AllowMissionStart, value); }
|
||
}
|
||
|
||
|
||
private ICommand _UnlockAllCommand;
|
||
public ICommand UnlockAllCommand
|
||
{
|
||
get
|
||
{
|
||
return _UnlockAllCommand ?? (_UnlockAllCommand = new RelayCommand(async () =>
|
||
{
|
||
AllowMissionStart = false;
|
||
// await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync()));
|
||
for (int i = 0; i < 3; i++)
|
||
{
|
||
await _commModuleManager.UnlockAsync();
|
||
await Task.Delay(1000).ConfigureAwait(false);
|
||
}
|
||
await Task.Delay(100);
|
||
AllowMissionStart = true;
|
||
|
||
/*
|
||
await Task.WhenAll(_copterManager.Copters.Select(async c => {
|
||
await c.UnlockAsync();
|
||
//解锁间隔一定要超过1s否则导致飞控以后无法解锁
|
||
for (int i = 0; !c.IsUnlocked; i++)
|
||
{
|
||
if (i > 200)
|
||
break;
|
||
if (i % (1000 / 25) == 1000 / 25 - 1)
|
||
{
|
||
await c.UnlockAsync(); // 每 1000 毫秒重试一次。解锁间隔一定要超过1s否则导致飞控以后无法解锁
|
||
}
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
}
|
||
}));
|
||
*/
|
||
}));
|
||
}
|
||
}
|
||
|
||
private const string NTF_GROUPLED_OFF = "NTF_G_OFF";
|
||
private ICommand _LEDOnOffCommand;
|
||
public ICommand LEDOnOffCommand
|
||
{
|
||
get
|
||
{
|
||
return _LEDOnOffCommand ?? (_LEDOnOffCommand = new RelayCommand<int>(async onOff=>
|
||
{
|
||
string paramstr = NTF_GROUPLED_OFF;
|
||
float paramvalue = onOff;
|
||
await _commModuleManager.SetParamAsync(paramstr, paramvalue, _copterManager.AcceptingControlCopters);
|
||
/*
|
||
await Task.WhenAll(
|
||
_copterManager.AcceptingControlCopters.Select(async copter =>
|
||
{
|
||
paramvalue = await copter.GetParamAsync(paramstr);
|
||
}));
|
||
|
||
float newParamvalue = paramvalue == 0 ? 1 : 0;
|
||
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select
|
||
(copter => copter.SetParamAsync(paramstr, newParamvalue)));
|
||
*/
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _UnlockCommand;
|
||
public ICommand UnlockCommand
|
||
{
|
||
get
|
||
{
|
||
return _UnlockCommand ?? (_UnlockCommand = new RelayCommand(async () =>
|
||
{
|
||
// await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync()));
|
||
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
await _commModuleManager.UnlockAsync(_copterManager.AcceptingControlCopters);
|
||
|
||
/*
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select( async c => {
|
||
await c.UnlockAsync();
|
||
//解锁间隔一定要超过1s否则导致飞控以后无法解锁
|
||
for (int i = 0; !c.IsUnlocked; i++)
|
||
{
|
||
if (i > 200)
|
||
break;
|
||
if (i % (1000 / 25) == 1000 / 25 - 1)
|
||
{
|
||
await c.UnlockAsync(); // 每 1000 毫秒重试一次。解锁间隔一定要超过1s否则导致飞控以后无法解锁
|
||
}
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
}
|
||
}));
|
||
*/
|
||
}));
|
||
}
|
||
}
|
||
|
||
private float _LatOffset = 18.4023833f;
|
||
public float LatOffset
|
||
{
|
||
get { return _LatOffset; }
|
||
set { Set(nameof(LatOffset), ref _LatOffset, value); }
|
||
}
|
||
|
||
private float _LngOffset = 109.799761f;
|
||
public float LngOffset
|
||
{
|
||
get { return _LngOffset; }
|
||
set { Set(nameof(LngOffset), ref _LngOffset, value); }
|
||
}
|
||
|
||
|
||
private ICommand _RLTOffsetCommand;
|
||
public ICommand RLTOffsetCommand
|
||
{
|
||
get
|
||
{
|
||
return _RLTOffsetCommand ?? (_RLTOffsetCommand = new RelayCommand(() =>
|
||
{
|
||
// Message.Show($"偏移Lat = {LatOffset}，Lng = {LngOffset}");
|
||
// if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
// await _commModuleManager.RLTOffsetAsync(LatOffset, LngOffset, _copterManager.AcceptingControlCopters);
|
||
|
||
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _LockCommand;
|
||
public ICommand LockCommand
|
||
{
|
||
get
|
||
{
|
||
return _LockCommand ?? (_LockCommand = new RelayCommand(async () =>
|
||
{
|
||
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
{
|
||
if (Alert.Show("您确定要上锁吗？飞行器将无视转速，立即强制停止运转！！！", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning)
|
||
== MessageBoxResult.OK)
|
||
{
|
||
await _commModuleManager.LockAsync(_copterManager.AcceptingControlCopters);
|
||
//await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.LockAsync()));
|
||
}
|
||
}
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _TakeOffCommand;
|
||
public ICommand TakeOffCommand
|
||
{
|
||
get
|
||
{
|
||
return _TakeOffCommand ?? (_TakeOffCommand = new RelayCommand(async () =>
|
||
{
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.TakeOffAsync()));
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _HoverCommand;
|
||
public ICommand HoverCommand
|
||
{
|
||
get
|
||
{
|
||
return _HoverCommand ?? (_HoverCommand = new RelayCommand(async () =>
|
||
{
|
||
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
await _commModuleManager.HoverAsync(_copterManager.AcceptingControlCopters);
|
||
//await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.HoverAsync()));
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _FloatCommand;
|
||
public ICommand FloatCommand
|
||
{
|
||
get
|
||
{
|
||
return _FloatCommand ?? (_FloatCommand = new RelayCommand(async () =>
|
||
{
|
||
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
await _commModuleManager.FloatAsync(_copterManager.AcceptingControlCopters);
|
||
//await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.FloatAsync()));
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _ReturnToLaunchCommand;
|
||
public ICommand ReturnToLaunchCommand
|
||
{
|
||
get
|
||
{
|
||
return _ReturnToLaunchCommand ?? (_ReturnToLaunchCommand = new RelayCommand(async () =>
|
||
{
|
||
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
await _commModuleManager.ReturnToLaunchAsync(_copterManager.AcceptingControlCopters);
|
||
//await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.ReturnToLaunchAsync()));
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
private ICommand _GetVersionsCommand;
|
||
public ICommand GetVersionsCommand
|
||
{
|
||
get
|
||
{
|
||
return _GetVersionsCommand ?? (_GetVersionsCommand = new RelayCommand(async () =>
|
||
{
|
||
//if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
await _commModuleManager.GetVersionsAsync();
|
||
}));
|
||
}
|
||
}
|
||
|
||
//打开校准界面
|
||
private ICommand _CalibrationSingleCommand;
|
||
public ICommand CalibrationSingleCommand
|
||
{
|
||
get
|
||
{
|
||
return _CalibrationSingleCommand ?? (_CalibrationSingleCommand = new RelayCommand(async () =>
|
||
{
|
||
CalibrationWindow calibrationWindow = new CalibrationWindow();
|
||
calibrationWindow.ShowDialog();
|
||
await Task.Delay(100);
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _LoadParamfile;
|
||
public ICommand LoadParamfile
|
||
{
|
||
get
|
||
{
|
||
return _LoadParamfile ?? (_LoadParamfile = new RelayCommand(async () =>
|
||
{
|
||
|
||
var dialog = new OpenFileDialog
|
||
{
|
||
DefaultExt = "param",
|
||
Filter = "飞行参数 (*.param)|*.param"
|
||
};
|
||
if (dialog.ShowDialog() == true)
|
||
{
|
||
|
||
bool ifallok = true;
|
||
var paramlist = ParamFile.loadParamFile(dialog.FileName);
|
||
|
||
foreach (string name in paramlist.Keys)
|
||
{
|
||
float value = float.Parse( paramlist[name].ToString());
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select
|
||
(copter => copter.SetParamAsync(name, value)));
|
||
|
||
/*
|
||
await Task.WhenAll(
|
||
|
||
_copterManager.AcceptingControlCopters.Select(async copter =>
|
||
{
|
||
float getvaule = await copter.GetParamAsync(name);
|
||
if (getvaule != value)
|
||
{
|
||
ifallok = false;
|
||
Alert.Show(copter.Id +"参数["+ name+ "]设置失败,读取的值是[" + getvaule+"]", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
}
|
||
}
|
||
|
||
));
|
||
*/
|
||
|
||
}
|
||
|
||
if (ifallok)
|
||
Alert.Show("所有已选的飞机参数设置完成", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
else
|
||
Alert.Show("有部分飞机参数设置失败，请检查并再次设置", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
}
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
private ICommand _ParamModify;
|
||
public ICommand ParamModify
|
||
{
|
||
get
|
||
{
|
||
return _ParamModify ?? (_ParamModify = new RelayCommand(async () =>
|
||
{
|
||
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
{
|
||
var ModifyParamWindow = new Plane.FormationCreator.ModifyParam();
|
||
if (ModifyParamWindow.ShowDialog() == true)
|
||
{
|
||
bool isLoadParam = ModifyParamWindow.LoadParam;
|
||
string paramstr = ModifyParamWindow.textParamName.Text;
|
||
|
||
if(!isLoadParam)
|
||
{
|
||
float paramvalue = Convert.ToSingle(ModifyParamWindow.textParamValue.Text);
|
||
int num = 0;
|
||
if (_copterManager.AcceptingControlCopters.Count() < _copterManager.Copters.Count)
|
||
num = await _commModuleManager.SetParamAsync(paramstr, paramvalue, _copterManager.AcceptingControlCopters);
|
||
else if (_copterManager.AcceptingControlCopters.Count() == _copterManager.Copters.Count)
|
||
num = await _commModuleManager.SetParamAsync(paramstr, paramvalue);
|
||
Alert.Show($"广播完成！ 当前序列号：{num}");
|
||
}
|
||
else
|
||
{
|
||
if (_copterManager.AcceptingControlCopters.Count() < _copterManager.Copters.Count)
|
||
await _commModuleManager.ReadParamAsnyc(paramstr, _copterManager.AcceptingControlCopters);
|
||
else if (_copterManager.AcceptingControlCopters.Count() == _copterManager.Copters.Count)
|
||
await _commModuleManager.ReadParamAsnyc(paramstr);
|
||
}
|
||
|
||
/*
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select
|
||
(copter => copter.SetParamAsync(paramstr, paramvalue)));
|
||
|
||
bool ifallok = true;
|
||
await Task.WhenAll(
|
||
|
||
_copterManager.AcceptingControlCopters.Select(async copter =>
|
||
{
|
||
float getvaule = await copter.GetParamAsync(paramstr);
|
||
if (getvaule != paramvalue)
|
||
{
|
||
ifallok = false;
|
||
Alert.Show(copter.Id + "设置失败,读取的参数是" + getvaule, "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
}
|
||
}
|
||
|
||
));
|
||
if (ifallok)
|
||
Alert.Show("所有已选的飞机参数设置完成", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
else
|
||
Alert.Show("有部分飞机参数设置失败，请检查并再次设置", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
|
||
*/
|
||
|
||
}
|
||
}
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _TurnOffTestLightsCommand;
|
||
public ICommand TurnOffTestLightsCommand
|
||
{
|
||
get
|
||
{
|
||
return _TurnOffTestLightsCommand ?? (_TurnOffTestLightsCommand = new RelayCommand(async () =>
|
||
{
|
||
int num = 0;
|
||
var o = new { NTF_G_RTLOFF = 1 };
|
||
num = await _commModuleManager.SetMultipleParamAsync(
|
||
"NTF_G_RTLOFF", "1",
|
||
"FS_GCS_ENABLE", "0",
|
||
"NTF_G_RTKTEST", "0",
|
||
"WAYPOINT_GLED", "0");
|
||
Alert.Show($"广播完成！ 当前序列号：{num}");
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
private string _RTKcomvalue = "COM6";
|
||
public string RTKcomvalue
|
||
{
|
||
get { return _RTKcomvalue; }
|
||
set { Set(nameof(RTKcomvalue), ref _RTKcomvalue, value); }
|
||
}
|
||
|
||
//RTK收发线程
|
||
private static void mainloop()
|
||
{
|
||
|
||
|
||
|
||
}
|
||
|
||
|
||
private async Task<byte[]> ReadRTKPacketAsync()
|
||
{
|
||
if (!Rtkport.IsOpen)
|
||
{
|
||
return null;
|
||
}
|
||
|
||
// limit to 110 byte packets
|
||
byte[] buffer;
|
||
int length = 0;
|
||
|
||
try
|
||
{
|
||
length = Math.Min(180, Rtkport.BytesToRead());
|
||
|
||
if (length > 0)//while (Rtkport.BytesToRead() > 0)
|
||
|
||
{
|
||
buffer = new byte[length];
|
||
|
||
int read = await Rtkport.ReadAsync(buffer, 0, Math.Min(buffer.Length, length));
|
||
|
||
// sendData(buffer, (byte)read);
|
||
|
||
}
|
||
else
|
||
return null;
|
||
|
||
/*
|
||
if (await Rtkport.ReadAsync(buffer, 0, 1) == 0)
|
||
{
|
||
// 做简单数据解析，得到协议类型，后续数据长度等
|
||
}
|
||
//根据数据长度读取实际数据
|
||
if (await Rtkport.ReadAsync(buffer, 1, 1) == 0)
|
||
{
|
||
|
||
}
|
||
*/
|
||
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
return null;
|
||
}
|
||
return buffer;
|
||
|
||
}
|
||
|
||
|
||
|
||
private ICommand _SendRTKCommand;
|
||
public ICommand SendRTKCommand
|
||
{
|
||
get
|
||
{
|
||
return _SendRTKCommand ?? (_SendRTKCommand = new RelayCommand(async () =>
|
||
{
|
||
if (!trkthreadrun)
|
||
{
|
||
Rtkport = new SerialPortConnection(RTKcomvalue, 57600) as IConnection;
|
||
await Rtkport.OpenAsync();
|
||
if (!Rtkport.IsOpen)
|
||
{
|
||
Alert.Show("无法打开" + RTKcomvalue, "警告", MessageBoxButton.OK, MessageBoxImage.Warning);
|
||
return;
|
||
}
|
||
trkthreadrun = true; //开始运行后台任务
|
||
/* //线程方式后台运行rtk转发任务
|
||
thrtk = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop))
|
||
{
|
||
IsBackground = true,
|
||
Name = "injectgps"
|
||
};
|
||
thrtk.Start();
|
||
*/
|
||
//后台任务方式运行rtk转发任务
|
||
Alert.Show("RTK数据开始发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
RTKState = "RTK数据发送中";
|
||
RTKbtntxt = "停止RTK";
|
||
|
||
await Task.Run(async () =>
|
||
{
|
||
//读取RTK数据循环
|
||
while (trkthreadrun)
|
||
{
|
||
//读入RTK数据
|
||
var packet = await ReadRTKPacketAsync().ConfigureAwait(false);
|
||
if (packet != null)
|
||
{
|
||
Console.WriteLine("rev:" + (ushort)packet.Length);
|
||
|
||
//分发到每个飞机
|
||
/*
|
||
foreach (var copter in _copterManager.Copters)
|
||
{
|
||
// int iid = Convert.ToInt32(copter.Name);
|
||
//临时用来区分RTK发送数据
|
||
//if (iid<50)
|
||
|
||
await copter.InjectGpsDataAsync(packet, (ushort)packet.Length);
|
||
}
|
||
*/
|
||
await _commModuleManager.InjectGpsDataAsync(packet, (ushort)packet.Length);
|
||
}
|
||
|
||
await Task.Delay(200).ConfigureAwait(false);
|
||
|
||
}
|
||
}).ConfigureAwait(false);
|
||
Alert.Show("RTK数据停止发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
}
|
||
else//停止RTK
|
||
{
|
||
trkthreadrun = false;
|
||
Rtkport.Close();
|
||
Rtkport = null;
|
||
|
||
RTKState = "未发送RTK数据";
|
||
RTKbtntxt = "发送RTK";
|
||
}
|
||
/*
|
||
await Task.Run(() =>
|
||
{
|
||
if (!Rtkport.IsOpen)
|
||
{
|
||
return null;
|
||
}
|
||
|
||
// var packet = await ReadRTKPacketAsync().ConfigureAwait(false);
|
||
|
||
while (trkthreadrun)
|
||
{
|
||
await ReadRTKPacketAsync();
|
||
|
||
// if (await Rtkport.ReadAsync(buffer, 0, Math.Min(buffer.Length, 110)) > 0)
|
||
{
|
||
}
|
||
}
|
||
});*/
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
private ICommand _SendRTCMCommand;
|
||
public ICommand SendRTCMCommand
|
||
{
|
||
get
|
||
{
|
||
return _SendRTCMCommand ?? (_SendRTCMCommand = new RelayCommand(async () =>
|
||
{
|
||
FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
|
||
if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0)
|
||
{
|
||
Alert.Show("作为参照的原点未设置，无法发送RTCM！", "提示");
|
||
return;
|
||
}
|
||
|
||
rtcmthreadrun = !rtcmthreadrun;
|
||
if (rtcmthreadrun)
|
||
{
|
||
RTKState = "RTK数据发送中";
|
||
NTRIPbtntxt = "停止RTCM";
|
||
Alert.Show("RTCM数据开始发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
|
||
await Task.Run(async () =>
|
||
{
|
||
string url = "http://qxarpz003:1dc880b@rtk.ntrip.qxwz.com:8002/RTCM32_GGB";
|
||
CommNTRIP commNTRIP = new CommNTRIP(url, _flightTaskManager.OriginLat, _flightTaskManager.OriginLng);
|
||
rtcm3 rtcm3 = new rtcm3();
|
||
commNTRIP.doConnect();
|
||
//读取RTK数据循环
|
||
while (rtcmthreadrun)
|
||
{
|
||
byte[] buffer = new byte[180];
|
||
|
||
while (commNTRIP.BytesToRead > 0)
|
||
{
|
||
int read = commNTRIP.Read(buffer, 0, Math.Min(buffer.Length, commNTRIP.BytesToRead));
|
||
|
||
for (int a = 0; a < read; a++)
|
||
{
|
||
int seenmsg = -1;
|
||
// rtcm
|
||
if ((seenmsg = rtcm3.Read(buffer[a])) > 0)
|
||
{
|
||
await _commModuleManager.InjectGpsRTCMDataAsync(rtcm3.packet, rtcm3.length);
|
||
}
|
||
}
|
||
}
|
||
await Task.Delay(10);
|
||
}
|
||
NTRIPbtntxt = "发送RTCM";
|
||
RTKState = "未发送RTK数据";
|
||
Alert.Show("RTCM数据停止发送", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
});
|
||
}
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
private ICommand _LandCommand;
|
||
public ICommand LandCommand
|
||
{
|
||
get
|
||
{
|
||
return _LandCommand ?? (_LandCommand = new RelayCommand(async () =>
|
||
{
|
||
if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
|
||
await _commModuleManager.LandAsync(_copterManager.AcceptingControlCopters);
|
||
/*
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.LandAsync()));
|
||
*/
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
private ICommand _ConnectCommand;
|
||
public ICommand ConnectCommand
|
||
{
|
||
get
|
||
{
|
||
return _ConnectCommand ?? (_ConnectCommand = new RelayCommand(async () =>
|
||
{
|
||
//await ConnectAsync();
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async copter =>
|
||
{
|
||
//await commModule.ConnectAsync();
|
||
await copter.ConnectAsync();
|
||
//await copter.GetCopterDataAsync();
|
||
}));
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _DisconnectCommand;
|
||
public ICommand DisconnectCommand
|
||
{
|
||
get
|
||
{
|
||
return _DisconnectCommand ?? (_DisconnectCommand = new RelayCommand(async () =>
|
||
{
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.DisconnectAsync()));
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _FlyToAltitudeCommand;
|
||
public ICommand FlyToAltitudeCommand
|
||
{
|
||
get
|
||
{
|
||
return _FlyToAltitudeCommand ?? (_FlyToAltitudeCommand = new RelayCommand<float>(async alt =>
|
||
{
|
||
await _formationController.FlyToAltitudeAsync(_copterManager.AcceptingControlCopters, targetAlt: alt).ConfigureAwait(false);
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _FlyInCircleCommand;
|
||
public ICommand FlyInCircleCommand
|
||
{
|
||
get
|
||
{
|
||
return _FlyInCircleCommand ?? (_FlyInCircleCommand = new RelayCommand(async () =>
|
||
{
|
||
await _formationController.FlyInCircleAsync(_copterManager.AcceptingControlCopters, centerDirection: 180, radius: 10, loop: true).ConfigureAwait(false);
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _FlyAroundCenterOfCoptersCommand;
|
||
public ICommand FlyAroundCenterOfCoptersCommand
|
||
{
|
||
get
|
||
{
|
||
return _FlyAroundCenterOfCoptersCommand ?? (_FlyAroundCenterOfCoptersCommand = new RelayCommand(async () =>
|
||
{
|
||
await _formationController.FlyAroundCenterOfCoptersAsync(_copterManager.AcceptingControlCopters).ConfigureAwait(false);
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _FlyInRectangleCommand;
|
||
public ICommand FlyInRectangleCommand
|
||
{
|
||
get
|
||
{
|
||
return _FlyInRectangleCommand ?? (_FlyInRectangleCommand = new RelayCommand(async () =>
|
||
{
|
||
await _formationController.FlyInRectangleAsync(_copterManager.AcceptingControlCopters, startDirection: 180, sideLength1: 15, sideLength2: 5, loop: true).ConfigureAwait(false);
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _FlyToVerticalLineAndMakeCircleCommand;
|
||
public ICommand FlyToVerticalLineAndMakeCircleCommand
|
||
{
|
||
get
|
||
{
|
||
return _FlyToVerticalLineAndMakeCircleCommand ?? (_FlyToVerticalLineAndMakeCircleCommand = new RelayCommand(async () =>
|
||
{
|
||
await _formationController.FlyToVerticalLineAndMakeCircle(
|
||
_copterManager.AcceptingControlCopters,
|
||
centerDirection: 180,
|
||
radius: 10
|
||
).ConfigureAwait(false);
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _MissionStartCommand;
|
||
public ICommand MissionStartCommand
|
||
{
|
||
get
|
||
{
|
||
return _MissionStartCommand ?? (_MissionStartCommand = new RelayCommand(async () =>
|
||
{
|
||
FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
|
||
if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0)
|
||
{
|
||
Alert.Show("作为参照的原点未设置，无法开始任务！", "提示");
|
||
return;
|
||
}
|
||
int utchour = DateTime.UtcNow.AddSeconds(5).Hour;
|
||
int utcminute = DateTime.UtcNow.AddSeconds(5).Minute;
|
||
int utcsecond = DateTime.UtcNow.AddSeconds(5).Second;
|
||
Message.Show("开始任务");
|
||
//循环3次 发送起飞命令 避免通信问题
|
||
for (int i = 0; i < 10; i++)
|
||
{
|
||
await _commModuleManager.DoMissionStartAsync(utchour,
|
||
utcminute,
|
||
utcsecond,
|
||
_flightTaskManager.OriginLng,
|
||
_flightTaskManager.OriginLat
|
||
);
|
||
|
||
/*
|
||
foreach (var vcopter in _copterManager.Copters)
|
||
{
|
||
await vcopter.MissionStartAsync(utchour,
|
||
utcminute,
|
||
utcsecond,
|
||
_flightTaskManager.OriginLng,
|
||
_flightTaskManager.OriginLat
|
||
);
|
||
}
|
||
*/
|
||
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
Alert.Show("所有飞机开始执行航点任务。请勿多次开始任务！", "提示", MessageBoxButton.OK, MessageBoxImage.Information);
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _MissionPauseCommand;
|
||
public ICommand MissionPauseCommand
|
||
{
|
||
get
|
||
{
|
||
return _MissionPauseCommand ?? (_MissionPauseCommand = new RelayCommand(async () =>
|
||
{
|
||
for (int i = 0; i < 3; i++)
|
||
{
|
||
await _commModuleManager.DoMissionPauseAsync();
|
||
}
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _MissionResumeCommand;
|
||
public ICommand MissionResumeCommand
|
||
{
|
||
get
|
||
{
|
||
return _MissionResumeCommand ?? (_MissionResumeCommand = new RelayCommand(async () =>
|
||
{
|
||
int utchour = DateTime.UtcNow.AddSeconds(5).Hour;
|
||
int utcminute = DateTime.UtcNow.AddSeconds(5).Minute;
|
||
int utcsecond = DateTime.UtcNow.AddSeconds(5).Second;
|
||
for (int i = 0; i < 3; i++)
|
||
{
|
||
await _commModuleManager.DoMissionResumeAsync(
|
||
utchour,
|
||
utcminute,
|
||
utcsecond);
|
||
}
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
// private ICommand _SetOriginalPointCommand;
|
||
// public ICommand SetOriginalPointCommand
|
||
// {
|
||
// get
|
||
// {
|
||
// return _SetOriginalPointCommand ?? (_SetOriginalPointCommand = new RelayCommand(() =>
|
||
// {
|
||
// FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
|
||
// _flightTaskManager.SetOriginal();
|
||
// }));
|
||
// }
|
||
// }
|
||
private ICommand _WriteMissionSingleCommand;
|
||
public ICommand WriteMissionSingleCommand
|
||
{
|
||
get
|
||
{
|
||
return _WriteMissionSingleCommand ?? (_WriteMissionSingleCommand = new RelayCommand(async () =>
|
||
{
|
||
var _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
|
||
if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0)
|
||
{
|
||
Alert.Show("作为参照的原点未设置，无法写入相对位置！", "提示");
|
||
return;
|
||
}
|
||
int taskcount = _flightTaskManager.Tasks.Count;
|
||
if (taskcount < 2) return;
|
||
foreach (var copter in _copterManager.AcceptingControlCopters)
|
||
{
|
||
int i = _copterManager.Copters.IndexOf(copter);
|
||
await CollectMissions(i);
|
||
}
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
private ICommand _WriteMissionFailedCommand;
|
||
public ICommand WriteMissionFailedCommand
|
||
{
|
||
get
|
||
{
|
||
return _WriteMissionFailedCommand ?? (_WriteMissionFailedCommand = new RelayCommand(async () =>
|
||
{
|
||
var _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
|
||
if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0)
|
||
{
|
||
Alert.Show("作为参照的原点未设置，无法写入相对位置！", "提示");
|
||
return;
|
||
}
|
||
int coptercount = _copterManager.Copters.Count;
|
||
int taskcount = _flightTaskManager.Tasks.Count;
|
||
if (taskcount < 2) return;
|
||
foreach(KeyValuePair<int, CommWriteMissinState> kv in _commModuleManager.MissionWriteState)
|
||
{
|
||
if (!kv.Value.SendAchieved || !kv.Value.WriteSucceed)
|
||
{
|
||
var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == kv.Key.ToString());
|
||
int i = _copterManager.Copters.IndexOf(copter);
|
||
await CollectMissions(i);
|
||
}
|
||
}
|
||
}));
|
||
}
|
||
}
|
||
|
||
private ICommand _WriteMissionCommand;
|
||
public ICommand WriteMissionCommand
|
||
{
|
||
get
|
||
{
|
||
return _WriteMissionCommand ?? (_WriteMissionCommand = new RelayCommand(async () =>
|
||
{
|
||
var _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
|
||
if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0)
|
||
{
|
||
Alert.Show("作为参照的原点未设置，无法写入相对位置！", "提示");
|
||
return;
|
||
}
|
||
|
||
int coptercount = _copterManager.Copters.Count;
|
||
int taskcount = _flightTaskManager.Tasks.Count;
|
||
if (taskcount < 2) return;
|
||
|
||
_commModuleManager.ClearMissionWriteState();
|
||
for (int i = 0; i < coptercount; i++)
|
||
{
|
||
///写航线开始
|
||
await CollectMissions(i).ConfigureAwait(false);
|
||
}
|
||
Alert.Show($"任务写入完成!请检测失败的飞机");
|
||
}));
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 收集航点信息
|
||
/// </summary>
|
||
/// <param name="i">copterIndex</param>
|
||
/// <returns></returns>
|
||
private async Task CollectMissions(int i)
|
||
{
|
||
float groudAlt = _copterManager.Copters[i].GroundAlt;
|
||
var missions = new List<IMission>();
|
||
var _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
|
||
for (int j = 0; j < _flightTaskManager.Tasks.Count; j++)
|
||
{
|
||
|
||
switch (_flightTaskManager.Tasks[j].TaskType)
|
||
{
|
||
case FlightTaskType.TakeOff:
|
||
int takeofftime = (int)(_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetAlt / 2.5 + 5);
|
||
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
|
||
missions.Add(Mission.CreateTakeoffMission(
|
||
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TakeOffWaitTime,
|
||
_flightTaskManager.Tasks[j].TakeOffTime,
|
||
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLat - _flightTaskManager.OriginLat,
|
||
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLng - _flightTaskManager.OriginLng,
|
||
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetAlt - groudAlt)
|
||
);
|
||
break;
|
||
|
||
case FlightTaskType.FlyTo:
|
||
|
||
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
|
||
if (_flightTaskManager.Tasks[j].SingleCopterInfos[i].IsChangeSpeed)
|
||
{
|
||
missions.Add(Mission.CreateChangeSpeedMission(
|
||
_flightTaskManager.Tasks[j].SingleCopterInfos[i].LevelSpeed,
|
||
_flightTaskManager.Tasks[j].SingleCopterInfos[i].UpSpeed,
|
||
_flightTaskManager.Tasks[j].SingleCopterInfos[i].DownSpeed)
|
||
);
|
||
}
|
||
double Lat = _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLat - _flightTaskManager.OriginLat;
|
||
double Lng = _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLng - _flightTaskManager.OriginLng;
|
||
if (_flightTaskManager.Tasks[j].SingleCopterInfos[i].IsLandWaypoint)
|
||
{
|
||
Lat = 90;
|
||
Lng = 180;
|
||
}
|
||
|
||
missions.Add(Mission.CreateWaypointMission(
|
||
_flightTaskManager.Tasks[j].LoiterTime,
|
||
_flightTaskManager.Tasks[j].FlytoTime,
|
||
Lat,
|
||
Lng,
|
||
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt - groudAlt)
|
||
);
|
||
break;
|
||
case FlightTaskType.Loiter:
|
||
break;
|
||
case FlightTaskType.ReturnToLand:
|
||
missions.Add(Mission.CreateReturnToLaunchMission());
|
||
break;
|
||
|
||
case FlightTaskType.Land:
|
||
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
|
||
|
||
missions.Add(Mission.CreateLandMission(
|
||
_flightTaskManager.Tasks[j].SingleCopterInfos[i].LandWaitTime)
|
||
);
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
bool result = await _commModuleManager.WriteMissionListAsync(short.Parse(_copterManager.Copters[i].Id), missions);
|
||
//CommWriteMissinState state = new CommWriteMissinState(result);
|
||
//_commModuleManager.missionWriteState.Add(int.Parse(_copterManager.Copters[i].Id), state);
|
||
if (!result)
|
||
{
|
||
Message.Show($"飞机：{_copterManager.Copters[i].Id} 通信模块传输失败!");
|
||
}
|
||
else
|
||
{
|
||
Message.Show($"飞机：{_copterManager.Copters[i].Id} 通信模块传输完成!");
|
||
}
|
||
await Task.Delay(500).ConfigureAwait(false);
|
||
}
|
||
|
||
private List<IMission> CreateLEDMissions(IEnumerable<LEDInfo> LEDInfos)
|
||
{
|
||
List<IMission> missions = new List<IMission>();
|
||
foreach (LEDInfo ledInfo in LEDInfos)
|
||
{
|
||
Color color = (Color)ColorConverter.ConvertFromString("#" + ledInfo.LEDRGB);
|
||
int ledMode = ledInfo.LEDMode;
|
||
if (ledMode == 8) ledMode = 50;
|
||
IMission LEDMission = Mission.CreateLEDControlMission(
|
||
(int)(ledInfo.Delay * 10),
|
||
ledMode,
|
||
ledInfo.LEDRate,
|
||
0, //ledInfo.LEDTimes,
|
||
color.R,
|
||
color.G,
|
||
color.B
|
||
);
|
||
missions.Add(LEDMission);
|
||
}
|
||
return missions;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
private ICommand _FlagCommand;
|
||
public ICommand FlagCommand
|
||
{
|
||
get
|
||
{
|
||
return _FlagCommand ?? (_FlagCommand = new RelayCommand(async () =>
|
||
{
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter =>
|
||
{
|
||
|
||
var copters = _copterManager.Copters;
|
||
int index = copters.IndexOf(copter);
|
||
_copterManager.CopterStatus[index] = true;
|
||
return Task.Run(()=> { });
|
||
}));
|
||
}));
|
||
}
|
||
}
|
||
|
||
private async Task UnlockTestSingleCopterAsync(ICopter SingleCopter)
|
||
{
|
||
await SingleCopter.UnlockAsync();
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
|
||
for (int i = 0; !SingleCopter.IsUnlocked; i++)
|
||
{
|
||
//8秒内每1000毫秒尝试解锁一次
|
||
//解锁间隔一定要超过1s否则导致飞控以后无法解锁
|
||
|
||
if (i >320)
|
||
return; //无法解锁后面不用执行了
|
||
if (i % (1000 / 25) == 1000 / 25 - 1)
|
||
{
|
||
await SingleCopter.UnlockAsync(); // 每 1000 毫秒重试一次。
|
||
}
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
}
|
||
|
||
|
||
await Task.Delay(1000);
|
||
if (SingleCopter.IsUnlocked)
|
||
{
|
||
await SingleCopter.SetShowLEDAsync(true);
|
||
await Task.Delay(50);
|
||
await SingleCopter.SetShowLEDAsync(true);
|
||
}
|
||
|
||
|
||
await Task.Delay(5000);
|
||
|
||
await SingleCopter.LockAsync();
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
for (int i = 0; SingleCopter.IsUnlocked; i++)
|
||
{
|
||
//8秒内每1000毫秒尝试解锁一次
|
||
//解锁间隔一定要超过1s否则导致飞控以后无法解锁
|
||
|
||
if (i > 320)
|
||
return; //无法解锁后面不用执行了
|
||
if (i % (1000 / 25) == 1000 / 25 - 1)
|
||
{
|
||
await SingleCopter.LockAsync(); // 每 1000 毫秒重试一次。
|
||
}
|
||
await Task.Delay(25).ConfigureAwait(false);
|
||
}
|
||
|
||
await Task.Delay(1000);
|
||
if (!SingleCopter.IsUnlocked)
|
||
{
|
||
await SingleCopter.SetShowLEDAsync(false);
|
||
await Task.Delay(50);
|
||
await SingleCopter.SetShowLEDAsync(false);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
private ICommand _TestCommand;
|
||
public ICommand TestCommand
|
||
{
|
||
get
|
||
{
|
||
return _TestCommand ?? (_TestCommand = new RelayCommand(async () =>
|
||
{
|
||
|
||
|
||
Message.Show("解锁测试--开始");
|
||
int i = 0;
|
||
var tasksUnlockTest = new Task[_copterManager.Copters.Count];
|
||
foreach (var vcopter in _copterManager.Copters)
|
||
{
|
||
tasksUnlockTest[i++] = UnlockTestSingleCopterAsync(vcopter);
|
||
//i++;
|
||
}
|
||
await Task.WhenAll(tasksUnlockTest).ConfigureAwait(false);
|
||
Message.Show("解锁测试--完成!");
|
||
return;
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
foreach (var vcopter in _copterManager.Copters)
|
||
{
|
||
await vcopter.UnlockAsync();
|
||
}
|
||
await Task.Delay(1000);
|
||
|
||
foreach (var vcopter in _copterManager.Copters)
|
||
{
|
||
if (vcopter.IsUnlocked)
|
||
await vcopter.SetShowLEDAsync(true);
|
||
}
|
||
|
||
await Task.Delay(5000);
|
||
|
||
foreach (var vcopter in _copterManager.Copters)
|
||
{
|
||
await vcopter.LockAsync();
|
||
await vcopter.SetShowLEDAsync(false);
|
||
|
||
}
|
||
|
||
|
||
return;
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
var firstCopter = _copterManager.Copters.First();
|
||
|
||
// Test TurnAsync.
|
||
//var latLng = GeographyUtils.CalcLatLngSomeMetersAway(firstCopter.Latitude, firstCopter.Longitude, 135, 10);
|
||
//await _formationController.TurnAsync(_copterManager.AcceptingControlCopters, latLng.Item1, latLng.Item2);
|
||
|
||
// Test FlyToLatLngAsync.
|
||
//var copters = _copterManager.AcceptingControlCopters;
|
||
//var centerLat = copters.Average(c => c.Latitude);
|
||
//var centerLng = copters.Average(c => c.Longitude);
|
||
//var latDelta = 10 * GeographyUtils.MetersToLocalLat;
|
||
//var lngDelta = 10 * GeographyUtils.GetMetersToLocalLon(copters.First().Latitude);
|
||
//var targetLat = centerLat + latDelta;
|
||
//var targetLng = centerLng + lngDelta;
|
||
//await _formationController.FlyToLatLngAsync(copters, targetLat, targetLng);
|
||
|
||
// Test FlyInCircleAsync.
|
||
//await _formationController.FlyInCircleAsync(_copterManager.AcceptingControlCopters, centerDirection: 180, radius: 10, loop: false);
|
||
|
||
// Test FlyAroundCenterOfCoptersAsync.
|
||
//await _formationController.FlyAroundCenterOfCoptersAsync(_copterManager.AcceptingControlCopters);
|
||
|
||
// Test FlyToLatLngAsync.
|
||
//var targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway(firstCopter.Latitude, firstCopter.Longitude, 135, 20);
|
||
//await _formationController.FlyToLatLngAsync(firstCopter, targetLatLng.Item1, targetLatLng.Item2);
|
||
|
||
//firstCopter.LoiterTurns(1, 10);
|
||
|
||
//var radius = await firstCopter.GetParamAsync("CIRCLE_RADIUS");
|
||
|
||
////await firstCopter.SetParamAsync("CIRCLE_RADIUS", 1500);
|
||
////var param = await firstCopter.GetParamAsync("CIRCLE_RADIUS");
|
||
//// Alert.Show($"半径设置成功啦: {param}");
|
||
|
||
////await firstCopter.SetParamAsync("CIRCLE_RATE", 30);
|
||
//var rate = await firstCopter.GetParamAsync("CIRCLE_RATE");
|
||
|
||
//Alert.Show($"半径: {radius} 角速度: {rate}");
|
||
|
||
//await firstCopter.SetMobileControlAsync(yaw: 90);
|
||
|
||
//if (!_listeningMissionItemReceived)
|
||
//{
|
||
// firstCopter.MissionItemReceived += (sender, e) => Alert.Show($"{e.Index} {e.Total} {(Protocols.MAVLink.MAV_CMD)e.Details.id}");
|
||
// _listeningMissionItemReceived = true;
|
||
//}
|
||
//await firstCopter.RequestMissionListAsync();
|
||
|
||
var copter = firstCopter;
|
||
|
||
///* 写航线 */
|
||
//// 任务总数。
|
||
//await copter.SetMissionCountAsync(6).ConfigureAwait(false);
|
||
//// 起飞前准备。
|
||
//await copter.WritePreTakeOffMissionAsync().ConfigureAwait(false);
|
||
//// 起飞。
|
||
//await copter.WriteTakeOffMissionAsync().ConfigureAwait(false);
|
||
//// 不断自增的编号。
|
||
//ushort index = 2;
|
||
//// 航点。
|
||
//await copter.WriteWaypointMissionAsync(new EHLocation(copter).AddLatLngAlt(0.001, 0.002, 2), index++).ConfigureAwait(false);
|
||
//// 航点。
|
||
//await copter.WriteWaypointMissionAsync(new EHLocation(copter).AddLatLngAlt(0.003, 0.002, 2), index++).ConfigureAwait(false);
|
||
//// 返航。
|
||
//await copter.WriteReturnToLaunchMissionAsync(index++).ConfigureAwait(false);
|
||
//// 降落。
|
||
//await copter.WriteLandMissionAsync(index++).ConfigureAwait(false);
|
||
|
||
///* 读航线 */
|
||
// 监听“任务项已接收”事件。
|
||
//(copter as EHCopter).MissionItemReceived += (sender, e) => Alert.Show($"{e.Mission.Sequence} {e.Mission.Latitude} {e.Mission.Command}");
|
||
// 请求任务列表。
|
||
var missions = await copter.RequestMissionListAsync();
|
||
// Alert.Show(string.Join(Environment.NewLine, missions.Select(m => $"{m.Sequence}\t{m.Command}\t\t{m.Latitude}")));
|
||
|
||
//ICopter previousCopter = firstCopter;
|
||
//foreach (var item in _copterManager.Copters.Where(c => c != firstCopter))
|
||
//{
|
||
// item.Follow(previousCopter, keepYawDifference: false, keepFacingTarget: false);
|
||
// previousCopter = item;
|
||
//}
|
||
|
||
//var missions = new Mission[]
|
||
//{
|
||
// Mission.CreateWaypointMission(new EHLocation(firstCopter).AddLatLngAlt(0.0001, 0, 10)),
|
||
// Mission.CreateReturnToLaunchMission(),
|
||
// Mission.CreateLandMission()
|
||
//};
|
||
//var result = await firstCopter.WriteMissionsAsync(missions);
|
||
//var result2 = await firstCopter.WriteMissionsAsync(missions);
|
||
//Debug.WriteLine($"{result1} {result2}");
|
||
|
||
//var missions = new Mission[]
|
||
//{
|
||
// Mission.CreateWaypointMission(23.123, 113.123, 10),
|
||
// Mission.CreateWaypointMission(23.123, 113.123, 10),
|
||
// Mission.CreateWaypointMission(23.123, 113.123, 10),
|
||
// Mission.CreateWaypointMission(23.123, 113.123, 10),
|
||
// Mission.CreateWaypointMission(23.123, 113.123, 10),
|
||
// Mission.CreateWaypointMission(23.123, 113.123, 10),
|
||
// Mission.CreateWaypointMission(23.123, 113.123, 10),
|
||
// Mission.CreateWaypointMission(23.123, 113.123, 10),
|
||
// Mission.CreateWaypointMission(23.123, 113.123, 10),
|
||
// Mission.CreateWaypointMission(23.123, 113.123, 10),
|
||
// Mission.CreateReturnToLaunchMission(),
|
||
// Mission.CreateLandMission()
|
||
//};
|
||
//var result = await firstCopter.WriteMissionListAsync(missions);
|
||
//MessageBox.Show($"The result of WriteMissions: {result}");
|
||
|
||
// 跟随测试
|
||
/*
|
||
ICopter previousCopter = firstCopter;
|
||
foreach (var item in _copterManager.Copters.Where(c => c != firstCopter))
|
||
{
|
||
item.Follow(previousCopter, keepYawDifference: false, keepFacingTarget: false);
|
||
previousCopter = item;
|
||
}
|
||
*/
|
||
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async c =>
|
||
{
|
||
|
||
float gpsLed = await c.GetParamAsync("NOTI_GPSLED");
|
||
if (gpsLed == 0.0f)
|
||
{
|
||
await LEDFlashAsync(c, true);
|
||
}
|
||
else
|
||
{
|
||
await LEDFlashAsync(c, false);
|
||
}
|
||
|
||
// for (int jj = 0; jj < 5; jj++)
|
||
{
|
||
// await LEDFlashAsync(c, true);
|
||
// await c.SetParamAsync("THR_ALT_P", AltP);
|
||
// await c.SetParamAsync("WPNAV_ACCEL_Z", AltP);
|
||
// await c.SetParamAsync("WPNAV_ACCEL", AltP);
|
||
// await c.SetParamAsync("WPNAV_SPEED", AltP);
|
||
// await c.SetParamAsync("WPNAV_SPEED_DN", AltP);
|
||
// await c.SetParamAsync("WPNAV_SPEED_UP", AltP);
|
||
// await LEDFlashAsync(c, false);
|
||
// await LEDFlashAsync(c, true);
|
||
// await Task.Delay(500).ConfigureAwait(false);
|
||
|
||
// await Task.Delay(1000).ConfigureAwait(false);
|
||
|
||
// await LEDFlashAsync(c, true);
|
||
// await Task.Delay(1000).ConfigureAwait(false);
|
||
}
|
||
}));
|
||
}));
|
||
}
|
||
}
|
||
|
||
private async Task LEDFlashAsync(ICopter copter, bool isOn)
|
||
{
|
||
// float gpsLed = await c.GetParamAsync("NOTI_GPSLED");
|
||
float ledControl = 0.0f;
|
||
if (isOn)
|
||
{
|
||
ledControl = 1.0f;
|
||
}
|
||
var tasks = new Task[2];
|
||
//tasks[0] = copter.SetParamAsync("NOTI_GPSLED", ledControl);
|
||
//tasks[1] = copter.SetParamAsync("NOTI_ARMLED", ledControl);
|
||
tasks[0] = Task.Run(async () =>
|
||
{
|
||
try
|
||
{
|
||
await copter.SetParamAsync("NOTI_GPSLED", ledControl, 5000);
|
||
}
|
||
catch (TimeoutException e)
|
||
{
|
||
_logger.Log($"NOTI_GPSLED 超时, CopterId: {copter.Id}。");
|
||
}
|
||
});
|
||
tasks[1] = Task.Run(async () =>
|
||
{
|
||
try
|
||
{
|
||
await copter.SetParamAsync("NOTI_ARMLED", ledControl, 5000);
|
||
}
|
||
catch (TimeoutException e)
|
||
{
|
||
_logger.Log($"NOTI_ARMLED 超时, CopterId: {copter.Id}。");
|
||
}
|
||
});
|
||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
||
|
||
}
|
||
|
||
private bool _listeningMissionItemReceived;
|
||
|
||
private ICommand _StopTaskCommand;
|
||
public ICommand StopTaskCommand
|
||
{
|
||
get
|
||
{
|
||
return _StopTaskCommand ?? (_StopTaskCommand = new RelayCommand(async () =>
|
||
{
|
||
await _formationController.AllStop().ConfigureAwait(false);
|
||
}));
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
}
|