Plane.Sdk3/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs

using Plane.Geography;
using System;
using System.ComponentModel;
using System.Threading;
using System.Threading.Tasks;

namespace Plane.Copters
{
    public abstract partial class CopterImplSharedPart : PLObservableObject, ILocation
    {
        protected CopterState? _forcedState;

        protected bool _isMobileControlActive;

        protected float? _lastChangedAlt;

        protected ILocation _lastChangedLocation;

        protected DateTime _lastRaiseAltitudeChangedTime;

        protected DateTime _lastRaiseLocationChangedTime;

        protected bool _shouldFollow;

        protected DateTime? _takeOffTime;

        /// <summary>
        /// 更新 <see cref="FlightTimeSpan"/> 的时间间隔。
        /// </summary>
        private static readonly TimeSpan _intervalToUpdateFlightTimeSpan = TimeSpan.FromSeconds(0.5);

        private float _followAltDifference;

        private float _followDistance;

        private bool _followKeepFacingTarget;

        private bool _followKeep3DRelativeLocations;

        private bool _followKeepYawDifference;

        private ILocation _followLastDestination;

        private IVisibleStatus _followTarget;

        private float _followSelfDirectionFromTarget;

        private float _followTargetOriginalYaw;

        private float? _followYaw;

        private DateTime _lastUpdateStatusTextTime;

        private int _mobileControlIntervalMilli = 50;

        #region Backing Fields

        private float _AirSpeed;

        private float _Altitude;

        private byte _BatteryPer = 100;

        private ushort _Channel1;

        private ushort _Channel2;

        private ushort _Channel3;

        private ushort _Channel4;

        private ushort _Channel5;

        private ushort _Channel6;

        private ushort _Channel7;

        private ushort _Channel8;

        private ushort? _DesiredChannel1;

        private ushort? _DesiredChannel2;

        private ushort? _DesiredChannel3;

        private ushort? _DesiredChannel4;

        private ushort? _DesiredChannel5;

        private ushort? _DesiredChannel6;

        private ushort? _DesiredChannel7;

        private ushort? _DesiredChannel8;

        private float? _DesiredYaw;

        private float _Elevation;

        private int? _FirmwareVersion;

        private string _FirmwareVersionText;

        private double _FlightDistance;

        private double _FlightDistance2D;

        private TimeSpan _FlightTime;

        private GpsFixType _GpsFixType;

        private float _GpsHdop;

        private float _GroundSpeed;

        private bool _HasSwitchedToGpsMode;

        private short _Heading;

        private ulong _HeartbeatCount;

        private bool _IsAbsolutelyConnected;

        private bool _IsCheckingConnection;

        private bool _CommModuleConnected;

        private bool _IsConnected;

        private bool _IsEmergencyHoverActive;

        private bool _IsGpsAccurate;

        private bool _IsPairing;

        private bool _IsUnlocked;

        private double _Latitude;

        private double _Longitude;

        private FlightMode _Mode;
        private FlightMode _CommModuleMode;

        private float _Pitch;

        private float _Roll;

        private byte _SatCount;

        private byte[] _Retain = new byte[4];

        private CopterState _State;

        private string _StatusText;

        private ILocation _TakeOffPoint;

        private uint _TimebootMs;

        private float _Voltage;

        private float _Yaw;

        //初始时位置：使用地图Center
        private double _RecordLat;   

        private double _RecordLng;

        #endregion Backing Fields

        public CopterImplSharedPart(SynchronizationContext uiSyncContext) : base(uiSyncContext)
        {
            PropertyChanged += CopterImplSharedPart_PropertyChanged;
            Task.Run(async () =>
            {
                while (true)
                {
                    UpdateFlightTimeSpanIfNeeded();
                    await Task.Delay(_intervalToUpdateFlightTimeSpan).ConfigureAwait(false);
                }
            });
            
            Task.Run(async () =>
            {
                while (true)
                {
                    if (!_isMobileControlActive)
                    {
                        await EnsureChannelsSetAsync();
                    }
                    if (_lastUpdateStatusTextTime.AddSeconds(2) <= DateTime.Now)
                    {
                        StatusText = null;
                    }
                    await Task.Delay(500);  //50
                }
            });
            
            Task.Run(async () =>
            {
                var lastHeartbeatCount = HeartbeatCount;
                while (true)
                {
                    if (!IsConnected || lastHeartbeatCount == HeartbeatCount)
                    {
                        // 如果 !IsConnected（连接通道断开），肯定失联了；
                        // 或者，过了一段时间，心跳数没变化，认为飞行器失联。
                        IsAbsolutelyConnected = false;

                        // 此处等待的时间较短，目的是在飞行器重新连上时快速知晓。
                        await Task.Delay(500).ConfigureAwait(false);
                    }
                    else
                    {
                        // 过了一段时间，心跳数变化了，确认飞行器连接正常。
                        IsAbsolutelyConnected = true;

                        // 记录此时的心跳数。
                        lastHeartbeatCount = HeartbeatCount;

                        // 此处等待的时间较长，目的是提高失联判断的可信程度。
                        await Task.Delay(5000).ConfigureAwait(false);
                    }
                }
            });
        }

        public event EventHandler AltitudeChanged;

        public event EventHandler AttitudeChanged;

#if PRIVATE
        public event EventHandler<DataStreamReceivedEventArgs> DataStreamReceived;
#endif

        public event EventHandler<HeartbeatReceivedEventArgs> HeartbeatReceived;

        public event EventHandler LocationChanged;

        public event EventHandler<MissionItemReceivedEventArgs> MissionItemReceived;

        public event EventHandler<PairingCompletedEventArgs> PairingCompleted;

        public event EventHandler SensorDataReceived;

        public event EventHandler<SystemStatusReceivedEventArgs> SystemStatusReceived;

        public event EventHandler<MessageCreatedEventArgs> TextReceived;


        protected enum PDataStreamType
        {
            SYS_STATUS = 1, // MAVLINK_MSG_ID_SYS_STATUS,
            GPS_RAW_INT = 24,   // MAVLINK_MSG_ID_GPS_RAW_INT,
            RC_CHANNELS_RAW = 35    // MAVLINK_MSG_ID_RC_CHANNELS_RAW
        }

        public float AirSpeed
        {
            get { return _AirSpeed; }
            protected set { Set(nameof(AirSpeed), ref _AirSpeed, value); }
        }

        public float Altitude
        {
            get { return _Altitude; }
            protected set { Set(nameof(Altitude), ref _Altitude, value); }
        }

        public byte BatteryPer
        {
            get { return _BatteryPer; }
            protected set { Set(nameof(BatteryPer), ref _BatteryPer, value); }
        }

        public ushort Channel1
        {
            get { return _Channel1; }
            protected set { Set(nameof(Channel1), ref _Channel1, value); }
        }

        public ushort Channel2
        {
            get { return _Channel2; }
            protected set { Set(nameof(Channel2), ref _Channel2, value); }
        }

        public ushort Channel3
        {
            get { return _Channel3; }
            protected set { Set(nameof(Channel3), ref _Channel3, value); }
        }

        public ushort Channel4
        {
            get { return _Channel4; }
            protected set { Set(nameof(Channel4), ref _Channel4, value); }
        }

        public ushort Channel5
        {
            get { return _Channel5; }
            protected set { Set(nameof(Channel5), ref _Channel5, value); }
        }

        public ushort Channel6
        {
            get { return _Channel6; }
            protected set { Set(nameof(Channel6), ref _Channel6, value); }
        }

        public ushort Channel7
        {
            get { return _Channel7; }
            protected set { Set(nameof(Channel7), ref _Channel7, value); }
        }

        public ushort Channel8
        {
            get { return _Channel8; }
            protected set { Set(nameof(Channel8), ref _Channel8, value); }
        }

        public ushort? DesiredChannel1
        {
            get { return _DesiredChannel1; }
            set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel1), ref _DesiredChannel1, value); } }
        }

        public ushort? DesiredChannel2
        {
            get { return _DesiredChannel2; }
            set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel2), ref _DesiredChannel2, value); } }
        }

        public ushort? DesiredChannel3
        {
            get { return _DesiredChannel3; }
            set
            {
                // 林俊清, 20160317, 紧急悬停时可调节高度。
                //if (!IsEmergencyHoverActive)
                //{
                if (Set(nameof(DesiredChannel3), ref _DesiredChannel3, value))
                {
                    if (State == CopterState.Initialized) SetStateAccordingToMode(Mode);
                }
                //}
            }
        }

        public ushort? DesiredChannel4
        {
            get { return _DesiredChannel4; }
            set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel4), ref _DesiredChannel4, value); } }
        }

        public ushort? DesiredChannel5
        {
            get { return _DesiredChannel5; }
            set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel5), ref _DesiredChannel5, value); } }
        }

        public ushort? DesiredChannel6
        {
            get { return _DesiredChannel6; }
            set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel6), ref _DesiredChannel6, value); } }
        }

        public ushort? DesiredChannel7
        {
            get { return _DesiredChannel7; }
            set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel7), ref _DesiredChannel7, value); } }
        }

        public ushort? DesiredChannel8
        {
            get { return _DesiredChannel8; }
            set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel8), ref _DesiredChannel8, value); } }
        }

        public float? DesiredYaw
        {
            get { return _DesiredYaw; }
            set { if (!IsEmergencyHoverActive && !(State == CopterState.Following && _followKeepFacingTarget)) { Set(nameof(DesiredYaw), ref _DesiredYaw, value); } }
        }

        public float Elevation
        {
            get { return _Elevation; }
            protected set { Set(nameof(Elevation), ref _Elevation, value); }
        }

        /// <summary>
        /// 获取固件版本。
        /// </summary>
        public int? FirmwareVersion
        {
            get { return _FirmwareVersion; }
            protected set
            {
                if (Set(nameof(FirmwareVersion), ref _FirmwareVersion, value))
                {
                    if (value == null)
                    {
                        FirmwareVersionText = null;
                    }
                    else
                    {
                        int major, minor, build;
                        int temp;
#if NETFX_CORE
                        major = value.Value / 0x1000;
                        temp = value.Value % 0x1000;
                        minor = temp / 0x100;
                        build = temp % 0x100;
#else
                        major = Math.DivRem(value.Value, 0x1000, out temp);
                        minor = Math.DivRem(temp, 0x100, out build);
#endif
                        FirmwareVersionText = $"{major}.{minor}.{build}";
                    }
                }
            }
        }

        /// <summary>
        /// 获取固件版本的文本形式。
        /// </summary>
        public string FirmwareVersionText
        {
            get { return _FirmwareVersionText; }
            protected set { Set(nameof(FirmwareVersionText), ref _FirmwareVersionText, value); }
        }

        public double FlightDistance
        {
            get { return _FlightDistance; }
            protected set { Set(nameof(FlightDistance), ref _FlightDistance, value); }
        }

        public double FlightDistance2D
        {
            get { return _FlightDistance2D; }
            protected set { Set(nameof(FlightDistance2D), ref _FlightDistance2D, value); }
        }

        public TimeSpan FlightTimeSpan
        {
            get { return _FlightTime; }
            protected set { Set(nameof(FlightTimeSpan), ref _FlightTime, value); }
        }

        /// <summary>
        /// 获取 GPS 锁定状态。
        /// </summary>
        public GpsFixType GpsFixType
        {
            get { return _GpsFixType; }
            protected set { Set(nameof(GpsFixType), ref _GpsFixType, value); }
        }

        /// <summary>
        /// 获取 GPS HDOP（horizontal dilution of position），单位为米。
        /// </summary>
        public float GpsHdop
        {
            get { return _GpsHdop; }
            protected set { Set(nameof(GpsHdop), ref _GpsHdop, value); }
        }

        public float GroundSpeed
        {
            get { return _GroundSpeed; }
            protected set { Set(nameof(GroundSpeed), ref _GroundSpeed, value); }
        }

        public bool HasSwitchedToGpsMode
        {
            get { return _HasSwitchedToGpsMode; }
            protected set { Set(nameof(HasSwitchedToGpsMode), ref _HasSwitchedToGpsMode, value); }
        }

        public short Heading
        {
            get { return _Heading; }
            protected set { Set(nameof(Heading), ref _Heading, value); }
        }

        public ulong HeartbeatCount
        {
            get { return _HeartbeatCount; }
            protected set { Set(nameof(HeartbeatCount), ref _HeartbeatCount, value); }
        }

        /// <summary>
        /// 获取一个值，指示是否确定飞行器与地面站正常连接。在实现中，一般根据对心跳包的接收情况判断。
        /// </summary>
        public bool IsAbsolutelyConnected
        {
            get { return _IsAbsolutelyConnected; }
            protected set { Set(nameof(IsAbsolutelyConnected), ref _IsAbsolutelyConnected, value); }
        }

        /// <summary>
        /// 获取或设置一个值，指示是否正在检查飞行器与地面站的连接以确定通信正常。在实现中，一般在建立连接后设为 true，在收到飞行器发回的数据后设为 false。
        /// </summary>
        public bool IsCheckingConnection
        {
            get { return _IsCheckingConnection; }
            set { Set(nameof(IsCheckingConnection), ref _IsCheckingConnection, value); }
        }

        public bool IsConnected
        {
            get { return _IsConnected; }
            protected set { Set(nameof(IsConnected), ref _IsConnected, value); }
        }

        public bool CommModuleConnected
        {
            get { return _CommModuleConnected; }
            set { Set(nameof(CommModuleConnected), ref _CommModuleConnected, value); }
        }

        public bool IsEmergencyHoverActive
        {
            get { return _IsEmergencyHoverActive; }
            protected set { Set(nameof(IsEmergencyHoverActive), ref _IsEmergencyHoverActive, value); }
        }

        /// <summary>
        /// 获取一个值，指示当前的 GPS 定位是否足够精确。
        /// </summary>
        public bool IsGpsAccurate
        {
            get { return _IsGpsAccurate; }
            protected set { Set(nameof(IsGpsAccurate), ref _IsGpsAccurate, value); }
        }

        public bool IsPairing
        {
            get { return _IsPairing; }
            protected set { Set(nameof(IsPairing), ref _IsPairing, value); }
        }

        public bool IsUnlocked
        {
            get { return _IsUnlocked; }
            protected set
            {
                if (Set(nameof(IsUnlocked), ref _IsUnlocked, value))
                {
                    if (value)
                    {
                        State = CopterState.Initialized;
                    }
                    else
                    {
                        State = CopterState.Locked;
                    }
                }
            }
        }

        public double Latitude
        {
            get { return _Latitude; }
            protected set { Set(nameof(Latitude), ref _Latitude, value); }
        }

        public double Longitude
        {
            get { return _Longitude; }
            protected set { Set(nameof(Longitude), ref _Longitude, value); }
        }

        public double RecordLat
        {
            get { return _RecordLat; }
            set { Set(nameof(RecordLat), ref _RecordLat, value); }
        }

        public double RecordLng
        {
            get { return _RecordLng; }
            set { Set(nameof(RecordLng), ref _RecordLng, value); }
        }

        /// <summary>
        /// 获取任务总数。
        /// </summary>
        public ushort? MissionCount { get; protected set; }

        public string Name { get; set; }


        public float Pitch
        {
            get { return _Pitch; }
            protected set { Set(nameof(Pitch), ref _Pitch, value); }
        }

        public float Roll
        {
            get { return _Roll; }
            protected set { Set(nameof(Roll), ref _Roll, value); }
        }

        public byte SatCount
        {
            get { return _SatCount; }
            protected set { Set(nameof(SatCount), ref _SatCount, value); }
        }

        public byte[] Retain
        {
            get { return _Retain; }
            protected set { Set(nameof(Retain), ref _Retain, value); }
        }

        public CopterState State
        {
            get { return _State; }
            protected set { Set(nameof(State), ref _State, _forcedState ?? value); }
        }

        public string StatusText
        {
            get { return _StatusText; }
            protected set
            {
                if (Set(nameof(StatusText), ref _StatusText, value))
                {
                    _lastUpdateStatusTextTime = DateTime.Now;
                }
            }
        }

        public ILocation TakeOffPoint
        {
            get { return _TakeOffPoint; }
            protected set { Set(nameof(TakeOffPoint), ref _TakeOffPoint, value); }
        }

        public float Voltage
        {
            get { return _Voltage; }
            protected set { Set(nameof(Voltage), ref _Voltage, value); }
        }

        public float Yaw
        {
            get { return _Yaw; }
            protected set { Set(nameof(Yaw), ref _Yaw, value); }
        }

        public FlightMode CommModuleMode
        {
            get { return _CommModuleMode; }
            protected set { Set(nameof(CommModuleMode), ref _CommModuleMode, value); }
        }

        private byte _CommModuleVersion;
        public byte CommModuleVersion
        {
            get { return _CommModuleVersion; }
            protected set { Set(nameof(CommModuleVersion), ref _CommModuleVersion, value); }
        }

        private string _LEDColor;
        public string LEDColor
        {
            get { return _LEDColor; }
            set { Set(nameof(LEDColor), ref _LEDColor, value); }
        }

#if PRIVATE
        public
#else
        internal
#endif
            FlightMode Mode
        {
            get { return _Mode; }
            set
            {
                // 林俊清, 20160325, Mode 属性改成 internal 了，如果仍然用 Set 或者 RaisePropertyChanged 方法，会抛异常（"Property not found"）。现决定不引发 PropertyChanged 事件了。
                var changed = _Mode != value;
                if (changed) _Mode = value;

                // 如果在已解锁状态下满足某些条件，就根据模式设置 State。
                if (IsUnlocked)
                {
                    var shouldSetState = false;
                    if (changed)
                    {
                        if (State == CopterState.Following)
                        {
                            if (value != FlightMode.GUIDED)
                            {
                                // 跟随时切换到其他模式，停止跟随并且设置 State。
                                _shouldFollow = false;
                                shouldSetState = true;
                            }
                        }
                        else
                        {
                            // 非跟随时模式变化了，设置 State。
                            shouldSetState = true;
                        }
                    }
                    if (shouldSetState) SetStateAccordingToMode(value, defaultState: CopterState.Initialized);
                }
            }
        }

        public async Task FloatAsync()
        {
            if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
            {
                await SetChannelsAsync(
                    ch1: 1500,
                    ch2: 1500,
                    ch3: 1500,
                    ch4: 1500
                ).ConfigureAwait(false);
            }
        }

        /// <summary>
        /// 使飞行器飞往水平面上指定的点。
        /// </summary>
        /// <param name="lat">纬度。</param>
        /// <param name="lng">经度。</param>
        /// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
        public Task FlyToAsync(double lat, double lng)
        {
            return FlyToAsync(lat, lng, Altitude);
        }

        public Task FlyToAsync(double lat, double lng, float alt)
        {
            _shouldFollow = false;
            //State = CopterState.CommandMode;
            return FlyToCoreAsync(lat, lng, alt);
        }

        public void Follow(IVisibleStatus target, bool keepYawDifference = false, bool keepFacingTarget = true, bool keep3DRelativeLocations = false)
        {
            // 林俊清, 20160409, 在目前的实现中，跟随状态使用 GUIDED 模式。

            if (!IsConnected || !IsUnlocked || this == target || IsEmergencyHoverActive) return;

            _followTarget = target;
            _followAltDifference = Altitude - target.Altitude;
            _followDistance = (float)target.CalcDistance2D(this);
            _followKeepYawDifference = keepYawDifference;
            _followKeepFacingTarget = keepFacingTarget;
            _followKeep3DRelativeLocations = keep3DRelativeLocations;
            _followSelfDirectionFromTarget = (float)target.CalcDirection2D(this).RadToDeg();
            _followTargetOriginalYaw = target.Yaw.NormalizeDirection();

            if (State == CopterState.Following) return;

            Task.Run(async () =>
            {
                State = CopterState.Following;
                _shouldFollow = true;
                while (IsConnected && _shouldFollow)
                {
                    // 计算飞行器应当处于什么位置，并调用 FlyTo 使其飞往彼处。

                    var destination2D = _followTarget.CalcLatLngSomeMetersAway2D(_followKeepYawDifference ? (_followSelfDirectionFromTarget - _followTargetOriginalYaw + _followTarget.Yaw.NormalizeDirection()).NormalizeDirection() : _followSelfDirectionFromTarget, _followDistance);
                    var destinationAlt = _followKeep3DRelativeLocations ? _followTarget.Altitude + _followAltDifference : Altitude;
                    var destination = new PLLocation(destination2D.Latitude, destination2D.Longitude, destinationAlt);
                    if (_followLastDestination == null || _followLastDestination.CalcDistance(destination) >= 1.5)
                    {
                        await FlyToCoreAsync(destination2D.Latitude, destination2D.Longitude, destinationAlt).ConfigureAwait(false);
                        _followLastDestination = destination;
                    }

                    // 如果需要保持面对目标，计算并使用偏航和云台俯仰的期望值。

                    if (_followKeepFacingTarget)
                    {
                        var yaw = (float)this.CalcDirection2D(_followTarget).RadToDeg();
                        SetFieldAndRaisePropertyChanged(ref _DesiredYaw, yaw, nameof(DesiredYaw));

                        var distance2DFromTargetToCopter = _followTarget.CalcDistance2D(this);
                        var altDifferenceFromTargetToCopter = Altitude - _followTarget.Altitude;
                        // 正前方值为 0，向下取正值，向上取负值。
                        var gimbalPitchRad = Math.Atan2(altDifferenceFromTargetToCopter, distance2DFromTargetToCopter);
                        // 正前方值为 1500，向下 80° 值为 1900。
                        var ch7 = (ushort)((1900 - 1500) * gimbalPitchRad / 80F.DegToRad() + 1500);
                        SetFieldAndRaisePropertyChanged(ref _DesiredChannel7, ch7, nameof(DesiredChannel7));
                    }

                    await Task.Delay(50).ConfigureAwait(false);
                }
                if (!IsConnected) _shouldFollow = false;
            });
        }

        public async Task HoverAsync()
        {
            if (await SetModeAsync(FlightMode.LOITER).ConfigureAwait(false))
            {
                await SetChannelsAsync(
                    ch1: 1500,
                    ch2: 1500,
                    ch3: 1500,
                    ch4: 1500
                ).ConfigureAwait(false);
            }
        }
        public async Task GuidAsync()
        {
            if (await SetModeAsync(FlightMode.GUIDED).ConfigureAwait(false))
            {
                await SetChannelsAsync(
                    ch1: 1500,
                    ch2: 1500,
                    ch3: 1500,
                    ch4: 1500
                ).ConfigureAwait(false);
            }
        }

        public async Task LandAsync()
        {
            await SetModeAsync(FlightMode.LAND).ConfigureAwait(false);
            await SetChannelsAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false);
        }

        public async Task ReturnToLaunchAsync()
        {
            await SetModeAsync(FlightMode.RTL).ConfigureAwait(false);
            await SetChannelsAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false);
        }

        public async Task SetChannelsAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null)
        {
            SetTargets(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8);

            await SetChannelsAsync().ConfigureAwait(false);
        }

        public abstract Task SetChannelsAsync();

        public async Task SetMobileControlAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null) // 林俊清, 20150912, 将来如有需要再补上 TargetAlt, float? alt = null)
        {
            SetTargets(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, yaw);

            await SetMobileControlAsync().ConfigureAwait(false);
        }

        public abstract Task SetMobileControlAsync();

        public async Task StartEmergencyHoverAsync()
        {
            _shouldFollow = false;
            State = CopterState.HoverMode;
            IsEmergencyHoverActive = true;

            // 林俊清, 20151028, 先 IsEmergencyHoverActive = true 再设置目标通道是为了防止别的线程正好在 IsEmergencyHoverActive == false 时设置通道。
            // 林俊清, 20151019, 在紧急悬停状态，外部设置 DesiredChannel[1-4] 无效，只能如此设置。
            SetFieldAndRaisePropertyChanged(ref _DesiredChannel1, 1500, nameof(DesiredChannel1));
            SetFieldAndRaisePropertyChanged(ref _DesiredChannel2, 1500, nameof(DesiredChannel2));
            SetFieldAndRaisePropertyChanged(ref _DesiredChannel3, 1500, nameof(DesiredChannel3));
            SetFieldAndRaisePropertyChanged(ref _DesiredChannel4, 1500, nameof(DesiredChannel4));

            await SetMobileControlAsync().ConfigureAwait(false);

            switch (Mode)
            {
                // 林俊清, 20151019, 波子说一律切 LOITER。
                //case FlightMode.ALT_HOLD:
                //    break;
                //case FlightMode.LOITER:
                //    break;
                //case FlightMode.STABILIZE:
                //    await SetModeCoreAsync(FlightMode.ALT_HOLD).ConfigureAwait(false);
                //    break;
                //case FlightMode.AUTO:
                //case FlightMode.GUIDED:
                //case FlightMode.RTL:
                //case FlightMode.CIRCLE:
                //case FlightMode.LAND:
                default:
                    await SetModeCoreAsync(FlightMode.LOITER).ConfigureAwait(false);
                    break;
            }
        }

        public async void StartMobileControl(int? millisecondsInterval = null)
        {
            if (millisecondsInterval != null) _mobileControlIntervalMilli = millisecondsInterval.Value;
            if (!_isMobileControlActive)
            {
                _isMobileControlActive = true;
                while (_isMobileControlActive)
                {
                    if (State == CopterState.Following)
                    {
                        await SetMobileControlAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false);
                    }
                    else {
                        switch (Mode)
                        {
                            case FlightMode.STABILIZE:
                            case FlightMode.ALT_HOLD:
                            case FlightMode.LOITER:
                                await SetMobileControlAsync().ConfigureAwait(false);
                                break;

                            case FlightMode.GUIDED:
                            case FlightMode.RTL:
                                await SetMobileControlAsync(ch1: 1500, ch2: 1500, ch3: 1500).ConfigureAwait(false);
                                break;

                            case FlightMode.LAND:
                                await SetMobileControlAsync(ch3: 1500);
                                break;

                            default:
                                await SetMobileControlAsync(ch1: 1500, ch2: 1500, yaw: Yaw).ConfigureAwait(false);
                                break;
                        }
                    }
                    await Task.Delay(_mobileControlIntervalMilli).ConfigureAwait(false);
                }
            }
        }

        public void StopEmergencyHover()
        {
            IsEmergencyHoverActive = false;
        }

        public void StopMobileControl()
        {
            _isMobileControlActive = false;
        }

        public async Task TakeOffAsync()
        {


          /*  await SetChannelsAsync(
                ch1: 1500,
                ch2: 1500,
                ch3: 1500,
                ch4: 1500
            ).ConfigureAwait(false);
            _forcedState = CopterState.TakingOff;
            State = CopterState.TakingOff;
            await DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt).ConfigureAwait(false);
            var temp = Task.Run(async () =>
            {
                var startTime = DateTime.Now;
                var secondsTimeout = alt / 0.7;
                while (currentTakeOffCount == _takeOffCount)
                {
                    if (alt - Altitude < 0.2 || startTime.AddSeconds(secondsTimeout) <= DateTime.Now)
                    {
                        _forcedState = null;
                        await HoverAsync().ConfigureAwait(false);
                        break;
                    }
                    await Task.Delay(50).ConfigureAwait(false);
                }
            });
*/








            if (await SetModeAsync(FlightMode.AUTO).ConfigureAwait(false))
            {
                await SetChannelsAsync(
                    ch1: 1500,
                    ch2: 1500,
                    ch3: 1500,
                    ch4: 1500
                ).ConfigureAwait(false);
            }
           
        }

        /// <summary>
        /// 切换飞行模式，返回的 <see cref="Task"/> 实例在确认切换成功或超时后完成。
        /// </summary>
        /// <param name="mode">飞行模式。</param>
        /// <param name="millisecondsTimeout">超时时间，单位为毫秒。</param>
        /// <returns>表示切换模式操作的 <see cref="Task{TResult}"/> 实例。若切换模式成功，异步操作的结果为 true；否则为 false。</returns>
#if PRIVATE
        public
#else
        internal
#endif
            async Task<bool> SetModeAsync(FlightMode mode, int millisecondsTimeout = 5000)
        {
            if (_shouldFollow && mode != FlightMode.GUIDED) _shouldFollow = false;
            // 林俊清, 20160317, 紧急悬停时可返航或降落。
            if (mode == FlightMode.RTL || mode == FlightMode.LAND)
            {
                StopEmergencyHover();
            }
            if (!IsEmergencyHoverActive)
            {
                return await SetModeCoreAsync(mode, millisecondsTimeout).ConfigureAwait(false);
            }
            return false;
        }

        /// <summary>
        /// 无论在不在紧急悬停状态下都可用的切换模式方法。
        /// </summary>
        /// <param name="mode">模式。</param>
        /// <param name="millisecondsTimeout">超时。</param>
        /// <returns>成功与否。</returns>
        internal abstract Task<bool> SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000);

        protected async Task EnsureChannelsSetAsync()
        {
            if (
                (DesiredChannel1 != null && Channel1 != DesiredChannel1) ||
                (DesiredChannel2 != null && Channel2 != DesiredChannel2) ||
                (DesiredChannel3 != null && Channel3 != DesiredChannel3) ||
                (DesiredChannel4 != null && Channel4 != DesiredChannel4) ||
                (DesiredChannel5 != null && Channel5 != DesiredChannel5) ||
                (DesiredChannel6 != null && Channel6 != DesiredChannel6) ||
                (DesiredChannel7 != null && Channel7 != DesiredChannel7) ||
                (DesiredChannel8 != null && Channel8 != DesiredChannel8)
            )   // Any channel not set.
            {
                await SetChannelsAsync().ConfigureAwait(false);
            }
        }

        /// <summary>
        /// 不主动设置 State，仅发送飞往某点命令。
        /// </summary>
        /// <param name="lat">纬度。</param>
        /// <param name="lng">经度。</param>
        /// <param name="alt">高度。</param>
        /// <returns>表示此异步发送操作的 <see cref="Task{TResult}"/> 实例。</returns>
        protected abstract Task FlyToCoreAsync(double lat, double lng, float alt);

        protected void RaiseAltitudeChanged() => AltitudeChanged?.Invoke(this, EventArgs.Empty);

        protected void RaiseAltitudeChangedIfNeeded()
        {
            if (_lastChangedAlt == null || Math.Abs(Altitude - _lastChangedAlt.Value) >= 0.5 || DateTime.Now.AddMilliseconds(-500) >= _lastRaiseAltitudeChangedTime)
            {
                RaiseAltitudeChanged();
                _lastChangedAlt = Altitude;
                _lastRaiseAltitudeChangedTime = DateTime.Now;
            }
        }

        protected void RaiseAttitudeChanged() => AttitudeChanged?.Invoke(this, EventArgs.Empty);

        protected void RaiseDataStreamReceived(PDataStreamType dataStreamType)
        {
#if PRIVATE
            DataStreamReceived?.Invoke(this, new DataStreamReceivedEventArgs((DataStreamType)dataStreamType));
#endif
        }

        protected void RaiseHeartbeatReceived(HeartbeatReceivedEventArgs e)
        {
            HeartbeatReceived?.Invoke(this, e);
        }

        protected void RaiseLocationChanged() => LocationChanged?.Invoke(this, EventArgs.Empty);

        protected void RaiseLocationChangedIfNeeded()
        {
            if (_lastChangedLocation == null || this.CalcDistance(_lastChangedLocation) >= 0.5 || DateTime.Now.AddMilliseconds(-500) >= _lastRaiseLocationChangedTime)
            {
                RaiseLocationChanged();
                _lastChangedLocation = new PLLocation(this);
                _lastRaiseLocationChangedTime = DateTime.Now;
            }
        }

        protected void RaiseMissionItemReceived(MissionItemReceivedEventArgs e)
        {
            MissionItemReceived?.Invoke(this, e);
        }

        protected void RaisePairingCompleted(PairingCompletedEventArgs e)
        {
            var handler = PairingCompleted;
            if (handler == null) return;
            if (SynchronizationContext.Current == _uiSyncContext)
            {
                handler(this, e);
            }
            else
            {
                _uiSyncContext.Post(() => handler(this, e));
            }
        }

        protected void RaiseSensorDataReceived(EventArgs e)
        {
            SensorDataReceived?.Invoke(this, e);
        }

        protected void RaiseSysStatusReceived(SystemStatusReceivedEventArgs e)
        {
            SystemStatusReceived?.Invoke(this, e);
        }


        protected void RaiseTextReceived(MessageCreatedEventArgs e)
        {
            TextReceived?.Invoke(this, e);
        }



        protected void SetTargets(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null)
        {
            if (ch1 != null) DesiredChannel1 = ch1;
            if (ch2 != null) DesiredChannel2 = ch2;
            if (ch3 != null) DesiredChannel3 = ch3;
            if (ch4 != null) DesiredChannel4 = ch4;
            if (ch5 != null) DesiredChannel5 = ch5;
            if (ch6 != null) DesiredChannel6 = ch6;
            if (ch7 != null) DesiredChannel7 = ch7;
            if (ch8 != null) DesiredChannel8 = ch8;
            if (yaw != null) DesiredYaw = yaw;
        }

        protected virtual void UpdateFlightDataIfNeeded()
        {
            if (!TakeOffPoint.IsNullOrEmpty())
            {
                FlightDistance = TakeOffPoint.CalcDistance(this);
            }
        }

        protected void UpdateFlightTimeSpanIfNeeded()
        {
            if (_takeOffTime != null && IsUnlocked)
            {
                var flightTimeSpan = DateTime.Now - _takeOffTime.Value;
                if (flightTimeSpan - FlightTimeSpan >= _intervalToUpdateFlightTimeSpan)
                {
                    FlightTimeSpan = flightTimeSpan;
                }
            }
        }

        private void CopterImplSharedPart_PropertyChanged(object sender, PropertyChangedEventArgs e)
        {
            switch (e.PropertyName)
            {
                case nameof(IsConnected):
                    if (!IsConnected)
                    {
                        IsCheckingConnection = false;
                        IsAbsolutelyConnected = false;
                    }
                    break;

                case nameof(IsUnlocked):
                    if (IsUnlocked)
                    {
                        // 林俊清, 20151029, 在解锁时重置起飞点、起飞时间、飞行距离、飞行时间、速度。
                        TakeOffPoint = new PLLocation { Latitude = Latitude, Longitude = Longitude, Altitude = Altitude };
                        _takeOffTime = DateTime.Now;
                        FlightDistance = 0;
                        FlightDistance2D = 0;
                        FlightTimeSpan = TimeSpan.FromSeconds(0);
                        AirSpeed = 0;
                        GroundSpeed = 0;
                    }
                    else
                    {
                        if (IsEmergencyHoverActive) StopEmergencyHover();
                    }
                    break;

                default:
                    break;
            }
        }

        private void SetFieldAndRaisePropertyChanged(ref ushort? field, ushort value, string propertyName)
        {
            if (field != value)
            {
                field = value;
                RaisePropertyChanged(propertyName);
            }
        }

        private void SetFieldAndRaisePropertyChanged(ref float? field, float value, string propertyName)
        {
            if (field != value)
            {
                field = value;
                RaisePropertyChanged(propertyName);
            }
        }

        private void SetStateAccordingToMode(FlightMode value, CopterState? defaultState = null)
        {
            switch (value)
            {
                case FlightMode.ALT_HOLD:
                    State = CopterState.FloatMode;
                    break;

                case FlightMode.AUTO:
                    State = CopterState.TakingOff;
                    break;

                case FlightMode.CIRCLE:
                    State = CopterState.Circling;
                    break;

                case FlightMode.GUIDED:
                    State = CopterState.CommandMode;
                    break;

                case FlightMode.LOITER:
                    State = CopterState.HoverMode;
                    break;

                case FlightMode.RTL:
                    State = CopterState.Returning;
                    break;

                case FlightMode.LAND:
                    State = CopterState.Landing;
                    break;

                default:
                    if (defaultState != null) State = defaultState.Value;
                    break;
            }
        }
    }
}