上传通信模块

修改了多网卡同时使用时的连接判断

PlaneGcsSdk_Shared/Communication/TcpConnection.cs

@@ -1,6 +1,7 @@
 #if !NETFX_CORE
 
 using System;
+using System.Net;
 using System.Net.Sockets;
 using System.Threading.Tasks;
 
@@ -48,6 +49,23 @@ namespace Plane.Communication
             _stream = _client.GetStream();
         }
 
+        public async Task<bool> BindIP(string ip)
+        {
+            bool bind = false;
+            try
+            {
+                IPAddress IPLocal = IPAddress.Parse(ip);
+                _client.Client.Bind(new IPEndPoint(IPLocal, 0));
+                bind = true;
+            }
+            catch
+            {
+                bind = false;
+            }
+            await Task.Delay(10).ConfigureAwait(false);
+            return bind;
+        }
+
         public override async Task OpenAsync()
         {
             if (!IsOpen)
@@ -56,13 +74,16 @@ namespace Plane.Communication
                 {
                     await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
                 }
+                //屏蔽掉异常处理
+                //CreateClientAndConnectAsync会new一个TcpClient并且重新连接  
+                //之前设置的TcpClient中Socket Bind会无效，在多网卡工作时会导致断线重连的时间特别长
                 catch (SocketException)
                 {
-                    await CreateClientAndConnectAsync();
+                    //await CreateClientAndConnectAsync();
                 }
                 catch (ObjectDisposedException)
                 {
-                    await CreateClientAndConnectAsync();
+                    //await CreateClientAndConnectAsync();
                 }
                 _isBroken = false;
             }

PlaneGcsSdk_Shared/CommunicationManagement/CommConnection.cs

@@ -0,0 +1,42 @@
+using Plane.Communication;
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace Plane.CommunicationManagement
+{
+    public class CommConnection : IConnection
+    {
+        public bool IsOpen { get; protected set; }
+
+        public event EventHandler<ExceptionThrownEventArgs> ExceptionThrown;
+
+        public int BytesToRead()
+        {
+            return 0;
+        }
+
+        public void Close()
+        {
+            IsOpen = false;
+        }
+
+        public Task OpenAsync()
+        {
+            IsOpen = true;
+            return TaskUtils.CompletedTask;
+        }
+
+        public async Task<int> ReadAsync(byte[] buffer, int offset, int count)
+        {
+            await TaskUtils.CompletedTask;
+            return 0;
+        }
+
+        public async Task WriteAsync(byte[] buffer, int offset, int count)
+        {
+            await TaskUtils.CompletedTask;
+        }
+    }
+}

PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs

@@ -0,0 +1,552 @@
+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: ExceptionThrownEventSource, IDisposable
+    {
+        private static CommModuleManager _Instance = new CommModuleManager();
+        public static CommModuleManager Instance { get { return _Instance; } }
+
+        public TcpConnection Connection = null;
+        public bool CommOK = false;
+        private const string MODULE_IP = "192.168.199.51";
+        private const string LOCAL_IP = "192.168.199.50";
+        private const int PORT = 9551;
+        private bool _disposed;
+        public int CommModuleCopterCount = 0;
+        public int CommModuleCurMode = 0;
+        private long packetCountNum = 0;
+
+        /// <summary>
+        /// 用于判断写入任务操作超时的秒表。
+        /// </summary>
+        private Stopwatch _writeMissionStopwatch;
+
+        public async Task ConnectAsync()
+        {
+            Connection = new TcpConnection(MODULE_IP, PORT);
+            await ConnectOpenAsync();
+        }
+
+        public void Connect()
+        {
+            Task.Run(async () =>
+            {
+                Connection = new TcpConnection(MODULE_IP, PORT);
+                await ConnectOpenAsync();
+            }
+            );
+        }
+
+        public void CloseConnection()
+        {
+            CommOK = false;
+            Connection?.Close();
+        }
+
+        private async Task ConnectOpenAsync()
+        {
+            CommOK = true;
+            bool bind = await Connection.BindIP(LOCAL_IP);
+            if (bind)
+            {
+                try
+                {
+                    await Connection.OpenAsync().ConfigureAwait(false);
+                }
+                catch
+                { }
+            }
+
+            if (Connection.IsOnline)
+            {
+                SendQuery();
+                await StartReadingPacketsAsync();
+            }
+            else
+            {
+                Reconnect();
+            }
+        }
+
+        //循环发送查询，保持通信正常
+        private void SendQuery()
+        {
+            Task.Run(async () =>
+            {
+                long lastPacketCountNum = 0;
+                while (CommOK)
+                {
+                    if (Connection != null)
+                    {
+                        await SendQueryStatusPacketsAsync();
+                        await Task.Delay(1200).ConfigureAwait(false);
+
+                        if (packetCountNum > lastPacketCountNum)
+                            lastPacketCountNum = packetCountNum;
+                        else
+                            break;
+                    }
+                }
+                Message.Show("通信断开");
+
+                Reconnect();
+            }).ConfigureAwait(false);
+        }
+
+        private async Task SendQueryStatusPacketsAsync()
+        {
+            await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
+            await Task.Delay(100);
+        }
+
+        int times = 1;
+        private void Reconnect()
+        {
+            Task.Run(async () =>
+            {
+                CloseConnection();
+                await Task.Delay(250).ConfigureAwait(false);
+                Message.Show($"正在重连:次数{times++}");
+                await Task.Delay(250).ConfigureAwait(false);
+                await ConnectAsync();
+                
+            }).ConfigureAwait(false);
+        }
+
+        private async Task StartReadingPacketsAsync()
+        {
+            await Task.Run(async () =>
+            {
+                while (CommOK)
+                {
+                    if (Connection == null || !Connection.IsOnline)
+                    {
+                        break;
+                    }
+
+                    var packet = await ReadPacketAsync().ConfigureAwait(false);
+                    if (packet != null)
+                    {
+                        packetCountNum++;
+                        short serialNum = BitConverter.ToInt16(packet, 4);
+                        short copterNum = BitConverter.ToInt16(packet, 6);
+                        short commandType = BitConverter.ToInt16(packet, 8);
+                        byte[] dealData;
+                        switch (commandType)
+                        {
+                            case MavComm.COMM_DOWNLOAD_MODE:
+                                dealData = packet.Skip(10).ToArray();
+                                AnalyzeMissionStartPacket(copterNum, dealData);
+                                break;
+
+                            case MavComm.COMM_GET_COPTERS_COUNT:
+                                dealData = packet.Skip(16).ToArray();
+                                AnalyzeCopterInfosPacket(dealData);
+                                break;
+
+                            case MavComm.COMM_NOTIFICATION:
+                                short messageType = BitConverter.ToInt16(packet, 10);
+                                dealData = packet.Skip(12).ToArray();
+                                switch (messageType)
+                                {
+                                    case (short)MavComm.MessageType.STR:
+                                        AnalyzeStringPacket(copterNum, dealData);
+                                        break;
+
+                                    case (short)MavComm.MessageType.SCAN2:
+                                        AnalyzeComm2RetrunPacket(copterNum, dealData);
+                                        break;
+
+                                    case (short)MavComm.MessageType.SCAN3:
+                                        AnalyzeComm3RetrunPacket(copterNum, dealData);
+                                        break;
+                                    case 4:
+                                        AnalyzeComm4RetrunPacket(copterNum, dealData);
+                                        break;
+                                }
+                                break;
+                            case 0:
+                                int packetLength = packet.Length;
+                                short errorId = BitConverter.ToInt16(packet, packetLength - 6);
+                                short copterCount = BitConverter.ToInt16(packet, packetLength - 4);
+                                short curMode = BitConverter.ToInt16(packet, packetLength - 2);
+                                string msg = string.Format("错误Id={0}, 检测飞机总数={1}，工作模式={2} 流水号={3}", errorId, copterCount, curMode, serialNum);
+                                CommModuleCopterCount = copterCount;
+                                CommModuleCurMode = curMode;
+                                //Message.Show(msg);                              
+                                break;
+                        } 
+                    }
+                }
+            }).ConfigureAwait(false);
+            Message.Show("连接断开");
+        }
+        
+        private async Task<byte[]> ReadPacketAsync()
+        {
+            int readnumber = 0;
+            if (Connection == null || !Connection.IsOnline)
+            {
+                return null;
+            }
+            try
+            {
+                byte[] bufferHaed = new byte[2];
+                readnumber = await Connection.ReadAsync(bufferHaed, 0, 2);
+                if (bufferHaed[0] == 0x13 && bufferHaed[1] == 0x77)
+                {
+                    byte[] bufferLength = new byte[2];
+                    readnumber = await Connection.ReadAsync(bufferLength, 0, 2);
+                    short datalength = BitConverter.ToInt16(bufferLength, 0);
+                    //Console.WriteLine("datalength = "+ datalength);
+
+                    byte[] bufferData = new byte[datalength];
+                    readnumber = await Connection.ReadAsync(bufferData, 0, datalength);
+                   
+//                     foreach (byte b in bufferData)
+//                     {
+//                         Console.Write(b.ToString("X2"));
+//                         Console.Write(" ");
+//                     }
+//                     Console.WriteLine("");
+
+                    byte[] needCRCData = new byte[datalength + 4];
+                    Array.Copy(bufferHaed, 0, needCRCData, 0, 2);
+                    Array.Copy(bufferLength, 0, needCRCData, 2, 2);
+                    Array.Copy(bufferData, 0, needCRCData, 4, datalength);
+
+                    byte[] crc = checkCRC16(needCRCData);
+                    byte[] bufferCRC16 = new byte[2];
+                    readnumber = await Connection.ReadAsync(bufferCRC16, 0, 2);
+
+                    if (crc[0] == bufferCRC16[0] && crc[1] == bufferCRC16[1])
+                    {
+                        return needCRCData;
+                    }
+                }                
+            }
+            catch
+            {
+                //Console.WriteLine("错误");
+                return null;
+            }
+            return null;
+        }
+
+       
+
+        short serial_Number = 1;
+
+        public async Task GenerateDataAsync<TMavCommPacket>(short copterId, short messageType, TMavCommPacket indata)
+        {
+            byte[] data;
+            data = MavlinkUtil.StructureToByteArray(indata);
+            await WriteCommPacketAsync(copterId, messageType, data);
+        }
+
+        bool temp = false;
+        //测试用 灯光间隔1S闪烁
+        public async Task TestLED(short id)
+        {
+            MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
+            led.target_system = 1;
+            led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
+            led.instance = 0;
+            led.pattern = 0;
+            led.custom_len = 4;
+            led.custom_bytes = new byte[24];
+            led.custom_bytes[0] = 80;
+            led.custom_bytes[1] = 0;
+            led.custom_bytes[2] = 0;
+            led.custom_bytes[3] = 3;
+            byte[] data;           
+            data = MavlinkUtil.StructureToByteArray(led);
+
+            byte[] packet = new byte[data.Length + 6 + 2];
+            packet[0] = MAVLink.MAVLINK_STX;
+
+            packet[1] = (byte)(data.Length);
+            packet[2] = 1;
+            packet[3] = 255; // this is always 255 - MYGCS
+
+            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
+
+            packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
+            int i = 6;
+            foreach (byte b in data)
+            {
+                packet[i] = b;
+                i++;
+            }
+           
+            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);          
+            checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], checksum);
+
+            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
+            byte ck_b = (byte)(checksum >> 8); ///< Low byte
+
+            packet[i] = ck_a;
+            i += 1;
+            packet[i] = ck_b;
+            i += 1;
+
+
+            //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+            byte[] senddata = packet;
+
+            for (int times = 0; times < 3; times++)
+            {
+                senddata = senddata.Concat(packet).ToArray();
+            }
+
+
+            temp = !temp;
+            while (temp)
+            {
+                await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet);
+                await Task.Delay(1000).ConfigureAwait(false);
+            } 
+        }
+
+        /// <summary>
+        /// 生成通信模块packet并且发送
+        /// </summary>
+        /// <param name="copterId">飞机ID</param>
+        /// <param name="messageType">命令类型</param>
+        /// <param name="data">数据类型：空表示只切换模式</param>
+        /// <param name="batchPacket">小批量数据包</param>
+        /// <returns></returns>
+        public async Task WriteCommPacketAsync(short copterId, short messageType, byte[] data = null, byte[] batchPacket = null)
+        {
+            if (data != null &&  batchPacket != null) data = batchPacket.Concat(data).ToArray();
+
+            int packetlength = data == null ? 0 : data.Length;
+
+            byte[] packet = new byte[packetlength + 10];
+
+            byte[] buffer_header = new byte[2];
+            buffer_header = BitConverter.GetBytes(MavComm.COMM_SEND_PACKET_HEADER);
+            Array.Copy(buffer_header, 0, packet, 0, 2);         //数据头
+
+            byte[] buffer_length; 
+            buffer_length = BitConverter.GetBytes((Int16)(packetlength + 6));
+            Array.Copy(buffer_length, 0, packet, 2, 2);         //数据长度
+
+            byte[] buffer_serial;
+            buffer_serial = BitConverter.GetBytes(serial_Number++); 
+            Array.Copy(buffer_serial, 0, packet, 4, 2);         //流水号
+
+            byte[] buffer_copterId = new byte[2];
+            buffer_copterId = BitConverter.GetBytes((Int16)copterId);
+            Array.Copy(buffer_copterId, 0, packet, 6, 2);       //飞机号
+
+            byte[] buffer_messageType = new byte[2];
+            buffer_messageType = BitConverter.GetBytes((Int16)messageType);
+            Array.Copy(buffer_messageType, 0, packet, 8, 2);    //命令类型
+
+            if (data != null)
+                Array.Copy(data, 0, packet, 10, data.Length);   //数据内容
+
+            byte[] buffer_CRC = checkCRC16(packet);
+
+            byte[] buffer_packet = new byte[packet.Length + 2];
+            Array.Copy(packet, buffer_packet, packet.Length);
+            Array.Copy(buffer_CRC, 0, buffer_packet, packet.Length, 2);
+
+            if (Connection != null && Connection.IsOnline)
+            { 
+                try
+                {
+                    await Connection.WriteAsync(buffer_packet, 0, buffer_packet.Length);
+                }
+                catch 
+                {
+
+                }
+            }
+
+        }
+
+        public async Task<bool> MissionPacket<TMavCommPacket>(short copterId, byte messageType, TMavCommPacket[] indata)
+        {
+            int dataLength = 6 + 2 + indata.Length * 32;
+            byte[] data = new byte[dataLength];
+
+            byte[] uses = new byte[] { 0, 0, 1, 0, 0, 0 };
+            Array.Copy(uses, 0, data, 0, 6);
+
+            Int16 countNum = (Int16)indata.Length;
+            Array.Copy(BitConverter.GetBytes(countNum), 0, data, 6, 2);
+
+            int offset = 8;
+
+            foreach (TMavCommPacket MavPacket in indata)
+            {
+                byte[] MavPacketData = MavlinkUtil.StructureToByteArray(MavPacket);
+                byte[] tempData = new byte[32];
+
+                //张伟只需要32个有用数据 0-27， 30-31， 34-35
+                Array.Copy(MavPacketData, 0, tempData, 0, 28);
+                Array.Copy(MavPacketData, 30, tempData, 28, 2);
+                Array.Copy(MavPacketData, 34, tempData, 30, 2);
+
+                Array.Copy(tempData, 0, data, offset, 32);
+                offset += 32;
+            }
+
+            await WriteCommPacketAsync(copterId, messageType, data);
+
+            if (_writeMissionStopwatch == null) _writeMissionStopwatch = Stopwatch.StartNew();
+            else _writeMissionStopwatch.Restart();
+
+            //先延时5秒判断通信模块是否返回错误ID，0为正确。 如果已经错误了就不需要等待下发了。
+            MissionStartCopterId = 0; MissionErrorId = -1;
+            ErrorCode = 0;
+
+            if (await AwaitCommMissionStartAsync(copterId, 5000))
+            {
+                _writeMissionStopwatch.Restart();
+                MissionSendCopterId = 0;
+                //等待通信模块地面站数据下发后，才能开始下个飞机的航点写入
+                //下发不代表写入成功
+                return await AwaitCommMissionRequestAsync(copterId, 10000).ConfigureAwait(false);
+            }
+            else
+                return false;
+        }
+
+        public async Task<bool> AwaitCommMissionStartAsync(short copterId, int millisecondsTimeout)
+        {
+            while (MissionStartCopterId != copterId)
+            {
+                if (_writeMissionStopwatch.ElapsedMilliseconds >= millisecondsTimeout)
+                {
+                    return false;
+                }
+                await Task.Delay(50).ConfigureAwait(false);
+            }
+            if (MissionErrorId == 0)
+                return true;
+            else
+            {
+                //Message.Show($"{copterId}:返回错误--{MissionErrorId}");
+                return false;
+            }
+        }
+
+        public async Task<bool> AwaitCommMissionRequestAsync(short copterId, int millisecondsTimeout)
+        {
+            while (MissionSendCopterId != copterId)
+            {
+                if (_writeMissionStopwatch.ElapsedMilliseconds >= millisecondsTimeout || ErrorCode != 0)
+                {
+                    return false;
+                }
+                await Task.Delay(50).ConfigureAwait(false);
+            }
+            return true;
+        }
+
+        private long GetCreatetime()
+        {
+            DateTime DateStart = new DateTime(2000, 1, 1, 0, 0, 0);
+            return Convert.ToInt64((DateTime.Now - DateStart).TotalMilliseconds);
+        }
+
+        public static byte[] CRC16(byte[] data)
+        {
+            int len = data.Length;
+            if (len > 0)
+            {
+                ushort crc = 0xFFFF;
+
+                for (int i = 0; i < len; i++)
+                {
+                    crc = (ushort)(crc ^ (data[i]));
+                    for (int j = 0; j < 8; j++)
+                    {
+                        crc = (crc & 1) != 0 ? (ushort)((crc >> 1) ^ 0xA001) : (ushort)(crc >> 1);
+                    }
+                }
+                byte hi = (byte)((crc & 0xFF00) >> 8);  //高位置
+                byte lo = (byte)(crc & 0x00FF);         //低位置
+                return new byte[] { hi, lo };
+            }
+            return new byte[] { 0, 0 };
+        }
+
+        public byte[] CRCCalc(byte[] crcbuf)
+        {
+            //计算并填写CRC校验码
+            int crc = 0xffff;
+            int len = crcbuf.Length;
+            for (int n = 0; n < len; n++)
+            {
+                byte i;
+                crc = crc ^ crcbuf[n];
+                for (i = 0; i < 8; i++)
+                {
+                    int TT;
+                    TT = crc & 1;
+                    crc = crc >> 1;
+                    crc = crc & 0x7fff;
+                    if (TT == 1)
+                    {
+                        crc = crc ^ 0xa001;
+                    }
+                    crc = crc & 0xffff;
+                }
+
+            }
+            byte[] redata = new byte[2];
+            redata[1] = (byte)((crc >> 8) & 0xff);
+            redata[0] = (byte)((crc & 0xff));
+            return redata;
+        }
+
+        public byte[] checkCRC16(byte[] puchMsg)
+        {
+
+            int usDataLen = puchMsg.Length;
+
+            ushort iTemp = 0x0;
+            ushort flag = 0x0;
+
+            for (int i = 0; i < usDataLen; i++)
+            {
+                iTemp ^= puchMsg[i];
+                for (int j = 0; j < 8; j++)
+                {
+                    flag = (ushort)(iTemp & 0x01);
+                    iTemp >>= 1;
+                    if (flag == 1)
+                    {
+                        iTemp ^= 0xa001;
+                    }
+                }
+            }
+            byte[] ret = BitConverter.GetBytes(iTemp);
+            return ret;
+        }
+
+        public void Dispose()
+        {
+            if (!CommOK)
+            {
+                return;
+            }
+            CommOK = false;
+            CloseConnection();
+        }
+    }
+}

PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs

@@ -0,0 +1,602 @@
+using Plane.Copters;
+using Plane.Protocols;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using static Plane.Copters.PlaneCopter;
+
+namespace Plane.CommunicationManagement
+{
+    public partial class CommModuleManager
+    {
+        public int packetcount = 0;
+       
+
+        /// <summary>
+        /// 设置通道
+        /// </summary>
+        /// <param name="id"></param>
+        /// <param name="ch1"></param>
+        /// <param name="ch2"></param>
+        /// <param name="ch3"></param>
+        /// <param name="ch4"></param>
+        /// <returns></returns>
+        public async Task SetChannelsAsync(IEnumerable<ICopter> copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters, out copterId, out batchPacket);
+            
+            byte[] packet = SetChannels(ch1, ch2, ch3, ch4);
+            await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+        }
+
+
+        private byte[] SetChannels(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null)
+        {
+            MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
+            rc.target_component = 1;
+            rc.target_system = 1;
+            rc.chan1_raw = ch1 ?? 1500;
+            rc.chan2_raw = ch2 ?? 1500;
+            rc.chan3_raw = ch3 ?? 1500;
+            rc.chan4_raw = ch4 ?? 1500; 
+            rc.chan5_raw = 1500;
+            rc.chan6_raw = 1500;
+            rc.chan7_raw = 1500;
+            rc.chan8_raw = 1500;
+            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc);
+        }
+
+        public async Task<bool> WriteMissionListAsync(short id, List<IMission> missions)
+        {
+            missions.Insert(0, PLCopter.PRE_TAKE_OFF_MISSION);
+            List<MAVLink.mavlink_mission_item_t> mission_list = new List<MAVLink.mavlink_mission_item_t>();
+            foreach (IMission mission in missions)
+            {
+                var frame = mission.Command == FlightCommand.Waypoint && mission.Sequence == 0 ? MAVLink.MAV_FRAME.GLOBAL : MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
+
+                var req = new MAVLink.mavlink_mission_item_t();
+
+                req.target_system = 1;
+                req.target_component = 1;
+
+                req.command = (byte)mission.Command;
+
+                req.current = 0;
+                req.autocontinue = 1;
+
+                req.frame = (byte)frame;
+                if (mission.Command == FlightCommand.LEDControl)
+                {
+                    req.x = mission.R;
+                    req.y = mission.G;
+                    req.z = mission.B;
+                }
+                else
+                {
+                    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;
+                mission_list.Add(req);
+            }
+
+            bool result =  await MissionPacket(id, (byte)MavComm.COMM_DOWNLOAD_MODE, mission_list.ToArray());
+            
+            lock(MissinLocker)
+            {
+                if (missionWriteState.ContainsKey(id))
+                {
+                    missionWriteState[id].StateReturn = MissionErrorId;
+                    missionWriteState[id].ErrorCode = ErrorCode;
+                    missionWriteState[id].SendAchieved = result;
+                    missionWriteState[id].WriteSucceed = false;
+                }
+                else
+                {
+                    CommWriteMissinState state = new CommWriteMissinState(result);
+                    state.StateReturn = MissionErrorId;
+                    state.ErrorCode = ErrorCode;
+                    missionWriteState.Add(id, state);
+                }
+                return result;
+            }
+        }
+
+        /// <summary>
+        /// 开始任务
+        /// </summary>
+        public async Task DoMissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
+        {
+            byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, 0);
+            await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
+        }
+
+        /// <summary>
+        /// 暂停任务
+        /// </summary>
+        public async Task DoMissionPauseAsync()
+        {
+            byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, 0, 0, 0, 0, 0, 1, 0);
+            await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
+        }
+
+        /// <summary>
+        /// 恢复任务
+        /// </summary>
+        public async Task DoMissionResumeAsync(int hour_utc, int minute_utc, int second_utc)
+        {
+            byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, 0, 0 , 2, 0);
+            await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
+        }
+        /// <summary>
+        /// 解锁
+        /// </summary>
+        /// <param name="armit"></param>
+        /// <returns></returns>
+        public async Task UnlockAsync(IEnumerable<ICopter> copters = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters,out copterId, out batchPacket);
+
+            await SetChannelsAsync(copters, 1500, 1500, 1100, 1500);
+
+
+            byte[] packet;
+            packet = SetModeAsync(FlightMode.ALT_HOLD);
+            await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+
+            await Task.Delay(100).ConfigureAwait(false);
+
+            packet = DoARMAsync(true);
+            await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+        }
+
+        /// <summary>
+        /// LED闪烁白灯
+        /// </summary>
+        /// <param name="copters"></param>
+        /// <returns></returns>
+        public void LED_FlickerAsync(IEnumerable<ICopter> copters = null)
+        {
+            Task.Run(async () =>
+            {
+                short copterId = 0;
+                byte[] batchPacket = null;
+                GetCopterIds(copters, out copterId, out batchPacket);
+
+                byte[] packet = DoLEDCommandAsync();
+                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+            });
+        }
+
+
+        /// <summary>
+        /// 设置参数
+        /// </summary>
+        /// <param name="paramname"></param>
+        /// <param name="value"></param>
+        /// <param name="copters"></param>
+        /// <returns></returns>
+        public async Task<int> SetParamAsync(string paramname, float value, IEnumerable<ICopter> copters = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters, out copterId, out batchPacket);
+           
+            byte[] packet = SetParam2Async(paramname, value);
+            int packetNum = packet[2];
+            await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+            return packetNum;
+        }
+
+        /// <summary>
+        /// 起飞测试
+        /// </summary>
+        /// <param name="alt"></param>
+        /// <param name="copters"></param>
+        /// <returns></returns>
+        public async Task TakeOffAsync(float alt, IEnumerable<ICopter> copters = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters, out copterId, out batchPacket);
+
+            byte[] packet;
+            packet = SetModeAsync(FlightMode.GUIDED);
+            await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+
+            await Task.Delay(10).ConfigureAwait(false);
+            await SetChannelsAsync(copters, 1500, 1500, 1500, 1500);
+            await Task.Delay(10).ConfigureAwait(false);
+
+            packet = DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt);
+            await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+        }
+
+        /// <summary>
+        /// 返航
+        /// </summary>
+        /// <param name="copters"></param>
+        /// <returns></returns>
+        public async Task ReturnToLaunchAsync(IEnumerable<ICopter> copters = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters, out copterId, out batchPacket);
+
+            byte[] packet = SetModeAsync(FlightMode.RTL);
+            await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+        }
+
+        /// <summary>
+        /// 获取版本
+        /// </summary>
+        /// <param name="copters"></param>
+        /// <returns></returns>
+        public async Task GetVersionsAsync(IEnumerable<ICopter> copters = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters, out copterId, out batchPacket);
+
+            MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request();
+            req.version = 0;
+            byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req);
+            await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+        }
+        /// <summary>
+        /// 降落
+        /// </summary>
+        /// <returns></returns>
+        public async Task LandAsync(IEnumerable<ICopter> copters = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters, out copterId, out batchPacket);
+
+            byte[] packet = SetModeAsync(FlightMode.LAND);
+            await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+        }
+
+        /// <summary>
+        /// 加锁
+        /// </summary>
+        /// <param name="armit"></param>
+        /// <returns></returns>
+        public async Task LockAsync(IEnumerable<ICopter> copters = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters, out copterId, out batchPacket);
+
+            byte[] packet = DoARMAsync(false);
+            await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+        }
+
+        /// <summary>
+        /// 测试电机
+        /// </summary>
+        /// <param name="motor"></param>
+        /// <returns></returns>
+        public async Task MotorTestAsync(int motor, IEnumerable<ICopter> copters = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters, out copterId, out batchPacket);
+
+            byte[] packet = DoMotorTestAsync(motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
+            await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket);
+        }
+
+        /// <summary>
+        /// 悬停
+        /// </summary>
+        /// <returns></returns>
+        public async Task HoverAsync(IEnumerable<ICopter> copters = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters, out copterId, out batchPacket);
+
+            await SetChannelsAsync(copters, 1500, 1500, 1500, 1500);
+
+            byte[] packet = SetModeAsync(FlightMode.LOITER);
+            await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+        }
+
+        /// <summary>
+        /// 手动
+        /// </summary>
+        /// <param name="copters"></param>
+        /// <returns></returns>
+        public async Task FloatAsync(IEnumerable<ICopter> copters = null)
+        {
+            short copterId = 0;
+            byte[] batchPacket = null;
+            GetCopterIds(copters, out copterId, out batchPacket);
+
+            byte[] packet = SetModeAsync(FlightMode.ALT_HOLD);
+            await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+        }
+
+        private byte[] SetModeAsync(FlightMode mode)
+        {
+            return SetModeAsync(mode.ToAC2Mode());
+        }
+
+        private byte[] DoARMAsync(bool armit)
+        {
+            return DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0);
+        }
+
+        private byte[] DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout)
+        {
+            return DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0);
+        }
+
+        private byte[] DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
+        {
+            MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();
+
+            req.target_system = 1;
+            req.target_component = 1;
+            if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM)
+            {
+                req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
+            }
+            req.command = (ushort)actionid;
+            req.param1 = p1;
+            req.param2 = p2;
+            req.param3 = p3;
+            req.param4 = p4;
+            req.param5 = p5;
+            req.param6 = p6;
+            req.param7 = p7;
+            
+            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); ;
+        }
+
+        private byte[] GeneratePacket<TMavlinkPacket>(byte messageType, TMavlinkPacket indata)
+        {
+            byte[] data;
+            data = MavlinkUtil.StructureToByteArray(indata);
+            byte[] packet = new byte[data.Length + 6 + 2];
+            packet[0] = MAVLink.MAVLINK_STX;
+            packet[1] = (byte)(data.Length);
+            packet[2] = (byte)packetcount;
+            packetcount++;
+            packet[3] = 255; // this is always 255 - MYGCS
+            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
+            packet[5] = messageType;
+
+            int i = 6;
+            foreach (byte b in data)
+            {
+                packet[i] = b;
+                i++;
+            }
+            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
+            checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
+
+            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
+            byte ck_b = (byte)(checksum >> 8); ///< Low byte
+            packet[i] = ck_a;
+            i += 1;
+            packet[i] = ck_b;
+            i += 1;
+
+            return packet;
+        }
+
+
+        private byte[] SetModeAsync(ac2modes modein)
+        {
+            try
+            {
+                MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();
+
+                if (TranslateMode(modein, ref mode))
+                {
+                    return SetModeAsync(mode);
+                }
+            }
+            catch { }
+            return null;
+        }
+
+        private byte[] SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0)
+        {
+            mode.base_mode |= (byte)base_mode;
+            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
+        }
+
+        private bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode)
+        {
+            mode.target_system = 1;
+            switch (modein)
+            {
+                case ac2modes.STABILIZE:
+                case ac2modes.AUTO:
+                case ac2modes.RTL:
+                case ac2modes.LOITER:
+                case ac2modes.ACRO:
+                case ac2modes.ALT_HOLD:
+                case ac2modes.CIRCLE:
+                case ac2modes.POSITION:
+                case ac2modes.LAND:
+                case ac2modes.OF_LOITER:
+                case ac2modes.GUIDED:
+                    mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
+                    mode.custom_mode = (uint)modein;
+                    break;
+                default:
+                    return false;
+            }
+            return true;
+        }
+
+
+        /// <summary>
+        ///广播Rtk
+        /// </summary>
+        /// <param name="data"></param>
+        /// <param name="length"></param>
+        /// <returns></returns>
+        public async Task InjectGpsDataAsync(byte[] data, ushort length)
+        {
+            MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
+            var msglen = 110;
+            int datalen = 0;
+            var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;
+            for (int a = 0; a < len; a++)
+            {
+                datalen = Math.Min(length - a * msglen, msglen);
+                gps.data = new byte[msglen];
+                Array.Copy(data, a * msglen, gps.data, 0, datalen);
+                gps.len = (byte)datalen;
+                gps.target_component = 1;
+                gps.target_system = 1;
+                byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
+                await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
+            }
+        }
+
+        public byte[] GenerateRTKPacketAsync<TMavlinkPacket>(byte messageType, TMavlinkPacket indata, int rtklength)
+        {
+            byte[] data;
+            int rtkdataleng = rtklength + 3;
+
+            data = MavlinkUtil.StructureToByteArray(indata);
+
+            byte[] packet = new byte[rtkdataleng + 6 + 2];
+
+            packet[0] = MAVLink.MAVLINK_STX;
+            packet[1] = (byte)(rtkdataleng);
+            packet[2] = (byte)packetcount;
+            packetcount++;
+            packet[3] = 255; // this is always 255 - MYGCS
+            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
+            packet[5] = messageType;
+
+            int i = 6;
+            foreach (byte b in data)
+            {
+                if (i < rtkdataleng + 6)
+                    packet[i] = b;
+                else break;
+                i++;
+            }
+
+            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
+
+            checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
+
+            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
+            byte ck_b = (byte)(checksum >> 8); ///< Low byte
+
+            packet[i] = ck_a;
+            i += 1;
+            packet[i] = ck_b;
+            i += 1;
+
+            return packet;
+        }
+
+        
+        private byte[] SetParam2Async(string paramname, float value)
+        {
+            // param type is set here, however it is always sent over the air as a float 100int = 100f.
+            var req = new MAVLink.mavlink_param_set_t { target_system = 1, target_component = 1, param_type = 4 };
+
+            byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname);
+
+            Array.Resize(ref temp, 16);
+            req.param_id = temp;
+            req.param_value = (value);
+
+            return GeneratePacket((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req);
+        }
+
+        private byte[] DoLEDCommandAsync()
+        {
+            MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
+            led.target_system = 1;
+            led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
+            led.instance = 0;
+            led.pattern = 0;
+            led.custom_len = 4;
+            led.custom_bytes = new byte[24];
+            led.custom_bytes[0] = 255;
+            led.custom_bytes[1] = 255;
+            led.custom_bytes[2] = 255;
+            led.custom_bytes[3] = 3;
+
+            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
+        }
+
+        private byte[] BatchCopterIdData(IEnumerable<ICopter> copters)
+        {
+            byte[] BatchByte = null;
+            List<ICopter> copterList = copters.OrderBy(c=>int.Parse(c.Id)).ToList();
+            for (int i = 0; i < copterList.Count; i++)
+            {
+                byte[] tempByte;
+                int id = int.Parse(copterList[i].Id);
+                int count = 1;
+                while(i+1 < copterList.Count && int.Parse(copterList[i + 1].Id) == id + count)
+                {
+                    count++;
+                    i++;
+                }
+
+                if (count == 1)
+                {
+                    short curId = (short)(0 << 12 ^ id);
+                    tempByte = BitConverter.GetBytes(curId);
+                }
+                else
+                {
+                    short beginId = (short)(2 << 12 ^ id);
+                    tempByte = BitConverter.GetBytes(beginId);
+
+                    short numCount = (short)(4 << 12 ^ count);
+                    tempByte = tempByte.Concat(BitConverter.GetBytes(numCount)).ToArray();
+                }
+
+                if (BatchByte == null)
+                    BatchByte = tempByte;
+                else
+                    BatchByte = BatchByte.Concat(tempByte).ToArray();
+            }
+            return BatchByte;
+        }
+
+        private void GetCopterIds(IEnumerable<ICopter> copters, out short copterId, out byte[] batchPacket)
+        {
+            copterId = 0;
+            batchPacket = null;
+
+            if (copters != null)
+            {
+                if (copters.Count() == 1)
+                    copterId = short.Parse(copters.FirstOrDefault().Id);
+                else
+                {
+                    batchPacket = BatchCopterIdData(copters);
+                }
+            }
+        }
+    }
+}

PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs

@@ -0,0 +1,195 @@
+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)
+        {
+            string msg = System.Text.Encoding.Default.GetString(buffer).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;
+            }
+        }
+
+        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 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;
+            }
+
+        }
+    }
+}

PlaneGcsSdk_Shared/CommunicationManagement/CommWriteMissinState.cs

@@ -0,0 +1,20 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace Plane.CommunicationManagement
+{
+    public class CommWriteMissinState
+    {
+        public CommWriteMissinState(bool sendAchieved)
+        {
+            this.SendAchieved = sendAchieved;
+        }
+        public int StateReturn = 0;
+        public int ErrorCode = 0;
+        public bool SendAchieved = false;
+        public bool WriteSucceed = false;
+
+        public bool IsFailed { get { return StateReturn != 0 || ErrorCode != 0 || !SendAchieved || !WriteSucceed; } }
+    }
+}

PlaneGcsSdk_Shared/CommunicationManagement/CommunicationReceiveCopterInfoEventArgs.cs

@@ -0,0 +1,40 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace Plane.CommunicationManagement
+{
+    public class CommunicationReceiveCopterInfoEventArgs
+    {
+        public CommunicationReceiveCopterInfoEventArgs(int id, byte[] package, byte commModuleVersion)
+        {
+            this.Id = id;
+            this.Package = package;
+            this.CommModuleVersion = commModuleVersion;
+        }
+
+        public int Id { get; private set; }
+        public byte[] Package { get; private set; }
+        public byte CommModuleVersion { get; private set; }
+    }
+
+    public class CommunicationConnectEventArgs
+    {
+        public CommunicationConnectEventArgs(int id)
+        {
+            this.Id = id;
+        }
+
+        public int Id { get; private set; }
+    }
+
+    public class CommunicationCopterDisconnectEventArgs
+    { 
+        public CommunicationCopterDisconnectEventArgs(int id)
+        {
+            this.Id = id;
+        }
+
+        public int Id { get; private set; }
+    }
+}

PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs

@@ -0,0 +1,148 @@
+using System;
+using System.Collections.Generic;
+using System.Runtime.InteropServices;
+using System.Text;
+
+namespace Plane.Protocols
+{
+    /*
+     * 
+     * 通信模块协议
+     * 
+     */
+    public class MavComm
+    {
+        /// <summary>
+        /// 发送数据时的数据包头
+        /// </summary>
+        public const ushort COMM_SEND_PACKET_HEADER = 0x9551;
+        /// <summary>
+        /// 接受数据时的数据包头
+        /// </summary>
+        public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713;
+
+
+        #region 命令类型 2字节
+        //-----------------命令类型-----------------
+        /// <summary>
+        /// 查询状态
+        /// </summary>
+        public const short COMM_QUERY_STATUS = 0x00;
+
+        /// <summary>
+        /// 变更飞机总数及参与者
+        /// </summary>
+        public const short COMM_SET_MAV_COUNT = 0x10;
+
+        /// <summary>
+        /// 获取飞机详细信息
+        /// </summary>
+        public const short COMM_GET_COPTERS_COUNT = 0x20;
+
+        /// <summary>
+        /// 时间同步，不改变当前模式
+        /// </summary>
+        public const short COMM_ASYN_TIME = 0x30;
+
+        /// <summary>
+        /// 进入空闲模式
+        /// </summary>
+        public const short COMM_IDLE_MODE = 0x50;
+
+        /// <summary>
+        /// 进入搜索模式，命名编号targetID  memcpy(pdata,input,2);
+        /// </summary>
+        public const short COMM_SEARCH_MODE = 0x51;
+
+        /// <summary>
+        /// 进入航点下载模式 (写航点)
+        /// </summary>
+        public const short COMM_DOWNLOAD_MODE = 0x52;
+
+        /// <summary>
+        /// 下载模式通信
+        /// </summary>
+        public const short COMM_DOWNLOAD_COMM = 0x53;
+
+        /// <summary>
+        /// 进入飞行模式（无负载）
+        /// </summary>
+        public const short COMM_FLIGHT_MODE = 0x54;
+
+        /// <summary>
+        /// 进入飞行模式（有负载数据）
+        /// </summary>
+        public const short COMM_FLIGHT_LOAD_MODE = 0x55;
+
+        /// <summary>
+        /// 通信模块
+        /// </summary>
+        public const short COMM_NOTIFICATION = 0x1234;
+
+        #endregion 
+
+        public enum CommMode
+        {
+            IDLE = 0,
+            SEARCH = 1,
+            DOWNLOAD = 3,
+            FLIGHT = 4
+        }
+
+        public enum MessageType
+        {
+            STR = 0,
+            SCAN2 = 2,
+            SCAN3 = 3
+        }
+
+        //search 搜索模式相关
+        public const ushort SEARCH_COMPLETED = 0x0502;                  //搜索模式 扫描成功
+        public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503;       //搜索模式 跳频表更新成功
+        public const ushort SEARCH_FINDCOPTER = 0x0401;                 //搜索模式 找到指定目标
+        public const ushort FREQUENCY_HOPPING_ERROR = 0x0100;           //搜索模式 跳频表更新错误
+        public const ushort SEARCH_OUTTIME = 0xFFFF;                    //搜索模式 没有人应答   超时时间20S
+
+
+        public const ushort MISSION_SEND_COMPLETED = 0x0503;            //航点传输到飞控成功，可开始传输下个飞机航点信息, 
+                                                                        //需要再后续等待一个成功消息，才算完成   
+
+        public const ushort P2P_SEND_FAILED = 0x0600;                   //点对点通信发送失败 
+
+        public const ushort ERROR_CODE_START = 0x0100;
+        public const ushort ERROR_CODE_END = 0x03FF;
+
+
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        public struct comm_set_mav_count
+        {
+            public Int16 mav_count;    //飞机总数
+        };
+
+        
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
+        public struct comm_asyn_time
+        {
+            public Int64 time_stamp;
+        };
+
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
+        public struct comm_copter_info
+        {
+            public Int32 control_mode;
+            public UInt32 lat;
+            public UInt32 lng;
+            public Int32 retain;
+            public Int32 alt;
+
+            public Int16 gps_status;
+            
+            public byte lock_status;
+            public byte gps_num_sats;
+            public Int16 battery_voltage;
+            public Int16 yaw;
+
+        };
+
+    }
+}