465 lines
19 KiB
C#
465 lines
19 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;
|
|
|
|
namespace Plane.FormationCreator.ViewModels
|
|
{
|
|
public class ControlPanelViewModel : ViewModelBase
|
|
{
|
|
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 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();
|
|
for (int i = 0; !c.IsUnlocked; i++)
|
|
{
|
|
if (i % (1000 / 25) == 1000 / 25 - 1)
|
|
{
|
|
await c.UnlockAsync(); // 每 1000 毫秒重试一次。
|
|
}
|
|
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 _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 _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 ICommand _TestCommand;
|
|
public ICommand TestCommand
|
|
{
|
|
get
|
|
{
|
|
return _TestCommand ?? (_TestCommand = new RelayCommand(async () =>
|
|
{
|
|
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);
|
|
}));
|
|
}
|
|
}
|
|
}
|
|
}
|