Plane.Sdk3/PlaneGcsSdk_Shared/CopterControllers/AvatarController.cs

using Plane.CopterManagement;
using Plane.Copters;
using Plane.Messaging;
using System.Collections.Generic;

namespace Plane.CopterControllers
{
    /// <summary>
    /// 通过倾斜、旋转设备对飞行器进行控制的控制器。
    /// </summary>
    public class AvatarController : CopterControllerBase
    {
        /// <summary>
        /// 允许最大的偏角范围的绝对值（例如飞行器左倾斜多少度为有效值，超出此值则取此值）
        /// </summary>
        private const int MAX_ANGLE_OFFSET = 45;

        private static readonly HashSet<CopterState> _acceptingMobileControlStates = new HashSet<CopterState> { CopterState.Locked, CopterState.HoverMode, CopterState.FloatMode, CopterState.CommandMode, CopterState.Returning, CopterState.Following };

        public AvatarController(IPLMessenger messenger, ICopterManager copterManager) : base(messenger, copterManager)
        {
            messenger.Register<LandingStateAcceptsAvatarChangedMessage>(this, m =>
            {
                if (m.LandingStateAcceptsAvatar) _acceptingMobileControlStates.Add(CopterState.Landing);
                else _acceptingMobileControlStates.Remove(CopterState.Landing);
            });
        }

        public override bool IsEnabled
        {
            get
            {
                return base.IsEnabled;
            }
            set
            {
                if (base.IsEnabled != value)
                {
                    base.IsEnabled = value;
                    if (value)
                    {
                        _messenger.Register<RequestUpdateAttitudeMessage>(this, OnRequestUpdateAttitude);
                    }
                    else
                    {
                        _messenger.Unregister(this);
                    }
                }
            }
        }

        public SpeedType Speed { get; set; }

        public override CopterControllerTypes Type
        {
            get
            {
                return CopterControllerTypes.AvatarController;
            }
        }

        private void OnRequestUpdateAttitude(RequestUpdateAttitudeMessage m)
        {
            if (_copterManager.Copter != null && _acceptingMobileControlStates.Contains(_copterManager.Copter.State))
            {
                // TODO: 王海, 20151109, 整理并使用 Plane.Copters.Constants 类中的常量。

                ushort maxChannelOffset = (ushort)(Speed == SpeedType.SpeedFast ? 400 : 200);

                if (m.RollDegrees.IsInRange(-5, 5))
                {
                    _copterManager.Copter.DesiredChannel1 = RollMedianValue;
                }
                else
                {
                    _copterManager.Copter.DesiredChannel1 = (ushort)(m.RollDegrees.Limit(-MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET) * maxChannelOffset / MAX_ANGLE_OFFSET + RollMedianValue);
                }
                if (m.PitchDegrees.IsInRange(-5, 5))
                {
                    _copterManager.Copter.DesiredChannel2 = PitchMedianValue;
                }
                else
                {
                    _copterManager.Copter.DesiredChannel2 = (ushort)(m.PitchDegrees.Limit(-MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET) * maxChannelOffset / MAX_ANGLE_OFFSET + PitchMedianValue);
                }
                _copterManager.Copter.DesiredYaw = m.YawDegrees;
            }
        }
    }
}