Plane.Sdk3/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs

using Plane.Communication;
using Plane.Copters;
using Plane.Messaging;
using System;
using System.Collections.ObjectModel;
using System.ComponentModel;
using System.Threading;
using System.Threading.Tasks;

namespace Plane.CopterManagement
{
    public class SingleCopterManager : PLObservableObject, ICopterManager
    {
        private ICopter _Copter = EmptyCopter.Instance;
        private bool _IsSearching;
        private IPLMessenger _messenger;
        private UdpServerConnectionManager _udpServerConnectionManager = UdpServerConnectionManager.Instance;

        public SingleCopterManager(IPLMessenger messenger, SynchronizationContext uiSyncContext) : base(uiSyncContext)
        {
            _messenger = messenger;
        }

        public ObservableCollection<ICopter> AllCopters { get { throw new NotSupportedException(); } }

        /// <summary>
        /// 获取或设置单个 <see cref="ICopter"/> 实例。用 <see cref="EmptyCopter.Instance"/> 代替了 null，使用时不必判断是否为 null。
        /// </summary>
        public ICopter Copter
        {
            get { return _Copter; }
            set { Set(nameof(Copter), ref _Copter, value ?? EmptyCopter.Instance); }
        }

        public bool IsSearching
        {
            get { return _IsSearching; }
            private set { Set(nameof(IsSearching), ref _IsSearching, value); }
        }

        public ObservableCollection<ICopter> SelectedCopters { get { throw new NotSupportedException(); } }

        public async Task AddOrUpdateCopterAsync(string id, string name, IConnection connection)
        {
            var copter = CreateCopter(id, name, connection);
            await ConnectAsync(copter);
        }

        public async Task<bool> CheckStatusAndFlyToAsync(double lat, double lng)
        {
            var copter = this.Copter;
            if (copter == null) return false;

            switch (copter.State)
            {
                case CopterState.TakingOff:
                case CopterState.HoverMode:
                case CopterState.CommandMode:
                case CopterState.Returning:
                case CopterState.Landing:
                    await copter.FlyToAsync(lat, lng).ConfigureAwait(false);
                    return true;

                case CopterState.Locked:
                case CopterState.Initialized:
                case CopterState.FloatMode:
                default:
                    return false;
            }
        }

        public async Task ConnectAsync(ICopter copter)
        {
            await DisconnectAsync(this.Copter);
            RegisterCopterEventHandlers(copter);
            await copter.ConnectAsync();
            copter.StartMobileControl();
            this.Copter = copter;
            _uiSyncContext.Post(() =>
                _messenger.Send(new CopterConnectedMessage(copter)));
        }

        public async Task DisconnectAsync(ICopter copter)
        {
            copter.StopMobileControl();
            await copter.DisconnectAsync().ConfigureAwait(false);
            UnregisterCopterEventHandlers(copter);
        }

        public async Task DisconnectAsync()
        {
            await DisconnectAsync(this.Copter);
            _udpServerConnectionManager.StopReceiving();
            _udpServerConnectionManager.ClearConnections();
        }

        public Task FloatAsync()
        {
            return Copter.FloatAsync();
        }

        public Task FlyToAsync(double lat, double lng)
        {
            return Copter.FlyToAsync(lat, lng);
        }

        public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
        {
            return Copter.FlyToAsync(lat, lng, alt);
        }

        public Task HoverAsync()
        {
            return Copter.HoverAsync();
        }
        public Task GuidAsync()
        {
            return Copter.GuidAsync();
        }

        public Task LandAsync()
        {
            return Copter.LandAsync();
        }

        public Task LockAsync()
        {
            return Copter.LockAsync();
        }

        public Task ReturnToLaunchAsync()
        {
            return Copter.ReturnToLaunchAsync();
        }

        public Task SetChannelsAsync(ushort? ch1 = default(ushort?), ushort? ch2 = default(ushort?), ushort? ch3 = default(ushort?), ushort? ch4 = default(ushort?), ushort? ch5 = default(ushort?), ushort? ch6 = default(ushort?), ushort? ch7 = default(ushort?), ushort? ch8 = default(ushort?))
        {
            return Copter.SetChannelsAsync(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8);
        }

        public Task SetMobileControlAsync(ushort? ch1 = default(ushort?), ushort? ch2 = default(ushort?), ushort? ch3 = default(ushort?), ushort? ch4 = default(ushort?), ushort? ch5 = default(ushort?), ushort? ch6 = default(ushort?), ushort? ch7 = default(ushort?), ushort? ch8 = default(ushort?), float? yaw = default(float?))
        {
            return Copter.SetMobileControlAsync(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, yaw);
        }

        public Task StartEmergencyHoverAsync()
        {
            return Copter.StartEmergencyHoverAsync();
        }
        public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
        {
            return Copter.MissionStartAsync(hour_utc, minute_utc, second_utc, Missionlng, Missionlat);
        }

        public void StartMobileControl(int? millisecondsInterval = default(int?))
        {
            Copter.StartMobileControl(millisecondsInterval);
        }

        public virtual void StartSearching()
        {
            if (!IsSearching)
            {
                _udpServerConnectionManager.ConnectionEstablished += OnConnectionEstablished;
                _udpServerConnectionManager.StartReceiving();
                IsSearching = true;
            }
        }

        public void StopEmergencyHover()
        {
            Copter.StopEmergencyHover();
        }

        public void StopMobileControl()
        {
            Copter.StopMobileControl();
        }

        public void StopSearching()
        {
            if (IsSearching)
            {
                _udpServerConnectionManager.ConnectionEstablished -= OnConnectionEstablished;
                IsSearching = false;
            }
        }

        public Task TakeOffAsync()
        {
            return Copter.TakeOffAsync();
        }
        public Task InjectGpsDataAsync(byte[] data, ushort length)
        {
            return Copter.InjectGpsDataAsync(data, length);
        }

        public Task MotorTestAsync(int motor)
        {
            return Copter.TakeOffAsync(motor);
        }


        public Task TakeOffAsync(float alt)
        {
            return Copter.TakeOffAsync(alt);
        }

        public Task UnlockAsync()
        {
            return Copter.UnlockAsync();
        }

        public Task LEDAsync()
        {
            return Copter.LEDAsync();
        }

        protected virtual ICopter CreateCopter(string id, string name, IConnection connection)
        {
            return new PLCopter(connection, _uiSyncContext)
            {
                Id = id,
                Name = name
            };
        }

        protected virtual void RegisterCopterEventHandlers(ICopter copter)
        {
            copter.HeartbeatReceived += Copter_HeartbeatReceived;
            copter.LocationChanged += Copter_LocationChanged;
            copter.PropertyChanged += Copter_PropertyChanged;
            copter.AltitudeChanged += Copter_AltitudeChanged;
            copter.AttitudeChanged += Copter_AttitudeChanged;
            copter.PairingCompleted += Copter_PairingCompleted;
        }

        protected virtual void UnregisterCopterEventHandlers(ICopter copter)
        {
            copter.HeartbeatReceived -= Copter_HeartbeatReceived;
            copter.LocationChanged -= Copter_LocationChanged;
            copter.PropertyChanged -= Copter_PropertyChanged;
            copter.AltitudeChanged -= Copter_AltitudeChanged;
            copter.AttitudeChanged -= Copter_AttitudeChanged;
            copter.PairingCompleted -= Copter_PairingCompleted;
        }

        private void Copter_AltitudeChanged(object sender, EventArgs e)
        {
            _messenger.Send(new CopterAltitudeChangedMessage(sender as ICopter));
        }

        private void Copter_AttitudeChanged(object sender, EventArgs e)
        {
            _messenger.Send(new CopterAttitudeChangedMessage(sender as ICopter));
        }

        private void Copter_HeartbeatReceived(object sender, HeartbeatReceivedEventArgs e)
        {
            _messenger.Send(new CopterHeartbeatReceivedMessage(sender as ICopter));
        }

        private void Copter_LocationChanged(object sender, EventArgs e)
        {
            _messenger.Send(new CopterLocationChangedMessage(sender as ICopter));
        }

        private void Copter_PairingCompleted(object sender, PairingCompletedEventArgs e)
        {
            _messenger.Send(new CopterPairingCompletedMessage(sender as ICopter, e.IsSuccessful));
        }

        private void Copter_PropertyChanged(object sender, PropertyChangedEventArgs e)
        {
            _messenger.Send(new CopterPropertyChangedMessage(sender as ICopter, e.PropertyName));
        }

        private async void OnConnectionEstablished(object sender, ConnectionEstablishedEventArgs e)
        {
            var ip = e.RemoteAddress;
            await AddOrUpdateCopterAsync(ip, ip.Substring(ip.LastIndexOfAny(new[] { '.', ':' }) + 1), e.Connection);
        }
    }
}