Plane.Sdk3/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs

using Plane.Communication;
using Plane.Protocols;
using Plane.Windows.Messages;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Plane.CommunicationManagement
{
    public partial class CommModuleManager
    {
        private int MissionStartCopterId = 0;
        private int MissionSendCopterId = 0;
        private int MissionErrorId = -1;
        private int ErrorCode = 0;
        public event EventHandler<CommunicationReceiveCopterInfoEventArgs> CommunicationReceived;
        public event EventHandler<CommunicationCopterDisconnectEventArgs> CommunicationCopterDisconnect;
        public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected;
        private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>();
        private static readonly object MissinLocker = new object();

        public Dictionary<int, CommWriteMissinState> MissionWriteState
        {
            get {return missionWriteState; }
        }

        public void ClearMissionWriteState()
        {
            missionWriteState.Clear();
        }

        private void AnalyzeMissionStartPacket(short copterId, byte[] buffer)
        {
            short errorId = BitConverter.ToInt16(buffer, 0);
            MissionStartCopterId = copterId;
            MissionErrorId = errorId;
            if(errorId != 0)
                Message.Show($"{copterId}:返回 = {errorId}");
        }

        private void AnalyzeStringPacket(short copterId, byte[] buffer)
        {
            int count = Array.IndexOf(buffer, (byte)0);
            string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube");
            Message.Show(msg);
        }

        private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer)
        {
            ushort ret = BitConverter.ToUInt16(buffer, 0);
            switch (ret)
            {
                case MavComm.SEARCH_FINDCOPTER:
                    Message.Show(copterId.ToString() + "---飞机开始相应");
                    break;

                case MavComm.SEARCH_COMPLETED:
                    Message.Show(copterId.ToString() + "---设置成功");
                    break;

                case MavComm.SEARCH_OUTTIME:
                    Message.Show("超时无响应");
                    break;

                case MavComm.MISSION_SEND_COMPLETED:
                    MissionSendCopterId = copterId;
                    break;

                case MavComm.P2P_SEND_FAILED:
                    Message.Show("点对点通信发送失败");
                    break;
                default:
                    if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END)
                    {
                        ErrorCode = ret;
                        Message.Show($"{copterId}--错误码：{ret}");
                    }
                        
                    break;
            }
        }

        private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer)
        {
            byte type = buffer[0];
            byte connectRet;
            switch (type)
            {
                case 0x01:
                    //connectRet = buffer[1];
                    //if (connectRet == 0x0)  //飞机连接上 不管飞控和通信模块的连接
                        CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
                    break;
                case 0x02:
                    connectRet = buffer[1];
                    if (connectRet == 0x0)  //飞机断开
                        CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
                    break;

                case 0x03:
                    SaveMissionWriteStat(copterId, buffer[1]);
                    break;

                case 0x04:
                    Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}");
                    break;

                case 0x0e:
                    if (copterId == 0)
                        Message.Show("----------全部更新完成----------");
                    else
                        Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
                    break;

                default:
                      //  Message.Show($"Comm3返回：{type}");
                    break;
            }
        }

        private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
        {
            Task.Run(async () =>
            {
                lock (MissinLocker)
                {
                   // var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);

                    if (wirteMissRet == 0x20)
                    {
                        if (missionWriteState.ContainsKey(copterId))
                            missionWriteState[copterId].WriteSucceed = true;
                        Message.Show($"ID:{copterId}:航点写入成功");
                    }
                    if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
                    {
                        if (missionWriteState.ContainsKey(copterId))
                            missionWriteState[copterId].WriteSucceed = false;
                        Message.Show($"ID:{copterId}:航点写入失败");
                    }
                }
                await Task.Delay(10).ConfigureAwait(false);
            }
           );
            
        }

        private void AnalyzeCopterInfosPacket(byte[] buffer)
        {
            ushort beginNum = BitConverter.ToUInt16(buffer, 0);
            ushort endNum = BitConverter.ToUInt16(buffer, 2);
            int count = endNum - beginNum + 1;
            byte[] copter_packets = buffer.Skip(4).ToArray();
            if (copter_packets.Length != count * 4)
            {
                return;
            }

            int offset = 0;
            for (int i = beginNum; i <= endNum; i++)
            {
                byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray();
                UInt16 state = BitConverter.ToUInt16(onePacket, 0);
                short copterId = (short)i;
                switch (state >> 13)
                {
                    //0B000
                    case 0x0000 >> 13:
                        CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
                        break;
                    //0B100
                    case 0x8000 >> 13:
                        break;
                    //0B110
                    case 0xC000 >> 13:
                    //0B111
                    case 0xE000 >> 13:
                        CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
                        break;
                }
                offset += 4;
            }
        }

        private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer)
        {
            byte[] packet = buffer.Take(28).ToArray();
            byte[] state_packet = buffer.Skip(28).ToArray();
            UInt16 state = BitConverter.ToUInt16(state_packet,0);
            byte version = state_packet[3];
            switch (state >> 13)
            {
                //0B000
                case 0x0000 >> 13:
                    CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
                    break;
                //0B100
                case 0x8000 >> 13:
                    break;
                //0B110
                case 0xC000 >> 13:
                //0B111
                case 0xE000 >> 13: //最后正在用的版本
                    CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
                    break;
            }

        }
    }
}