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();
        private byte[] configData1 = null;
        private byte[] configData2 = null;
        private byte[] configurationData = null;

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

        private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
        {
            Task.Run(async () =>
            {
                lock (MissinLocker)
                {
                    if (wirteMissRet == 0x20)
                    {
                        if (missionWriteState.ContainsKey(copterId))
                            missionWriteState[copterId].WriteSucceed = true;
                        Message.Show($"{copterId}:航点写入成功");
                    }
                    if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
                    {
                        if (missionWriteState.ContainsKey(copterId))
                            missionWriteState[copterId].WriteSucceed = false;
                        Message.Show($"{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 Analyze_curr_sys_parm_Packet(byte[] buffer)
        {
            Message.Show($"buffer length = {buffer.Length}");
            short num = BitConverter.ToInt16(buffer, 0);
            short offset = BitConverter.ToInt16(buffer, 2);
            short curLength = BitConverter.ToInt16(buffer, 4);
            short allLength = BitConverter.ToInt16(buffer, 6);

            if (num == 0)
            {
                configData1 = new byte[curLength];
                Array.Copy(buffer, 8, configData1, 0, curLength);
                Message.Show($"part0 长度={curLength}， 总长{allLength}");
            }
            else if (num == 1)
            {
                configData2 = new byte[curLength];
                Array.Copy(buffer, 8, configData2, 0, curLength);
                Message.Show($"part1 长度={curLength}， 总长{allLength}");
            }

            if (configData1 != null && configData2 != null)
            {
                Message.Show("合成数据");
                configurationData = new byte[configData1.Length + configData2.Length];
                configurationData = configData1.Concat(configData2).ToArray();
            }
        }

        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;
            }

        }
    }
}