Plane.Sdk3/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.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;
using System.Windows.Media;

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)
            {
                Message.Connect(true);
                SendQuery();
                await StartReadingPacketsAsync();
            }
            else
            {
                Message.Connect(false);
                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()
        {
            //Message.Show($"正在重新连接...");
            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);
        }

        public async Task UpdateCommModule()
        {
            MavComm.comm_update_copter_module commUpdate = new MavComm.comm_update_copter_module();
            commUpdate.mav_count = (short)CommModuleCopterCount;
            commUpdate.update_code = 0x7713;
            await GenerateDataAsync(0, MavComm.COMM_UPDATE_COPTER_MODULE, commUpdate);
        }

        bool temp = false;
        //测试用 灯光间隔1S闪烁
        public async Task TestLED(short id, string colorString)
        {
            
            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];
      
            if (colorString == "") colorString = "330033";

            Color color = (Color)ColorConverter.ConvertFromString("#" + colorString);

            led.custom_bytes[0] = color.R;
            led.custom_bytes[1] = color.G;
            led.custom_bytes[2] = color.B;
            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();
        }
    }
}