Plane.Sdk3/PlaneGcsSdk_Shared/Copters/PLCopter.cs

using Plane.Communication;
using Plane.Protocols;
using System;
using System.Diagnostics;
using System.Threading;
using System.Threading.Tasks;
using TaskTools.Utilities;

namespace Plane.Copters
{
    [DebuggerDisplay("Name={Name}")]
    public partial class PLCopter : CopterImplSharedPart, ICopter
    {
        private double _FlightDistance2D;

        private PlaneCopter _internalCopter;

        private int _setModeCount = 0;

        private int _takeOffCount;
        private bool _showLed=false;

#if PRIVATE
        public PLCopter(IConnection connection, SynchronizationContext uiSyncContext, bool autoSendHeartbeat = true, bool autoRequestData = true) : base(uiSyncContext)
        {
            _internalCopter = new PlaneCopter(connection, uiSyncContext, autoSendHeartbeat, autoRequestData);
#else
        public PLCopter(IConnection connection, SynchronizationContext uiSyncContext) : base(uiSyncContext)
        {
            _internalCopter = new PlaneCopter(connection, uiSyncContext);
#endif
            RegisterInternalCopterEventHandlers();
        }

        public IConnection Connection
        {
            get { return _internalCopter.Connection; }
            set { _internalCopter.Connection = value; }
        }

        /// <summary>
        /// 飞控发回的在二维平面上的飞行距离（隐藏了基类计算的同名属性）。
        /// </summary>
        public new double FlightDistance2D
        {
            get { return _FlightDistance2D; }
            protected set { Set(nameof(FlightDistance2D), ref _FlightDistance2D, value); }
        }

        public string Id { get { return _internalCopter.Id; } set { _internalCopter.Id = value; } }

        public virtual async Task ConnectAsync()
        {
            await _internalCopter.ConnectAsync().ConfigureAwait(false);
            IsConnected = _internalCopter.IsConnected;
            IsCheckingConnection = true;
            //连接完成后做一些初始化的工作
            await InitAsync();
        }

        public virtual async Task DisconnectAsync()
        {
            if (_isMobileControlActive) StopMobileControl();
            await _internalCopter.DisconnectAsync();
            IsConnected = _internalCopter.IsConnected;
            IsCheckingConnection = false;
        }

        public async Task GetCopterDataAsync()
        {
            await _internalCopter.GetCopterDataAsync();
        }

        public async Task<float> GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite)
        {
            return await _internalCopter.GetParamAsync(paramName, millisecondsTimeout).ConfigureAwait(false);
        }

        /// <summary>
        /// 锁定飞行器。在调用之前务必判断高度及让使用者确认！
        /// </summary>
        public async Task LockAsync()
        {
            if (IsUnlocked)
            {
                await _internalCopter.DoARMAsync(false).ConfigureAwait(false);
            }
        }

        public override async Task SetChannelsAsync()
        {
            await _internalCopter.SetChannelsAsync(
                DesiredChannel1,
                DesiredChannel2,
                DesiredChannel3,
                DesiredChannel4,
                DesiredChannel5,
                DesiredChannel6,
                DesiredChannel7,
                DesiredChannel8
            ).ConfigureAwait(false);
        }

        public override async Task SetMobileControlAsync()
        {
            await _internalCopter.SetMobileControlAsync(
                DesiredChannel1,
                DesiredChannel2,
                DesiredChannel3,
                DesiredChannel4,
                DesiredChannel5,
                DesiredChannel6,
                DesiredChannel7,
                DesiredChannel8,
                DesiredYaw
            ).ConfigureAwait(false);
        }






        public bool GetShowLEDAsync()
        {
            return _showLed;

            /*
            int showled;
             showled=(int)await _internalCopter.GetParamAsync("NTF_SHOWLED", 0).ConfigureAwait(false);
            if (showled == 0)
                return false;
            else return true;
            */

        }

        public async Task InjectGpsDataAsync(byte[] data, ushort length)
        {
            await _internalCopter.InjectGpsDataAsync(data, length).ConfigureAwait(false);
        }



        public async Task SetShowLEDAsync(bool Ledon)
        {
            if (Ledon)
                await SetParamNoCheckAsync("NTF_SHOWLED", 1);
            else
                await SetParamNoCheckAsync("NTF_SHOWLED", 0);
            _showLed = Ledon;
         }




        public async Task SetShowRGBLEDAsync(int RedValue,int GreenValue,int BlueValue)
        {
            float rgbvalue = (RedValue & 255) << 16 + (GreenValue & 255) << 8 + (BlueValue & 255);
            await SetParamNoCheckAsync("NTF_SHOWRGBLED", rgbvalue);
        }



        /// <summary>
        /// 闪烁SHOWLED，如果当前没亮，那就亮一下，如果本来就是亮的，那就暗一下
        /// </summary>
        /// <param name="Flashtype">闪烁类型，目前没用</param>
        /// <param name="millisecondsTime">闪烁时间ms</param>
        /// <returns></returns>
        public async Task SetShowLEDFlashAsync(int Flashtype,int millisecondsTime)
        {
            bool Ledison;
            Ledison= GetShowLEDAsync();
            if (!Ledison)
            {
                await SetShowLEDAsync(true);
                await Task.Delay(millisecondsTime).ConfigureAwait(false);
                await SetShowLEDAsync(false);
                await SetShowLEDAsync(false);
            }else
            {
                await SetShowLEDAsync(false);
                await Task.Delay(millisecondsTime).ConfigureAwait(false);
                await SetShowLEDAsync(true);
                await SetShowLEDAsync(true);
            }


        }

        public async Task SetParamNoCheckAsync(string paramName, float paramValue)
        {
                for (int ii = 0; ii < 5; ii++)
                {
                    await _internalCopter.SetParam2Async(paramName, paramValue).ConfigureAwait(false);
                    await Task.Delay(5).ConfigureAwait(false);
                }
        }
        public async Task SetParamAsync(string paramName, float paramValue, int millisecondsTimeout = Timeout.Infinite)
        {
            var stopwatch = Stopwatch.StartNew();
            while (true)
            {
                for (int ii = 0; ii < 5; ii++)
                {
                    await _internalCopter.SetParam2Async(paramName, paramValue).ConfigureAwait(false);
                    await Task.Delay(5).ConfigureAwait(false);
                }
                
                int i = 0;
                try
                {
                    //for (; i < 5 && await _internalCopter.GetParamAsync(paramName, millisecondsTimeout-4000) != paramValue; i++)
                    //{
                    //    await Task.Delay(5).ConfigureAwait(false);
                    //}
                    
                    if (await _internalCopter.GetParamAsync(paramName, 1000) == paramValue)
                    {
                        i = 1;
                    }
                }
                catch (TimeoutException)
                {
                    stopwatch.Stop();
                    throw;
                }
                if (i == 1)
                {
                    stopwatch.Stop();
                    break;
                }
                else if (millisecondsTimeout != Timeout.Infinite && stopwatch.ElapsedMilliseconds >= millisecondsTimeout)
                {
                    stopwatch.Stop();
                    throw new TimeoutException("The SetParamAsync operation has timed out.");
                }
            }
        }

        public async Task StartPairingAsync()
        {
            if (IsPairing) return;
            IsPairing = true;
            var packet = new MAVLink.mavlink_set_pair_t
            {
                pair = 1
            };
            await _internalCopter.GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_PAIR, packet).ConfigureAwait(false);
        }

        public async Task InitAsync()
        {
            float Gpstype=  await _internalCopter.GetParamAsync("GPS_TYPE") ;
            if (Gpstype == 15)
                LocationType = CopterLocationType.RTK;
            else
                LocationType = CopterLocationType.GPS;
        }



        public async Task StopPairingAsync()
        {
            if (!IsPairing) return;
            IsPairing = false;
            var packet = new MAVLink.mavlink_set_pair_t
            {
                pair = 0
            };
            await _internalCopter.GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_PAIR, packet).ConfigureAwait(false);
        }

        public async Task TakeOffAsync(float alt)
        {
            var currentTakeOffCount = ++_takeOffCount;
            // 林俊清, 20160312, 从固件版本 3.1.1（0x3101）开始支持直接解锁起飞命令。
            if (FirmwareVersion >= 0x3101)
            {
                await SetChannelsAsync(
                    ch1: 1500,
                    ch2: 1500,
                    ch3: 1500,
                    ch4: 1500
                ).ConfigureAwait(false);
                _forcedState = CopterState.TakingOff;
                State = CopterState.TakingOff;
                await _internalCopter.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);
                    }
                });
            }
            else
            {
                await UnlockAsync().ConfigureAwait(false);
                // 循环检查是否已解锁，若已解锁则起飞。最多检查 4 次，每次检查之前等待 500 毫秒。
                for (int i = 0; currentTakeOffCount == _takeOffCount && i < 4; i++)
                {
                    await Task.Delay(500).ConfigureAwait(false);
                    if (IsUnlocked)
                    {
                        await TakeOffAsync().ConfigureAwait(false);
                        return;
                    }
                }
            }
        }

        public async Task UnlockAsync()
        {
            if (!IsUnlocked)
            {
                await SetChannelsAsync(
                    ch1: 1500,
                    ch2: 1500,
                    ch3: 1100,
                    ch4: 1500
                ).ConfigureAwait(false);
                if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
                {
                    await _internalCopter.DoARMAsync(true).ConfigureAwait(false);
                }
            }
        }

        /// <summary>
        /// 无论在不在紧急悬停状态下都可用的切换模式方法。
        /// </summary>
        /// <param name="mode">模式。</param>
        /// <param name="millisecondsTimeout">超时。</param>
        /// <returns>成功与否。</returns>
        internal override async Task<bool> SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000)
        {
            var currentSetModeCount = ++_setModeCount;
            Stopwatch stopwatch = null;
            bool noTimeout = millisecondsTimeout == Timeout.Infinite;
            if (!noTimeout)
            {
                stopwatch = Stopwatch.StartNew();
            }
            await _internalCopter.SetModeAsync(mode.ToAC2Mode()).ConfigureAwait(false);
            int i = 1;
            bool anotherSetModeActionCalled = false;
            while ((noTimeout || stopwatch.ElapsedMilliseconds < millisecondsTimeout)
                && Mode != mode)
            {
                if (currentSetModeCount != _setModeCount)
                {
                    anotherSetModeActionCalled = true;
                    break;
                }
                if (i % 8 == 0) await _internalCopter.SetModeAsync(mode.ToAC2Mode()).ConfigureAwait(false);
                await Task.Delay(30).ConfigureAwait(false);
                i++;
            }
            if (stopwatch != null && stopwatch.IsRunning)
            {
                stopwatch.Stop();
            }
            return !anotherSetModeActionCalled && Mode == mode;
        }

        protected override async Task FlyToCoreAsync(double lat, double lng, float alt)
        {
            if (!IsEmergencyHoverActive)
            {
                await _internalCopter.SetGuidedModeWPAsync(new Locationwp
                {
                    alt = alt,
                    id = (byte)MAVLink.MAV_CMD.WAYPOINT,
                    lat = lat,
                    lng = lng
                }).ConfigureAwait(false);
            }
        }
    }
}