Plane.Sdk3/PlaneGcsSdk_Shared/Copters/EHCopter.Mission.cs

// 读写任务序列的标准流程： http://qgroundcontrol.org/mavlink/waypoint_protocol

using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Threading.Tasks;
using static Plane.Protocols.MAVLink;

namespace Plane.Copters
{
    public partial class PLCopter
    {
        private static readonly IMission PRE_TAKE_OFF_MISSION = new Mission
        {
            Command = FlightCommand.Waypoint,
            Sequence = 0
        };

        private static readonly IMission TAKE_OFF_MISSION = new Mission
        {
            Command = FlightCommand.TakeOff,
            Altitude = 10,
            Sequence = 1
        };

        /// <summary>
        /// 飞行器正在请求的任务序号。
        /// </summary>
        private ushort? _copterRequestingMissionSeq;

        /// <summary>
        /// 指示是否正在查询任务序列。
        /// </summary>
        private bool _isRequestingMissionList;

        /// <summary>
        /// 指示是否正在写入任务序列。
        /// </summary>
        private bool _isWritingMissionList;

        /// <summary>
        /// 向飞行器查询得到的任务序列。
        /// </summary>
        private List<IMission> _missionList;

        /// <summary>
        /// 用于判断查询任务序列操作超时的秒表。
        /// </summary>
        private Stopwatch _requestMissionListStopwatch;

        /// <summary>
        /// 写任务操作的结果。
        /// </summary>
        private MAV_MISSION_RESULT? _writeMissionListResult;

        /// <summary>
        /// 用于判断写入任务序列操作超时的秒表。
        /// </summary>
        private Stopwatch _writeMissionListStopwatch;

        /// <summary>
        /// 查询任务序列。若上一个操作未完成，本次结果为 null。
        /// </summary>
        /// <param name="millisecondsTimeout">超时时间。</param>
        /// <returns>飞行器返回的任务序列。若上一个操作未完成，本次结果为 null；若超时，结果为不完整的序列（_mission.Count 小于 MissionCount）；若顺利完成，结果为完整的任务序列。</returns>
        public async Task<IEnumerable<IMission>> RequestMissionListAsync(int millisecondsTimeout = 10000)
        {
            if (_isRequestingMissionList) return null;
            _isRequestingMissionList = true;

            MissionCount = null;

            if (_missionList == null) _missionList = new List<IMission>();
            else _missionList.Clear();

            if (_requestMissionListStopwatch == null) _requestMissionListStopwatch = Stopwatch.StartNew();
            else _requestMissionListStopwatch.Restart();

            // 查询任务总数。
            await _internalCopter.GetWPsAsync().ConfigureAwait(false);

            // 异步等待 mavlink_mission_count_t 消息（收到后会赋值给 MissionCount）。
            while (MissionCount == null)
            {
                if (_requestMissionListStopwatch.ElapsedMilliseconds >= millisecondsTimeout)
                {
                    // 超时。
                    return EndRequestingMissionList();
                }
                await Task.Delay(50).ConfigureAwait(false);
            }

            for (ushort i = 0; i < MissionCount; i++)
            {
                // 查询 i 号任务。
                await _internalCopter.GetWPAsync(i).ConfigureAwait(false);

                // 异步等待 mavlink_mission_item_t 消息（收到后会把 Mission 对象放到 _missions 列表的合适位置）。
                while (_missionList.Count <= i)
                {
                    if (_requestMissionListStopwatch.ElapsedMilliseconds >= millisecondsTimeout)
                    {
                        // 超时。
                        return EndRequestingMissionList();
                    }
                    await Task.Delay(50).ConfigureAwait(false);
                }
            }

            // 成功接收了全部任务，发送 Ack。
            await SendMissionAckAsync();

            // 清理各字段并返回任务序列。
            return EndRequestingMissionList();
        }

        /// <summary>
        /// 写入任务序列。
        /// </summary>
        /// <param name="missions">任务序列。</param>
        /// <param name="millisecondsTimeout">超时时间，单位为毫秒。</param>
        /// <returns>若确定写入成功，返回 true；否则（上一个写任务操作未完成，或者飞控返回失败消息，或者超时）返回 false。</returns>
        public async Task<bool> WriteMissionListAsync(IEnumerable<IMission> missions, int millisecondsTimeout = 10000)
        {
            if (_isWritingMissionList) return false;

            _isWritingMissionList = true;
            _writeMissionListResult = null;
            _copterRequestingMissionSeq = null;

            if (_writeMissionListStopwatch == null) _writeMissionListStopwatch = Stopwatch.StartNew();
            else _writeMissionListStopwatch.Restart();

            // 写任务总数。
            await WriteMissionCountAsync((ushort)(missions.Count() + 1)).ConfigureAwait(false);

            // 任务序号。
            ushort seq = 0;

            // 等待飞控请求 seq 号任务。
            if (!await AwaitMissionRequestAsync(seq, millisecondsTimeout).ConfigureAwait(false))
            {
                // 超时。
                return EndWritingMissions();
            }

            // 写起飞前准备任务。
            await WriteMissionAsync(PRE_TAKE_OFF_MISSION).ConfigureAwait(false);
/*
            seq++;

            // 等待飞控请求 seq 号任务。
            if (!await AwaitMissionRequestAsync(seq, millisecondsTimeout).ConfigureAwait(false))
            {
                // 超时。
                return EndWritingMissions();
            }

            // 写起飞任务。
            await WriteMissionAsync(TAKE_OFF_MISSION).ConfigureAwait(false);
            */

            foreach (var mission in missions)
            {
                // 等待飞控请求 seq 号任务。
                if (!await AwaitMissionRequestAsync(++seq, millisecondsTimeout).ConfigureAwait(false))
                {
                    // 超时。
                    return EndWritingMissions();
                }
                mission.Sequence = seq;
                await WriteMissionAsync(mission).ConfigureAwait(false);
            }

            // 等待飞控返回 Ack。
            while (_writeMissionListResult == null && _writeMissionListStopwatch.ElapsedMilliseconds < millisecondsTimeout)
            {
                await Task.Delay(50).ConfigureAwait(false);
            }

            return EndWritingMissions();
        }

        /// <summary>
        /// 异步等待飞行器向地面站请求 seq 号任务。
        /// </summary>
        /// <param name="seq">任务序号。</param>
        /// <param name="millisecondsTimeout">超时时间。</param>
        /// <returns>若未超时并成功收到请求，返回 true；否则返回 false。</returns>
        private async Task<bool> AwaitMissionRequestAsync(ushort seq, int millisecondsTimeout)
        {
            while (_copterRequestingMissionSeq != seq)
            {
                if (_writeMissionListStopwatch.ElapsedMilliseconds >= millisecondsTimeout)
                {
                    return true;
                }
                await Task.Delay(50).ConfigureAwait(false);
            }
            return true;
        }

        private IEnumerable<IMission> EndRequestingMissionList()
        {
            _requestMissionListStopwatch.Stop();
            _isRequestingMissionList = false;
            return _missionList;
        }

        private bool EndWritingMissions()
        {
            _writeMissionListStopwatch.Stop();
            _isWritingMissionList = false;
            return _writeMissionListResult == MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED;
        }

        private async Task SendMissionAckAsync()
        {
            var ack = new mavlink_mission_ack_t
            {
                target_component = _internalCopter.compid,
                target_system = _internalCopter.sysid,
                type = (byte)MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED
            };
            await _internalCopter.GeneratePacketAsync(MAVLINK_MSG_ID_MISSION_ACK, ack).ConfigureAwait(false);
        }

        private Task WriteMissionAsync(IMission mission)
        {
            var frame = mission.Command == FlightCommand.Waypoint && mission.Sequence == 0 ? MAV_FRAME.GLOBAL : MAV_FRAME.GLOBAL_RELATIVE_ALT;

            var req = new mavlink_mission_item_t();

            req.target_system = _internalCopter.sysid;
            req.target_component = _internalCopter.compid;

            req.command = (byte)mission.Command;

            req.current = 0;
            req.autocontinue = 1;

            req.frame = (byte)frame;
            req.x = (float)mission.Latitude;
            req.y = (float)mission.Longitude;
            req.z = mission.Altitude;

            req.param1 = mission.Param1;
            req.param2 = mission.Param2;
            req.param3 = mission.Param3;
            req.param4 = mission.Param4;

            req.seq = mission.Sequence;

            return _internalCopter.GeneratePacketAsync(MAVLINK_MSG_ID_MISSION_ITEM, req);
        }

        private Task WriteMissionCountAsync(ushort count)
        {
            return _internalCopter.SetWPTotalAsync(count);
        }
    }
}