CyberX8/CyberX8_RT/Devices/FinsPlc.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
using System.Xml;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.IOCore;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.PLC;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using MECF.Framework.Common.IOCore;
using MECF.Framework.RT.Core.IoProviders;

namespace CyberX8_RT.Devices
{
    public enum BlockType
    {
        CIO = 0x30,
        WR = 0x31,
        D = 0x82,
    }
 

    public class FinsPlc :IoProvider
    {
 
        TcpClient msender;
        Socket msock;
        byte[] Client, Server;

        private bool _isOpened = false;

        private string _ip = "192.168.10.10";
        private int _port = 9600;

        private int _aoBlockStartPosition = 1000;
        private int _aiBlockStartPosition = 2000;
        private int _doBlockStartPosition = 0;
        private int _diBlockStartPosition = 20;

        private BlockType _diBlockType = BlockType.WR;
        private BlockType _doBlockType = BlockType.WR;

        private BlockType _aiBlockType = BlockType.D;
        private BlockType _aoBlockType = BlockType.D;

        R_TRIG _failedTrigger = new R_TRIG();

        DeviceTimer _timerWrite = new DeviceTimer();
        DeviceTimer _timerRead = new DeviceTimer();

        private double _averageWriteTime = 0;
        private double _averageReadTime = 0;
 
        public FinsPlc()
        {

        }
 

        protected override void SetParameter(XmlElement nodeParameter)
        {
            string strIp = nodeParameter.GetAttribute("ip");
            string strPort = nodeParameter.GetAttribute("port");

            string diBlockType = nodeParameter.GetAttribute("diBlockType");
            string doBlockType = nodeParameter.GetAttribute("doBlockType");
            string aiBlockType = nodeParameter.GetAttribute("aiBlockType");
            string aoBlockType = nodeParameter.GetAttribute("aoBlockType");

            string diStartPosition = nodeParameter.GetAttribute("diStartPosition");
            string doStartPosition = nodeParameter.GetAttribute("doStartPosition");

            string aiStartPosition = nodeParameter.GetAttribute("aiStartPosition");
            string aoStartPosition = nodeParameter.GetAttribute("aoStartPosition");

            _port = int.Parse(strPort);
            _ip = strIp;

            if (!Enum.TryParse(diBlockType, out _diBlockType))
            {
                //LOG.Error($"plc config error, block type {diBlockType} not valid");
            }
            if (!Enum.TryParse(doBlockType, out _doBlockType))
            {
                //LOG.Error($"plc config error, block type {doBlockType} not valid");
            }
            if (!Enum.TryParse(aiBlockType, out _aiBlockType))
            {
                //LOG.Error($"plc config error, block type {aiBlockType} not valid");
            }
            if (!Enum.TryParse(aoBlockType, out _aoBlockType))
            {
                //LOG.Error($"plc config error, block type {aoBlockType} not valid");
            }

            if (!int.TryParse(diStartPosition, out _diBlockStartPosition))
            {
                //LOG.Error($"plc config error, start position {diStartPosition} not valid");
            }
            if (!int.TryParse(doStartPosition, out _doBlockStartPosition))
            {
                //LOG.Error($"plc config error, start position {doStartPosition} not valid");
            }
            if (!int.TryParse(aiStartPosition, out _aiBlockStartPosition))
            {
                //LOG.Error($"plc config error, start position {aiStartPosition} not valid");
            }
            if (!int.TryParse(aoStartPosition, out _aoBlockStartPosition))
            {
                //LOG.Error($"plc config error, start position {aoStartPosition} not valid");
            }
        }
        public override void Initialize(string module, string name, List<IoBlockItem> lstBuffers, IIoBuffer buffer, XmlElement nodeParameter, string ioMappingPathFile, string ioModule)
        {
            Module = module;
            Name = name;

            _source = module + "." + name;

            _buffer = buffer;

            _nodeParameter = nodeParameter;

            _blockSections = lstBuffers;

            buffer.SetBufferBlock(_source, lstBuffers);

            SetParameter(nodeParameter);

            SetIoMap(_source, 0, ioMappingPathFile, ioModule);

            State = IoProviderStateEnum.Uninitialized;

            _thread = new PeriodicJob(50, OnTimer, name);
        }

        private int GetIndex(string addr, int first)
        {
            if (String.IsNullOrEmpty(addr))
            {
                //LOG.Write("GetIndex addr is empty");
                return -1;
            }

            string[] parts = addr.Trim().ToUpper().Split('.');

            int len = parts.Length;

            if (len == 1)
            {
                string ch = parts[0].TrimStart('D');
                int index = Convert.ToUInt16(ch);

                return (index - first) ;
            }

            if (len == 2)
            {
                char[] trim = { 'W', 'C', 'I', 'O', ' ' };
                string ch = parts[0].TrimStart(trim);

                int index = Convert.ToUInt16(ch);

                int bit = Convert.ToUInt16(parts[1]);
                return (index - first) * 16 + bit;
            }

            //LOG.Info("IOManager GetIndex error");

            return -1;
        }
        private void SetIoMap(string provider, int blockOffset, string xmlPathFile, string module = "")
        {
 
            XmlDocument xml = new XmlDocument();
            xml.Load(xmlPathFile);

            XmlNodeList lstDi = xml.SelectNodes("IO_DEFINE/Dig_In/DI_ITEM");

            var scConfig = SC.GetConfigItem("System.IsIgnoreSaveDB");
            var isIgnoreSaveDB = scConfig == null ? false : scConfig.BoolValue;

            //<DI_ITEM Index="0" Name="" BufferOffset="0" Addr="0" Description=""/>

            var dibuffer= _buffer.GetDiBuffer(provider);
            List<DIAccessor> diList = new List<DIAccessor>();
            foreach (var diItem in lstDi)
            {
                XmlElement element = diItem as XmlElement;
                if (element == null)
                    continue;

                string index = element.GetAttribute("Index");
                string name = element.GetAttribute("Name");
                string address = element.GetAttribute("Addr");
                string description = element.GetAttribute("Description");
                if (string.IsNullOrEmpty(name) || string.IsNullOrEmpty(index) || string.IsNullOrEmpty(address))
                    continue;

                name = name.Trim();
                index = index.Trim();
                string moduleName = string.IsNullOrEmpty(module) ? name : $"{module}.{name}";

                int intIndex;
                if (!int.TryParse(index, out intIndex))
                    continue;

                int intBufferOffset = GetIndex(address, _diBlockStartPosition);
                if (intBufferOffset == -1)
                {
                    throw new Exception($"address not valid, " + provider);
                }

                if (!dibuffer.ContainsKey(blockOffset))
                {
                    throw new Exception("Not defined DI buffer from IO provider, " + provider);
                }

                DIAccessor diAccessor = new DIAccessor(moduleName, intBufferOffset, dibuffer[blockOffset], dibuffer[blockOffset]);

                diAccessor.IoTableIndex = intIndex;
                diAccessor.Addr = address;
                diAccessor.Provider = provider;
                diAccessor.BlockOffset = blockOffset;
                diAccessor.Description = description;

                diList.Add(diAccessor);
            }
            _buffer.SetIoMap(provider, blockOffset, diList);

            XmlNodeList lstDo = xml.SelectNodes("IO_DEFINE/Dig_Out/DO_ITEM");
            var dobuffer = _buffer.GetDoBuffer(provider);
            List<DOAccessor> doList = new List<DOAccessor>();
            foreach (var doItem in lstDo)
            {
                XmlElement element = doItem as XmlElement;
                if (element == null)
                    continue;

                string index = element.GetAttribute("Index");
 
                string name = element.GetAttribute("Name");
                string address = element.GetAttribute("Addr");
                string description = element.GetAttribute("Description");
                if (string.IsNullOrEmpty(name) || string.IsNullOrEmpty(index) || string.IsNullOrEmpty(address))
                    continue;

                name = name.Trim();
                index = index.Trim();
 
                string moduleName = string.IsNullOrEmpty(module) ? name : $"{module}.{name}";

                int intIndex;
                if (!int.TryParse(index, out intIndex))
                    continue;

                int intBufferOffset = GetIndex(address, _doBlockStartPosition);
                if (intBufferOffset == -1)
                {
                    throw new Exception($"address not valid, " + provider);
                }

                if (!dobuffer.ContainsKey(blockOffset))
                {
                    throw new Exception("Not defined DO buffer from IO provider, " + provider);
                }

                DOAccessor doAccessor = new DOAccessor(moduleName, intBufferOffset, dobuffer[blockOffset]);
                
                doAccessor.IoTableIndex = intIndex;
                doAccessor.Addr = address;
                doAccessor.Provider = provider;
                doAccessor.BlockOffset = blockOffset;
                doAccessor.Description = description;

                doList.Add(doAccessor);
            }
            _buffer.SetIoMap(provider, blockOffset, doList);

            XmlNodeList lstAo = xml.SelectNodes("IO_DEFINE/Ana_Out/AO_ITEM");
            var aobuffer = _buffer.GetAoBuffer(provider);
            List<AOAccessor> aoList = new List<AOAccessor>();
            foreach (var aoItem in lstAo)
            {
                XmlElement element = aoItem as XmlElement;
                if (element == null)
                    continue;

                string index = element.GetAttribute("Index");
 
                string name = element.GetAttribute("Name");
                string address = element.GetAttribute("Addr");
                string description = element.GetAttribute("Description");
                if (string.IsNullOrEmpty(name) || string.IsNullOrEmpty(index) || string.IsNullOrEmpty(address))
                    continue;

                name = name.Trim();
                index = index.Trim();
 
                string moduleName = string.IsNullOrEmpty(module) ? name : $"{module}.{name}";

                int intIndex;
                if (!int.TryParse(index, out intIndex))
                    continue;

                int intBufferOffset = GetIndex(address, _aoBlockStartPosition);
                if (intBufferOffset == -1)
                {
                    throw new Exception($"address not valid, " + provider);
                }

                if (!aobuffer.ContainsKey(blockOffset))
                {
                    throw new Exception("Not defined AO buffer from IO provider, " + provider);
                }

                AOAccessor aoAccessor = new AOAccessor(moduleName, intBufferOffset, aobuffer[blockOffset]);
                aoAccessor.IoTableIndex = intIndex;
                aoAccessor.Addr = address;
                aoAccessor.Provider = provider;
                aoAccessor.BlockOffset = blockOffset;
                aoAccessor.Description = description;

                aoList.Add(aoAccessor);
            }
            _buffer.SetIoMap(provider, blockOffset, aoList);

            XmlNodeList lstAi = xml.SelectNodes("IO_DEFINE/Ana_In/AI_ITEM");
            var aibuffer = _buffer.GetAiBuffer(provider);
            List<AIAccessor> aiList = new List<AIAccessor>();
            foreach (var aiItem in lstAi)
            {
                XmlElement element = aiItem as XmlElement;
                if (element == null)
                    continue;

                string index = element.GetAttribute("Index");
 
                string name = element.GetAttribute("Name");
                string address = element.GetAttribute("Addr");
                string description = element.GetAttribute("Description");
                if (string.IsNullOrEmpty(name) || string.IsNullOrEmpty(index) || string.IsNullOrEmpty(address))
                    continue;

                name = name.Trim();
                index = index.Trim();
 
                string moduleName = string.IsNullOrEmpty(module) ? name : $"{module}.{name}";

                int intIndex;
                if (!int.TryParse(index, out intIndex))
                    continue;

                int intBufferOffset = GetIndex(address, _aiBlockStartPosition);
                if (intBufferOffset == -1)
                {
                    throw new Exception($"address not valid, " + provider);
                }

                if (!aibuffer.ContainsKey(blockOffset))
                {
                    throw new Exception("Not defined AI buffer from IO provider, " + provider);
                }

                AIAccessor aiAccessor = new AIAccessor(moduleName, intBufferOffset, aibuffer[blockOffset]);
                 
                aiAccessor.IoTableIndex = intIndex;
                aiAccessor.Addr = address;
                aiAccessor.Provider = provider;
                aiAccessor.BlockOffset = blockOffset;
                aiAccessor.Description = description;

                aiList.Add(aiAccessor);
            }
            _buffer.SetIoMap(provider, blockOffset, aiList);
        }


        protected override void Open()
        {
            try
            {
                //LOG.Write(String.Format("试图连接PLC {0}:{1}", _ip, _port));

                Close();

                _isOpened = false;

                msender = new TcpClient(_ip, _port);
                msock = msender.Client;
                msock.Send(HandShake());
                byte[] buffer = new byte[24];
                msock.Receive(buffer, SocketFlags.None);

                {
                    Client = new byte[4];
                    Client[0] = buffer[16];
                    Client[1] = buffer[17];
                    Client[2] = buffer[18];
                    Client[3] = buffer[19];
                    Server = new byte[4];
                    Server[0] = buffer[20];
                    Server[1] = buffer[21];
                    Server[2] = buffer[22];
                    Server[3] = buffer[23];

                }
                _isOpened = true;
                SetState(IoProviderStateEnum.Opened);
            }
            catch (Exception ex)
            {
                _failedTrigger.CLK = true;

                if (_failedTrigger.Q)
                {
                    LOG.WriteExeption(String.Format("Communication failed with PLC {0}:{1}", _ip, _port), ex);
                }

                return  ;
            }

            _failedTrigger.RST = true;

            _isOpened = true;

            return  ;
        }

        protected override  void Close()
        {
            try
            {
                if (_isOpened)
                {
                    _isOpened = false;
                    msender.Close();
                }

            }
            catch (Exception ex)
            {
                LOG.WriteExeption(ex);
            }
        }

        protected override bool[] ReadDi(int offset, int size)
        {
            try
            {
                byte[] data = new byte[200];

                double interval = _timerRead.GetElapseTime();
                if (interval > _averageWriteTime)
                {
                    //LOG.Write(_ip + "：Max read PLC interval : " + interval);
                    if (_averageReadTime < 0.1)
                        _averageReadTime = interval;
                    _averageReadTime = (interval + _averageReadTime) / 2;
                }
                ReadBlock<byte>(_diBlockType, (ushort)_diBlockStartPosition, (ushort)data.Length, ref data);
 
                _timerRead.Start(0);
                return Array.ConvertAll(data, x => x == 1);
            }
            catch (Exception ex)
            {
                LOG.WriteExeption(String.Format("PLC ({0}) Read exception.", _ip), ex);

                Close();
            }
            return null;
        }

        protected override short[] ReadAi(int offset, int size)
        {
            try
            {
                short[] data = new short[200];

                double interval = _timerRead.GetElapseTime();
                if (interval > _averageWriteTime)
                {
                    //LOG.Write(_ip + "：Max read PLC interval : " + interval);
                    if (_averageReadTime < 0.1)
                        _averageReadTime = interval;
                    _averageReadTime = (interval + _averageReadTime) / 2;
                }
                ReadBlock_short(_aiBlockType, (ushort)_aiBlockStartPosition, (ushort)data.Length, ref data);

                _timerRead.Start(0);
                return data;
            }
            catch (Exception ex)
            {
                LOG.WriteExeption(String.Format("PLC ({0}) Read exception.", _ip), ex);

                Close();
            }
            return null;
        }

        protected override void WriteDo(int offset, bool[] buffer)
        {
            try
            {
                _timerWrite.Start(0);
 
                WriteBlock_Byte(_doBlockType, (ushort)_doBlockStartPosition, (ushort)buffer.Length, Array.ConvertAll(buffer, x=>x?(byte)1: (byte)0));
 
                double interval = _timerWrite.GetElapseTime();
                if (interval > _averageWriteTime)
                {
                    //LOG.Write(_ip + "：Max write PLC interval : " + interval);
                    if (_averageWriteTime < 0.1)
                        _averageWriteTime = interval;
                    _averageWriteTime = (_averageWriteTime + interval) / 2;
                }
                return  ;
            }
            catch (Exception ex)
            {
                LOG.WriteExeption(String.Format("PLC ({0})Write exception.", _ip), ex);
                Close();
                return  ;
            }
        }

        protected override void WriteAo(int offset, short[] buffer)
        {
            try
            {
                _timerWrite.Start(0);
 
                WriteBlock_short(_aoBlockType, (ushort)_aoBlockStartPosition, (ushort)buffer.Length, buffer);

                double interval = _timerWrite.GetElapseTime();



                if (interval > _averageWriteTime)
                {
                    //LOG.Write(_ip + "：Max write PLC interval : " + interval);
                    if (_averageWriteTime < 0.1)
                        _averageWriteTime = interval;
                    _averageWriteTime = (_averageWriteTime + interval) / 2;
                }
                return  ;
            }
            catch (Exception ex)
            {
                LOG.WriteExeption(String.Format("PLC ({0})Write exception.", _ip), ex);
                Close();
                return  ;
            }
        }

 
        private void ReadBlock<T>(BlockType type, ushort addr, ushort len, ref T[] data)
        {
            byte[] FullCmd = new byte[34];
            //TCP FINS header
            FullCmd[0] = 0x46;//F
            FullCmd[1] = 0x49;//I
            FullCmd[2] = 0x4e;//N
            FullCmd[3] = 0x53;//S
            FullCmd[4] = 0;//cmd length
            FullCmd[5] = 0;
            FullCmd[6] = 0;
            FullCmd[7] = 0x1A;
            FullCmd[8] = 0;//frame command
            FullCmd[9] = 0;
            FullCmd[10] = 0;
            FullCmd[11] = 0x02;
            FullCmd[12] = 0;//err
            FullCmd[13] = 0;
            FullCmd[14] = 0;
            FullCmd[15] = 0;

            //command frame header
            FullCmd[16] = 0x80;//ICF
            FullCmd[17] = 0x00;//RSV
            FullCmd[18] = 0x02;//GCT, less than 8 network layers
            FullCmd[19] = 0x00;//DNA, local network
            FullCmd[20] = Server[3];//DA1
            FullCmd[21] = 0x00;//DA2, CPU unit
            FullCmd[22] = 0x00;//SNA, local network
            FullCmd[23] = Client[3];//SA1
            FullCmd[24] = 0x00;//SA2, CPU unit
            FullCmd[25] = Convert.ToByte(21);//SID

            FullCmd[26] = 0x01;   //FINS CMD Main Request Code    0101 Read IO Area
            FullCmd[27] = 0x01;   //Sub Request Code

            //Parameter
            FullCmd[28] = (byte)type;

            FullCmd[29] = (byte)((addr >> 8) & 0xFF);    //Block Add
            FullCmd[30] = (byte)(addr & 0xFF);
            FullCmd[31] = 0;    //Bit Add

            FullCmd[32] = (byte)((len >> 8) & 0xFF); ;    //Read Count
            FullCmd[33] = (byte)(len & 0xFF);

            msock.Send(FullCmd, SocketFlags.None);
            byte[] buffer = new byte[len + 30];
            msock.Receive(buffer);
            bool Succeed = true;
            if (buffer[11] == 3)
                Succeed = CheckHeadError(buffer[15]);
            if (Succeed)//no header error
            {
                T[] buf = new T[len];
                Array.Copy(buffer, 30, buf, 0, len);

                if (CheckEndCode(buffer[28], buffer[29]))
                {
                    for (int i = 0; i < len; i++)
                    {
                        data[i] = buf[i];
                    }
                }
            }
        }

        private void ReadBlock_short(BlockType type, ushort addr, ushort len, ref short[] data)
        {
            byte[] FullCmd = new byte[34];
            //TCP FINS header
            FullCmd[0] = 0x46;//F
            FullCmd[1] = 0x49;//I
            FullCmd[2] = 0x4e;//N
            FullCmd[3] = 0x53;//S
            FullCmd[4] = 0;//cmd length
            FullCmd[5] = 0;
            FullCmd[6] = 0;
            FullCmd[7] = 0x1A;
            FullCmd[8] = 0;//frame command
            FullCmd[9] = 0;
            FullCmd[10] = 0;
            FullCmd[11] = 0x02;
            FullCmd[12] = 0;//err
            FullCmd[13] = 0;
            FullCmd[14] = 0;
            FullCmd[15] = 0;

            //command frame header
            FullCmd[16] = 0x80;//ICF
            FullCmd[17] = 0x00;//RSV
            FullCmd[18] = 0x02;//GCT, less than 8 network layers
            FullCmd[19] = 0x00;//DNA, local network
            FullCmd[20] = Server[3];//DA1
            FullCmd[21] = 0x00;//DA2, CPU unit
            FullCmd[22] = 0x00;//SNA, local network
            FullCmd[23] = Client[3];//SA1
            FullCmd[24] = 0x00;//SA2, CPU unit
            FullCmd[25] = Convert.ToByte(21);//SID

            FullCmd[26] = 0x01;   //FINS CMD Main Request Code    0101 Read IO Area
            FullCmd[27] = 0x01;   //Sub Request Code

            //Parameter
            FullCmd[28] = (byte)type;

            FullCmd[29] = (byte)((addr >> 8) & 0xFF);    //Block Add
            FullCmd[30] = (byte)(addr & 0xFF);
            FullCmd[31] = 0;    //Bit Add

            int count = len  ;

            FullCmd[32] = (byte)((count >> 8) & 0xFF); ;    //Read Count
            FullCmd[33] = (byte)(count & 0xFF);

            msock.Send(FullCmd, SocketFlags.None);
            byte[] buffer = new byte[31 + len * 2];
            msock.Receive(buffer);
            bool Succeed = true;
            if (buffer[11] == 3)
                Succeed = CheckHeadError(buffer[15]);
            if (Succeed)//no header error
            {

                if (CheckEndCode(buffer[28], buffer[29]))
                {
                    byte[] buf = new byte[2];

                    for (int i = 0; i < len; i++)
                    {

                        buf[0] = buffer[30 + i * 2 + 1];
                        buf[1] = buffer[30 + i * 2 + 0];
 
                        data[i] = BitConverter.ToInt16(buf, 0);
                    }
                }
            }
        }
 
        private void WriteBlock_Byte(BlockType type, ushort addr, ushort len, byte[] data)
        {
            byte[] FullCmd = new byte[34 + len];

            //TCP FINS header
            FullCmd[0] = 0x46;//F
            FullCmd[1] = 0x49;//I
            FullCmd[2] = 0x4e;//N
            FullCmd[3] = 0x53;//S

            int cmdLen = 26 + len;
            FullCmd[4] = 0;//cmd length
            FullCmd[5] = 0;
            FullCmd[6] = (byte)((cmdLen >> 8) & 0xFF);
            FullCmd[7] = (byte)(cmdLen & 0xFF);



            FullCmd[8] = 0;//frame command
            FullCmd[9] = 0;
            FullCmd[10] = 0;
            FullCmd[11] = 0x02;
            FullCmd[12] = 0;//err
            FullCmd[13] = 0;
            FullCmd[14] = 0;
            FullCmd[15] = 0;
            //command frame header
            FullCmd[16] = 0x80;//ICF
            FullCmd[17] = 0x00;//RSV
            FullCmd[18] = 0x02;//GCT, less than 8 network layers
            FullCmd[19] = 0x00;//DNA, local network
            FullCmd[20] = Server[3];//DA1
            FullCmd[21] = 0x00;//DA2, CPU unit
            FullCmd[22] = 0x00;//SNA, local network
            FullCmd[23] = Client[3];//SA1
            FullCmd[24] = 0x00;//SA2, CPU unit

            FullCmd[25] = Convert.ToByte(21);//SID


            FullCmd[26] = 0x01;   //FINS CMD Main Request Code    0101 Read IO Area
            FullCmd[27] = 0x02;   //Sub Request Code

            //Parameter
            FullCmd[28] = (byte)type;

            FullCmd[29] = (byte)((addr >> 8) & 0xFF);    //Block Add
            FullCmd[30] = (byte)(addr & 0xFF);
            FullCmd[31] = 0;    //Bit Add

            FullCmd[32] = (byte)((len >> 8) & 0xFF); ;    //Write Count
            FullCmd[33] = (byte)(len & 0xFF);

            for (int i = 0; i < len; i++)
            {
                FullCmd[34 + i] = data[i];
            }

            msock.Send(FullCmd, SocketFlags.None);
            byte[] buffer = new byte[len + 34];
            msock.Receive(buffer);
            bool Succeed = true;
            if (buffer[11] == 3)
                Succeed = CheckHeadError(buffer[15]);
            if (Succeed)//no header error
            {
                if (CheckEndCode(buffer[28], buffer[29]))
                {
                    //do nothing
                }
            }
        }

        private void WriteBlock_short(BlockType type, ushort addr, ushort len, short[] data)
        {

            byte[] FullCmd = new byte[34 + len * 2];

            //TCP FINS header
            FullCmd[0] = 0x46;//F
            FullCmd[1] = 0x49;//I
            FullCmd[2] = 0x4e;//N
            FullCmd[3] = 0x53;//S

            int cmdLen = 26 + len * 2;
            FullCmd[4] = 0;//cmd length
            FullCmd[5] = 0;
            FullCmd[6] = (byte)((cmdLen >> 8) & 0xFF);
            FullCmd[7] = (byte)(cmdLen & 0xFF);

            FullCmd[8] = 0;//frame command
            FullCmd[9] = 0;
            FullCmd[10] = 0;
            FullCmd[11] = 0x02;
            FullCmd[12] = 0;//err
            FullCmd[13] = 0;
            FullCmd[14] = 0;
            FullCmd[15] = 0;
            //command frame header
            FullCmd[16] = 0x80;//ICF
            FullCmd[17] = 0x00;//RSV
            FullCmd[18] = 0x02;//GCT, less than 8 network layers
            FullCmd[19] = 0x00;//DNA, local network
            FullCmd[20] = Server[3];//DA1
            FullCmd[21] = 0x00;//DA2, CPU unit
            FullCmd[22] = 0x00;//SNA, local network
            FullCmd[23] = Client[3];//SA1
            FullCmd[24] = 0x00;//SA2, CPU unit

            FullCmd[25] = Convert.ToByte(21);//SID


            FullCmd[26] = 0x01;   //FINS CMD Main Request Code    0101 Read IO Area
            FullCmd[27] = 0x02;   //Sub Request Code

            //Parameter
            FullCmd[28] = (byte)type;

            FullCmd[29] = (byte)((addr >> 8) & 0xFF);    //Block Add
            FullCmd[30] = (byte)(addr & 0xFF);
            FullCmd[31] = 0;    //Bit Add

            int count = len  ;
            FullCmd[32] = (byte)((count >> 8) & 0xFF); ;    //Write Count
            FullCmd[33] = (byte)(count & 0xFF);

            try
            {
                for (int i = 0; i < len; i++)
                {
                    byte[] buf = BitConverter.GetBytes(data[i]);

                    FullCmd[34 + i * 2 + 0] = buf[1];
                    FullCmd[34 + i * 2 + 1] = buf[0];
                }

            }
            catch (Exception ex)
            {
                LOG.WriteExeption(ex);
            }


            msock.Send(FullCmd, SocketFlags.None);
            byte[] buffer = new byte[31 + len * 2];
            msock.Receive(buffer);
            bool Succeed = true;
            if (buffer[11] == 3)
                Succeed = CheckHeadError(buffer[15]);
            if (Succeed)//no header error
            {
                if (CheckEndCode(buffer[28], buffer[29]))
                {
                    //do nothing
                }
            }
        }

 
        private byte[] HandShake()
        {
            //handshake
            byte[] Handshake = new byte[20];
            Handshake[0] = 0x46;
            Handshake[1] = 0x49;
            Handshake[2] = 0x4e;
            Handshake[3] = 0x53;

            Handshake[4] = 0;
            Handshake[5] = 0;
            Handshake[6] = 0;
            Handshake[7] = 0x0c;

            Handshake[8] = 0;
            Handshake[9] = 0;
            Handshake[10] = 0;
            Handshake[11] = 0;

            Handshake[12] = 0;
            Handshake[13] = 0;
            Handshake[14] = 0;
            Handshake[15] = 0;

            Handshake[16] = 0;
            Handshake[17] = 0;
            Handshake[18] = 0;
            Handshake[19] = 0;//ask for client and server node number, the client node will allocated automatically

            return Handshake;
        }
        /// <summary>
        /// （若返回的头指令为3）检查命令头中的错误代码
        /// </summary>
        /// <param name="Code">错误代码</param>
        /// <returns>指示程序是否可以继续进行</returns>
        bool CheckHeadError(byte Code)
        {
            switch (Code)
            {
                case 0x00: return true;
                case 0x01: RaiseException("the head is not 'FINS'"); return false;
                case 0x02: RaiseException("the data length is too long"); return false;
                case 0x03: RaiseException("the command is not supported"); return false;
            }
            //no hit
            RaiseException("unknown exception"); return false;
        }

        private void RaiseException(string p)
        {

            _isOpened = false;
        }

        /// <summary>
        /// 检查命令帧中的EndCode
        /// </summary>
        /// <param name="Main">主码</param>
        /// <param name="Sub">副码</param>
        /// <returns>指示程序是否可以继续进行</returns>
        bool CheckEndCode(byte Main, byte Sub)
        {
            switch (Main)
            {
                case 0x00:
                    switch (Sub)
                    {
                        case 0x00: return true;//the only situation of success
                        case 0x01: RaiseException("service canceled"); return false;
                    }
                    break;
                case 0x01:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("local node not in network"); return false;
                        case 0x02: RaiseException("token timeout"); return false;
                        case 0x03: RaiseException("retries failed"); return false;
                        case 0x04: RaiseException("too many send frames"); return false;
                        case 0x05: RaiseException("node address range error"); return false;
                        case 0x06: RaiseException("node address duplication"); return false;
                    }
                    break;
                case 0x02:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("destination node not in network"); return false;
                        case 0x02: RaiseException("unit missing"); return false;
                        case 0x03: RaiseException("third node missing"); return false;
                        case 0x04: RaiseException("destination node busy"); return false;
                        case 0x05: RaiseException("response timeout"); return false;
                    }
                    break;
                case 0x03:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("communications controller error"); return false;
                        case 0x02: RaiseException("CPU unit error"); return false;
                        case 0x03: RaiseException("controller error"); return false;
                        case 0x04: RaiseException("unit number error"); return false;
                    }
                    break;
                case 0x04:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("undefined command"); return false;
                        case 0x02: RaiseException("not supported by model/version"); return false;
                    }
                    break;
                case 0x05:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("destination address setting error"); return false;
                        case 0x02: RaiseException("no routing tables"); return false;
                        case 0x03: RaiseException("routing table error"); return false;
                        case 0x04: RaiseException("too many relays"); return false;
                    }
                    break;
                case 0x10:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("command too long"); return false;
                        case 0x02: RaiseException("command too short"); return false;
                        case 0x03: RaiseException("elements/data don't match"); return false;
                        case 0x04: RaiseException("command format error"); return false;
                        case 0x05: RaiseException("header error"); return false;
                    }
                    break;
                case 0x11:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("area classification missing"); return false;
                        case 0x02: RaiseException("access size error"); return false;
                        case 0x03: RaiseException("address range error"); return false;
                        case 0x04: RaiseException("address range exceeded"); return false;
                        case 0x06: RaiseException("program missing"); return false;
                        case 0x09: RaiseException("relational error"); return false;
                        case 0x0a: RaiseException("duplicate data access"); return false;
                        case 0x0b: RaiseException("response too long"); return false;
                        case 0x0c: RaiseException("parameter error"); return false;
                    }
                    break;
                case 0x20:
                    switch (Sub)
                    {
                        case 0x02: RaiseException("protected"); return false;
                        case 0x03: RaiseException("table missing"); return false;
                        case 0x04: RaiseException("data missing"); return false;
                        case 0x05: RaiseException("program missing"); return false;
                        case 0x06: RaiseException("file missing"); return false;
                        case 0x07: RaiseException("data mismatch"); return false;
                    }
                    break;
                case 0x21:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("read-only"); return false;
                        case 0x02: RaiseException("protected , cannot write data link table"); return false;
                        case 0x03: RaiseException("cannot register"); return false;
                        case 0x05: RaiseException("program missing"); return false;
                        case 0x06: RaiseException("file missing"); return false;
                        case 0x07: RaiseException("file name already exists"); return false;
                        case 0x08: RaiseException("cannot change"); return false;
                    }
                    break;
                case 0x22:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("not possible during execution"); return false;
                        case 0x02: RaiseException("not possible while running"); return false;
                        case 0x03: RaiseException("wrong PLC mode"); return false;
                        case 0x04: RaiseException("wrong PLC mode"); return false;
                        case 0x05: RaiseException("wrong PLC mode"); return false;
                        case 0x06: RaiseException("wrong PLC mode"); return false;
                        case 0x07: RaiseException("specified node not polling node"); return false;
                        case 0x08: RaiseException("step cannot be executed"); return false;
                    }
                    break;
                case 0x23:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("file device missing"); return false;
                        case 0x02: RaiseException("memory missing"); return false;
                        case 0x03: RaiseException("clock missing"); return false;
                    }
                    break;
                case 0x24:
                    switch (Sub)
                    { case 0x01: RaiseException("table missing"); return false; }
                    break;
                case 0x25:
                    switch (Sub)
                    {
                        case 0x02: RaiseException("memory error"); return false;
                        case 0x03: RaiseException("I/O setting error"); return false;
                        case 0x04: RaiseException("too many I/O points"); return false;
                        case 0x05: RaiseException("CPU bus error"); return false;
                        case 0x06: RaiseException("I/O duplication"); return false;
                        case 0x07: RaiseException("CPU bus error"); return false;
                        case 0x09: RaiseException("SYSMAC BUS/2 error"); return false;
                        case 0x0a: RaiseException("CPU bus unit error"); return false;
                        case 0x0d: RaiseException("SYSMAC BUS No. duplication"); return false;
                        case 0x0f: RaiseException("memory error"); return false;
                        case 0x10: RaiseException("SYSMAC BUS terminator missing"); return false;
                    }
                    break;
                case 0x26:
                    switch (Sub)
                    {
                        case 0x01: RaiseException("no protection"); return false;
                        case 0x02: RaiseException("incorrect password"); return false;
                        case 0x04: RaiseException("protected"); return false;
                        case 0x05: RaiseException("service already executing"); return false;
                        case 0x06: RaiseException("service stopped"); return false;
                        case 0x07: RaiseException("no execution right"); return false;
                        case 0x08: RaiseException("settings required before execution"); return false;
                        case 0x09: RaiseException("necessary items not set"); return false;
                        case 0x0a: RaiseException("number already defined"); return false;
                        case 0x0b: RaiseException("error will not clear"); return false;
                    }
                    break;
                case 0x30:
                    switch (Sub)
                    { case 0x01: RaiseException("no access right"); return false; }
                    break;
                case 0x40:
                    switch (Sub)
                    { case 0x01: RaiseException("service aborted"); return false; }
                    break;
            }
            //no hit
            RaiseException("unknown exception"); return false;
        }
 
    }
}