panxu
71f2870968
[App.xaml.cs]停用tcp连接和编队控制器formationController [Copter.cs]关闭心跳包发送，减小数据发送量 [Plane.FormationCreator.csproj]加入ParamFile.cs修改参数 [ControlPanelViewModel.cs] 1加入读参数文件，可批量设置参数 2修改ReadRTKPacketAsync的bug，导致发送数据量太大>1000/s 3航线暂时改为停2秒 [CopterListViewModel.cs]加入数据收发量统计
1150 lines
45 KiB
C#
1150 lines
45 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;
|
|
|
|
namespace Plane.FormationCreator.ViewModels
|
|
{
|
|
public class ControlPanelViewModel : ViewModelBase
|
|
{
|
|
// serialport
|
|
internal static IConnection Rtkport ;
|
|
private static System.Threading.Thread thrtk;
|
|
private static bool trkthreadrun = false;
|
|
|
|
|
|
|
|
public ControlPanelViewModel(FormationController formationController, CopterManager copterManager)
|
|
{
|
|
_formationController = formationController;
|
|
_copterManager = copterManager;
|
|
}
|
|
|
|
private FormationController _formationController;
|
|
private CopterManager _copterManager;
|
|
|
|
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 ICommand _UnlockAllCommand;
|
|
public ICommand UnlockAllCommand
|
|
{
|
|
get
|
|
{
|
|
return _UnlockAllCommand ?? (_UnlockAllCommand = new RelayCommand(async () =>
|
|
{
|
|
// await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync()));
|
|
|
|
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 ICommand _UnlockCommand;
|
|
public ICommand UnlockCommand
|
|
{
|
|
get
|
|
{
|
|
return _UnlockCommand ?? (_UnlockCommand = new RelayCommand(async () =>
|
|
{
|
|
// await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.UnlockAsync()));
|
|
|
|
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 ICommand _LockCommand;
|
|
public ICommand LockCommand
|
|
{
|
|
get
|
|
{
|
|
return _LockCommand ?? (_LockCommand = new RelayCommand(async () =>
|
|
{
|
|
if (Alert.Show("您确定要上锁吗？飞行器将立即停止运转！", "警告", MessageBoxButton.OKCancel, MessageBoxImage.Warning)
|
|
== MessageBoxResult.OK)
|
|
{
|
|
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 () =>
|
|
{
|
|
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.HoverAsync()));
|
|
}));
|
|
}
|
|
}
|
|
|
|
private ICommand _FloatCommand;
|
|
public ICommand FloatCommand
|
|
{
|
|
get
|
|
{
|
|
return _FloatCommand ?? (_FloatCommand = new RelayCommand(async () =>
|
|
{
|
|
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.FloatAsync()));
|
|
}));
|
|
}
|
|
}
|
|
|
|
private ICommand _ReturnToLaunchCommand;
|
|
public ICommand ReturnToLaunchCommand
|
|
{
|
|
get
|
|
{
|
|
return _ReturnToLaunchCommand ?? (_ReturnToLaunchCommand = new RelayCommand(async () =>
|
|
{
|
|
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.ReturnToLaunchAsync()));
|
|
}));
|
|
}
|
|
}
|
|
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 () =>
|
|
{
|
|
|
|
var ModifyParamWindow = new Plane.FormationCreator.ModifyParam();
|
|
if (ModifyParamWindow.ShowDialog() == true)
|
|
{
|
|
|
|
string paramstr = ModifyParamWindow.textParamName.Text;
|
|
float paramvalue = Convert.ToSingle(ModifyParamWindow.textParamValue.Text);
|
|
|
|
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 string _RTKcomvalue = "COM3";
|
|
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 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 _LandCommand;
|
|
public ICommand LandCommand
|
|
{
|
|
get
|
|
{
|
|
return _LandCommand ?? (_LandCommand = new RelayCommand(async () =>
|
|
{
|
|
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(copter => copter.LandAsync()));
|
|
}));
|
|
}
|
|
}
|
|
|
|
private ICommand _ConnectCommand;
|
|
public ICommand ConnectCommand
|
|
{
|
|
get
|
|
{
|
|
return _ConnectCommand ?? (_ConnectCommand = new RelayCommand(async () =>
|
|
{
|
|
await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async copter =>
|
|
{
|
|
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 () =>
|
|
{
|
|
int utchour = DateTime.UtcNow.AddSeconds(5).Hour;
|
|
int utcminute = DateTime.UtcNow.AddSeconds(5).Minute;
|
|
int utcsecond = DateTime.UtcNow.AddSeconds(5).Second;
|
|
foreach (var vcopter in _copterManager.Copters)
|
|
{
|
|
|
|
await vcopter.MissionStartAsync(utchour,
|
|
utcminute,
|
|
utcsecond,
|
|
110.23456,
|
|
40.23432);
|
|
}
|
|
}));
|
|
}
|
|
}
|
|
|
|
|
|
|
|
private ICommand _WriteMissionCommand;
|
|
public ICommand WriteMissionCommand
|
|
{
|
|
get
|
|
{
|
|
return _WriteMissionCommand ?? (_WriteMissionCommand = new RelayCommand(async () =>
|
|
{
|
|
|
|
|
|
ViewModels.ModifyTaskViewModel _taskmodimodel = ServiceLocator.Current.GetInstance<ViewModels.ModifyTaskViewModel>();
|
|
var _flightTaskManager = _taskmodimodel.FlightTaskManager;
|
|
int coptercount = _copterManager.Copters.Count;
|
|
int taskcount = _flightTaskManager.Tasks.Count;
|
|
bool havewritefault = true;
|
|
if (taskcount < 2) return;
|
|
|
|
for (int i = 0; i < coptercount; i++)
|
|
{
|
|
|
|
///写航线开始
|
|
var missions = new IMission[taskcount]; //不要起飞任务但增加一个起飞后低空航点，见起飞任务,再增加一个降落低空航点，见降落部分
|
|
int missindex = 0;
|
|
|
|
for (int j = 0; j < _flightTaskManager.Tasks.Count; j++)
|
|
{
|
|
|
|
switch (_flightTaskManager.Tasks[j].TaskType)
|
|
{
|
|
case FlightTaskType.TakeOff:
|
|
|
|
missions[missindex++] = Mission.CreateTakeoffMission(2,5,_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLat,
|
|
_flightTaskManager.Tasks[j + 1].SingleCopterInfos[i].TargetLng, _flightTaskManager.Tasks[j+1].SingleCopterInfos[i].TargetAlt);
|
|
//要起飞任务
|
|
|
|
break;
|
|
case FlightTaskType.FlyTo:
|
|
missions[missindex++] = Mission.CreateWaypointMission(5,15,_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLat,
|
|
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLng, _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt);
|
|
break;
|
|
case FlightTaskType.Loiter:
|
|
// missions[missindex++] = Mission.CreateWaypointMission(10,10,_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLat,
|
|
// _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetLng, _flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt);
|
|
break;
|
|
case FlightTaskType.ReturnToLand:
|
|
//降落低空航点
|
|
// missions[missindex++] = Mission.CreateWaypointMission(_flightTaskManager.Tasks[j - 1].SingleCopterInfos[i].TargetLat,
|
|
// _flightTaskManager.Tasks[j - 1].SingleCopterInfos[i].TargetLng, 3);
|
|
//返航
|
|
missions[missindex++] = Mission.CreateReturnToLaunchMission();
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
var result = await _copterManager.Copters[i].WriteMissionListAsync(missions);
|
|
if (!result)
|
|
{
|
|
|
|
Alert.Show($"飞机：{_copterManager.Copters[i].Name} 任务写入失败!");
|
|
return;
|
|
}
|
|
|
|
}
|
|
Alert.Show($"所有任务写入成功!");
|
|
|
|
|
|
// 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 PLCopter).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 IMission[]
|
|
{
|
|
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}");
|
|
*/
|
|
/////////////////写航向结束
|
|
|
|
|
|
}));
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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);
|
|
}));
|
|
}
|
|
}
|
|
}
|
|
}
|