diff --git a/ProductionSystem/Untils/MFSerial.cs b/ProductionSystem/Untils/MFSerial.cs new file mode 100644 index 0000000..83bd75c --- /dev/null +++ b/ProductionSystem/Untils/MFSerial.cs @@ -0,0 +1,1566 @@ +using System.Net; +using System.Net.Sockets; + +namespace ConsoleApp6; +using System.IO.Ports; + + + public class MFCan_DevInfo + { + public int Send_Count; + public int Recv_Count; + public string InterfaceName; + public string ProtocolVersion; + public string Name; + public string SN; + + public MFCan_DevInfo() + { + Send_Count = Recv_Count = 0; + InterfaceName = ""; + ProtocolVersion = ""; + Name = ""; + SN = ""; + } + + } + + public class MFCan_MSG + { + public const int CAN_MSG_TYPE_SD = 0; + public const int CAN_MSG_TYPE_SR = 1; + public const int CAN_MSG_TYPE_ED = 2; + public const int CAN_MSG_TYPE_ER = 3; + + public byte Ch; + public UInt32 TimeStamp_100us; + public UInt32 TimeStamp; + public Byte Type; + public UInt32 Id; + public Byte Dlc; + public Byte[] Dat; + + public MFCan_MSG() + { + Dat = new byte[64]; + } + public override String ToString() + { + String str; + str = string.Format("Ch={0:D2},T={1:X8},", Ch, TimeStamp); + switch(Type) + { + case CAN_MSG_TYPE_SD: + str += string.Format("SD,ID={0:X3},L={1:D2},Dat=", Id, Dlc); + break; + case CAN_MSG_TYPE_SR: + str += string.Format("SR,ID={0:X3},L={1:D2},Dat=", Id, Dlc); + break; + case CAN_MSG_TYPE_ED: + str += string.Format("ED,ID={0:X8},L={1:D2},Dat=", Id, Dlc); + break; + case CAN_MSG_TYPE_ER: + str += string.Format("ER,ID={0:X8},L={1:D2},Dat=", Id, Dlc); + break; + } + for (int i = 0; i < Dlc; i++) + str += string.Format(" {0:X2}", Dat[i]); + return str; + } + } + public class MFCan_CFG + { + //old define + public const int CHECK_MODE_NORMAL = 0; + public const int CHECK_MODE_SILENT = 1; + public const int CHECK_MODE_LOOPBACK_OUT = 2; + public const int CHECK_MODE_LOOPBACK_IN = 3; + + public const int CHECK_TYPE_NOMAL = 0; + public const int CHECK_TYPE_FD_BRS = 1; + public const int CHECK_TYPE_FD_NOBRS = 2; + public const int CHECK_TYPE_FD_BRS_NONISO = 3; + public const int CHECK_TYPE_FD_NOBRS_NONISO = 4; + + //redefine + public const int MODE_NORMAL = 0; + public const int MODE_SILENT = 1; + public const int MODE_LOOPBACK_EXT = 2; + public const int MODE_LOOPBACK_IN = 3; + + public const int FORMAT_TYPE_CLASSIC = 0; + public const int FORMAT_TYPE_FD_BRS = 1; + public const int FORMAT_TYPE_FD_NOBRS = 2; + public const int FORMAT_TYPE_FD_BRS_NONISO = 3; + public const int FORMAT_TYPE_FD_NOBRS_NONISO = 4; + + + public byte Ch; + public byte Mode; + public byte Type; + public bool Ack; + public bool single; + public int NormalBps; + public int DataBps; + + public MFCan_CFG(Byte ch, int bps) + { + Ch = ch; + NormalBps = bps; + DataBps = bps; + Mode = MODE_NORMAL; + Type = FORMAT_TYPE_CLASSIC; + Ack = false; + single = false; + } + public MFCan_CFG(Byte ch, int nbps, int dbps) + { + Ch = ch; + NormalBps = nbps; + DataBps = dbps; + Mode = MODE_NORMAL; + Type = FORMAT_TYPE_FD_BRS; + Ack = false; + single = false; + } + public MFCan_CFG() + { + Ch = 0; + NormalBps = 500000; + DataBps = 500000; + Mode = MODE_NORMAL; + Type = FORMAT_TYPE_CLASSIC; + Ack = false; + single = false; + } + } + public class MFLin_DevInfo + { + public int Lin_Count; + public string InterfaceName; + public string ProtocolVersion; + public string Name; + public string SN; + + public MFLin_DevInfo() + { + Lin_Count = 0; + InterfaceName = ""; + ProtocolVersion = ""; + Name = ""; + SN = ""; + } + + } + public class MFLin_MSG + { + public UInt32 TimeStamp; + public Byte Ch; + public Byte Err; + public Byte Id; + public Byte Dlc; + public Byte[] Dat; + + public MFLin_MSG() + { + Dat = new byte[64]; + } + public override String ToString() + { + String str; + str = string.Format("Ch={0:D2},T={1:X8},ID={2:X2},Len={3:D2},ERR={4:D},Data=", Ch, TimeStamp, Id, Dlc, Err); + for (int i = 0; i < Dlc; i++) + str += string.Format(" {0:X2}", Dat[i]); + return str; + } + } + + public class MFLin_CFG + { + public const int CHECK_MODE_CLASSIC = 0; + public const int CHECK_MODE_ENHANCE = 1; + public Byte Ch; + public bool Master; + public Byte Check; + public int Bps; + + public MFLin_CFG(Byte ch,bool ms, Byte chk,int bps) + { + Ch = ch; + Master = ms; + Check = chk; + Bps = bps; + } + public MFLin_CFG() + { + Ch = 0; + Master = true; + Check = CHECK_MODE_ENHANCE; + Bps = 19200; + } + } + public class MFLin_IDLength + { + public Byte Id; + public Byte Len; + public MFLin_IDLength(byte id,byte len) + { + Id = id; + Len = len; + } + public MFLin_IDLength() + { + Id = 0; + Len = 0; + } + } + + + + public class MFSerial + { + public int VERSION = 0x0003; + + public SerialPort Serial; + public NetworkStream DataStream; + + public Byte LinkChar; + + private Byte[] send_buff; + private Byte[] recv_buff; + private Byte[] msg_buff; + + private const int MSG_HEAD = 0x68; + private const int MSG_TAIL = 0x16; + + private const int MSG_STA_HEAD = 0; + private const int MSG_STA_LENGTH = 1; + private const int MSG_STA_DAT = 2; + private const int MSG_STA_CS = 3; + private const int MSG_STA_TAIL = 4; + + //command code for can + private const int U2CAN_COM_ENTER = 0; + private const int U2CAN_COM_LINK = 1; + private const int U2CAN_COM_SYNC = 9; + private const int U2CAN_COM_SEND = 2; + private const int U2CAN_COM_RECV = 3; + private const int U2CAN_COM_SETCFG = 4; + private const int U2CAN_COM_SETMODE = 5; + private const int U2CAN_COM_READCFG = 6; + private const int U2CAN_COM_FILTER_ID = 7; + private const int U2CAN_COM_ADVSET = 10; + private const int U2CAN_COM_ADVMSG = 11; + + private const int U2LIN_COM_ENTER = 0; + private const int U2LIN_COM_LINK = 1; + private const int U2LIN_COM_SYNC = 9; + + //command code for lin + private const int U2LIN_COM_RECV = 0x10; + private const int U2LIN_COM_SEND = 0X11; + private const int U2LIN_COM_SETRES = 0x12; + private const int U2LIN_COM_SETMODE = 0x13; + private const int U2LIN_COM_EVENT = 0x14; + private const int U2LIN_COM_SETLEN = 0x15; + private const int U2LIN_COM_SETBPS = 0x16; + private const int U2LIN_COM_SERLEN = 0x17; + + private const int U2LIN_CH_MAX = 4; + private const int U2CAN_CH_MAX = 4; + + private Queue Q_can_ack; //can + private Queue Q_lin_ack; + private Queue Q_uni_ack; //common ack + private Queue Q_can_dat; + private Queue Q_lin_dat; + + public event EventHandler MsgPushEvent; + + //for recv msg state + private int MsgState; + private int MsgRp; + private int MsgLen; + private Byte MsgCs; + private Thread read_thread; + + private Mutex Queuemutex; + private Mutex Writemutex; + + //device infomation + public string Device_CanVersion; + public string Device_LinVersion; + public string Device_Name; + public string Device_SN; + //lin + public MFLin_CFG Lin_Cfg; + public bool Lin_Is_Opened; + public int Lin_Count; + public int Lin_Count_Max = 0; + public int UL_RES_DAT_MAX = 64; + + private byte[] IdLengthBuff; + //can + public bool Can_Is_Opened; + public int Can_Send_Count; + public int Can_Recv_Count; + public int Can_Recv_Count_Max = 999999999; + + public MFSerial() + { + + send_buff = new Byte[1024]; + recv_buff = new Byte[1024]; + msg_buff = new Byte[1024]; + IdLengthBuff = new Byte[64* U2LIN_CH_MAX]; + + for (int i = 0; i < IdLengthBuff.Length; i++) + IdLengthBuff[i] = 0; + + Q_uni_ack = new Queue(); + Q_can_ack = new Queue(); + Q_lin_ack = new Queue(); + Q_can_dat = new Queue(); + Q_lin_dat = new Queue(); + + Queuemutex = new Mutex(); + Writemutex = new Mutex(); + + MsgState = MSG_STA_HEAD; + + read_thread = null; + + Lin_Cfg = new MFLin_CFG(0, true, MFLin_CFG.CHECK_MODE_CLASSIC, 19200); + + Serial = null; + DataStream = null; + Can_Is_Opened = false; + Lin_Is_Opened = false; + + LinkChar = (Byte)'L'; + } + + + + public bool SyncTimeStamp(UInt32 ts) + { + Byte[] msg = new Byte[1024]; + msg[0] = 0x09; + msg[1] = (Byte)(ts >> 0) ; + msg[2] = (Byte)(ts >> 8); + msg[3] = (Byte)(ts >> 16); + msg[4] = (Byte)(ts >> 24); + + write_msg(msg, 5); + return true; + } + + public bool Can_Open(string portname, int bps) + { + if (DataStream != null) return false; + if (Can_Is_Opened) return true; + + if(Serial == null) + { + Serial = new SerialPort(); + Serial.Parity = System.IO.Ports.Parity.None; + Serial.DataBits = 8; + Serial.StopBits = System.IO.Ports.StopBits.One; + Serial.Handshake = System.IO.Ports.Handshake.None; + Serial.WriteBufferSize = 4096; + Serial.ReadBufferSize = 4096; + // Set the read/write timeouts + Serial.ReadTimeout = 1; + Serial.WriteTimeout = 50; + Serial.DtrEnable = true; + } + if (!Serial.IsOpen) + { + + try + { + Serial.PortName = portname; + Serial.BaudRate = bps; + Serial.Open(); + } + catch + { + return false; + } + } + + if (read_thread == null) + { + read_thread = new Thread(new ThreadStart(read_task)); + read_thread.Start(); + } + + Queuemutex.WaitOne(); + Q_can_ack.Clear(); + Queuemutex.ReleaseMutex(); + + Byte[] msg = new Byte[1024]; + msg[0] = 0x41; + msg[1] = LinkChar; + + write_msg(msg, 2); + Thread.Sleep(100); + + while (Q_can_ack.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] buff = Q_can_ack.Dequeue(); + Queuemutex.ReleaseMutex(); + if (buff[1] == 'U' && buff[2] == '2' && buff[3] == 'C') + { + Can_Is_Opened = true; + Can_Recv_Count = Q_lin_dat.Count; + Can_Send_Count = 0; + Device_CanVersion = System.Text.Encoding.Default.GetString(buff, 1, 8); + + if (buff.Length > 9) + { + int i, pos; + for (i = 0; i < 32; i++) + { + byte c = buff[10 + i]; + if (c == 0) break; + msg_buff[i] = c; + } + msg_buff[i] = 0; + Device_Name = System.Text.Encoding.Default.GetString(msg_buff, 0, i); + + pos = i + 10 + 1; + for (i = 0; i < 32; i++) + { + if (pos + i >= buff.Length) break; + byte c = buff[pos + i]; + if (c == 0) break; + msg_buff[i] = c; + } + msg_buff[i] = 0; + Device_SN = System.Text.Encoding.Default.GetString(msg_buff, 0, i); + + } + return true; + } + } + try + { + if(Lin_Is_Opened == false) Serial.Close(); + } + catch { }; + return false; + + } + + public bool Can_Open(NetworkStream stream) + { + if (Serial != null) return false; + if (DataStream != null) + { + if (DataStream != stream) return false; + } + else + { + DataStream = stream; + } + + if (Can_Is_Opened) return true; + + if (read_thread == null) + { + read_thread = new Thread(new ThreadStart(read_task)); + read_thread.Start(); + } + Queuemutex.WaitOne(); + Q_can_ack.Clear(); + Queuemutex.ReleaseMutex(); + + Byte[] msg = new Byte[1024]; + msg[0] = 0x41; + msg[1] = LinkChar; + + write_msg(msg, 2); + Thread.Sleep(100); + + while (Q_can_ack.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] buff = Q_can_ack.Dequeue(); + Queuemutex.ReleaseMutex(); + if (buff[1] == 'U' && buff[2] == '2' && buff[3] == 'C') + { + Can_Is_Opened = true; + Can_Recv_Count = Q_lin_dat.Count; + Can_Send_Count = 0; + Device_CanVersion = System.Text.Encoding.Default.GetString(buff, 1, 8); + + if (buff.Length > 9) + { + int i, pos; + for (i = 0; i < 32; i++) + { + byte c = buff[10 + i]; + if (c == 0) break; + msg_buff[i] = c; + } + msg_buff[i] = 0; + Device_Name = System.Text.Encoding.Default.GetString(msg_buff, 0, i); + + pos = i + 10 + 1; + for (i = 0; i < 32; i++) + { + if (pos + i >= buff.Length) break; + byte c = buff[pos + i]; + if (c == 0) break; + msg_buff[i] = c; + } + msg_buff[i] = 0; + Device_SN = System.Text.Encoding.Default.GetString(msg_buff, 0, i); + + } + return true; + } + } + + return false; + + } + public MFCan_DevInfo MFCanGetInfo() + { + MFCan_DevInfo dev; + dev = new MFCan_DevInfo(); + dev.Send_Count = Can_Send_Count; + dev.Recv_Count = Can_Recv_Count; + dev.ProtocolVersion = Device_CanVersion; + dev.Name = Device_Name; + dev.SN = Device_SN; + if (DataStream != null) dev.InterfaceName = "TCP"; + else if (Serial != null) dev.InterfaceName = Serial.PortName; + + return dev; + } + public bool CanClose() + { + Can_Is_Opened = false; + Device_CanVersion = ""; + + + if (Lin_Is_Opened == false) + { + Device_Name = ""; + Device_SN = ""; + if (Serial != null) + { + try { Serial.Close(); } catch { } + Serial = null; + } + if (DataStream != null) DataStream = null; + + if (read_thread != null) read_thread.Abort(); + read_thread = null; + } + return true; + } + + public bool CanSet(MFCan_CFG cfg) + { + Byte[] buff = new Byte[1024]; + + if (Can_Is_Opened == false) return false; + + Queuemutex.WaitOne(); + Q_can_ack.Clear(); + Queuemutex.ReleaseMutex(); + + + buff[0] = 0x40+ U2CAN_COM_SETCFG; + buff[1] = cfg.Ch; + buff[2] = (Byte)(cfg.NormalBps >> 0); + buff[3] = (Byte)(cfg.NormalBps >> 8); + buff[4] = (Byte)(cfg.NormalBps >> 16); + buff[5] = (Byte)(cfg.NormalBps >> 24); + buff[6] = 0; + + + write_msg(buff, 7); + + for (int i = 0; i < 200; i++) + { + if (Q_can_ack.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] rmsg = Q_can_ack.Dequeue(); + Queuemutex.ReleaseMutex(); + if (rmsg[1] == U2CAN_COM_SETCFG) return true; + } + Thread.Sleep(1); + + } + + return false; + } + public bool CanAdvSet(MFCan_CFG cfg) + { + Byte[] buff = new Byte[1024]; + + if (Can_Is_Opened == false) return false; + + Queuemutex.WaitOne(); + Q_can_ack.Clear(); + Queuemutex.ReleaseMutex(); + + + buff[0] = 0x40 + U2CAN_COM_ADVSET; + buff[1] = cfg.Ch; + buff[2] = cfg.Mode; + buff[3] = cfg.Type; + buff[4] = (Byte)(cfg.NormalBps >> 0); + buff[5] = (Byte)(cfg.NormalBps >> 8); + buff[6] = (Byte)(cfg.NormalBps >> 16); + buff[7] = (Byte)(cfg.NormalBps >> 24); + buff[8] = (Byte)(cfg.DataBps >> 0); + buff[9] = (Byte)(cfg.DataBps >> 8); + buff[10] = (Byte)(cfg.DataBps >> 16); + buff[11] = (Byte)(cfg.DataBps >> 24); + + + write_msg(buff, 12); + + for (int i = 0; i < 200; i++) + { + if (Q_can_ack.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] rmsg = Q_can_ack.Dequeue(); + Queuemutex.ReleaseMutex(); + if (rmsg[1] == U2CAN_COM_ADVSET) return true; + } + Thread.Sleep(1); + + } + + return false; + } + public bool CanSendOne(MFCan_MSG msg) + { + if (Can_Is_Opened == false) return false; + Byte[] buff = new Byte[256]; + for (int i = 0; i < buff.Length; i++) + buff[i] = 0; + + buff[0] = U2CAN_COM_SEND; + buff[1] = (byte)msg.Ch; + buff[2] = 1; + + //3 4 5 6 + byte n = msg.Dlc; + if (n > 8) n = 8; + buff[5] = n; + switch (msg.Type) + { + case MFCan_MSG.CAN_MSG_TYPE_SR: + buff[6] = 0x40; + break; + case MFCan_MSG.CAN_MSG_TYPE_ED: + buff[6] = 0x80; + break; + case MFCan_MSG.CAN_MSG_TYPE_ER: + buff[6] = 0xC0; + break; + default:break; + } + + + //id 7 8 9 10 + buff[7] = (Byte)(msg.Id >> 0); + buff[8] = (Byte)(msg.Id >> 8); + buff[9] = (Byte)(msg.Id >> 16); + buff[10] = (Byte)(msg.Id >> 24); + + //DAT + for (int i = 0; i < n; i++) + buff[11 + i] = msg.Dat[i]; + + + write_msg(buff, 19); + + return false; + } + public bool CanSendAdvOne(MFCan_MSG msg) + { + if (Can_Is_Opened == false) return false; + Byte[] buff = new Byte[256]; + + buff[0] = U2CAN_COM_ADVMSG; + buff[1] = (byte)msg.Ch; + buff[2] = 1; + //id + buff[3] = (Byte)(msg.Id >> 0); + buff[4] = (Byte)(msg.Id >> 8); + buff[5] = (Byte)(msg.Id >> 16); + buff[6] = (Byte)(msg.Id >> 24); + + buff[7] = msg.Type; + + byte n = msg.Dlc; + if (n > 64) n = msg.Dlc; + buff[8] = n; + + //DAT + for (int i = 0; i < n; i++) + buff[9 + i] = msg.Dat[i]; + + + write_msg(buff, n+9); + + return false; + } + public int MFCanGetMsg(MFCan_MSG[] msg,int num) + { + int cnt = 0; + while (Q_can_dat.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] buff = Q_can_dat.Dequeue(); + Queuemutex.ReleaseMutex(); + if (buff[0] == U2CAN_COM_RECV) + { + msg[cnt].Ch = buff[1]; + msg[cnt].TimeStamp = (uint)(buff[3 ]) + (uint)(buff[4] << 8) + (uint)(buff[5] << 16) + (uint)(buff[6] << 24); + + msg[cnt].Dlc = buff[9]; + + if ((buff[10] & 0x80) > 0) //ext + { + if ((buff[10 ] & 0x40) > 0) //remote + { + msg[cnt].Type = MFCan_MSG.CAN_MSG_TYPE_ER; + } + else //data + { + msg[cnt].Type = MFCan_MSG.CAN_MSG_TYPE_ED; + } + + } + else //std + { + if ((buff[10] & 0x40) > 0) //remote + { + msg[cnt].Type = MFCan_MSG.CAN_MSG_TYPE_SR; + } + else //data + { + msg[cnt].Type = MFCan_MSG.CAN_MSG_TYPE_SD; + } + + } + + msg[cnt].Id = (uint)(buff[11 ]) + (uint)(buff[12] << 8) + (uint)(buff[13] << 16) + (uint)(buff[14] << 24); + + + for (int i = 0; i < 8; i++) + { + msg[cnt].Dat[i] = buff[15 + i]; + } + + + if(Can_Recv_Count > 0) Can_Recv_Count--; + cnt++; + } + else if (buff[0] == U2CAN_COM_ADVMSG) + { + msg[cnt].Ch = buff[1]; + msg[cnt].TimeStamp_100us = (uint)(buff[3]) + (uint)(buff[4] << 8) + (uint)(buff[5] << 16) + (uint)(buff[6] << 24); + msg[cnt].TimeStamp = msg[cnt].TimeStamp_100us/10; + msg[cnt].Id = (uint)(buff[7]) + (uint)(buff[8] << 8) + (uint)(buff[9] << 16) + (uint)(buff[10] << 24); + msg[cnt].Type = buff[11]; + msg[cnt].Dlc = buff[12]; + + for (int i = 0; i < msg[cnt].Dlc; i++) + { + msg[cnt].Dat[i] = buff[13 + i]; + } + + + if (Can_Recv_Count > 0) Can_Recv_Count--; + cnt++; + } + if (cnt >= num) break; + } + return cnt; + + } + + public bool Lin_Open(string portname,int bps) + { + if (DataStream != null) return false; + if (Lin_Is_Opened == true) return true; + + if (Serial == null) + { + Serial = new SerialPort(); + Serial.Parity = System.IO.Ports.Parity.None; + Serial.DataBits = 8; + Serial.StopBits = System.IO.Ports.StopBits.One; + Serial.Handshake = System.IO.Ports.Handshake.None; + Serial.WriteBufferSize = 4096; + Serial.ReadBufferSize = 4096; + // Set the read/write timeouts + Serial.ReadTimeout = 1; + Serial.WriteTimeout = 50; + Serial.DtrEnable = true; + } + if (!Serial.IsOpen) + { + + try + { + Serial.PortName = portname; + Serial.BaudRate = bps; + Serial.Open(); + } + catch + { + return false; + } + } + if (read_thread == null) + { + read_thread = new Thread(new ThreadStart(read_task)); + read_thread.Start(); + + } + Queuemutex.WaitOne(); + Q_lin_ack.Clear(); + Queuemutex.ReleaseMutex(); + + Byte[] msg = new Byte[1024]; + msg[0] = 0x41; + msg[1] = LinkChar; + + write_msg(msg, 2); + Thread.Sleep(100); + + while (Q_lin_ack.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] buff = Q_lin_ack.Dequeue(); + Queuemutex.ReleaseMutex(); + if (buff[1] == 'U' && buff[2] == '2' && buff[3] == 'L') + { + Lin_Is_Opened = true; + Lin_Count = Q_lin_dat.Count; + Device_LinVersion = System.Text.Encoding.Default.GetString(buff, 1,8); + + if (buff.Length > 9) + { + int i,pos; + for (i = 0; i < 32; i++) + { + byte c = buff[10 + i]; + if (c == 0) break; + msg_buff[i] = c; + } + msg_buff[i] = 0; + Device_Name = System.Text.Encoding.Default.GetString(msg_buff, 0, i); + + pos = i + 10 + 1; + for (i = 0; i < 32; i++) + { + if(pos + i >= buff.Length) break; + byte c = buff[pos + i]; + if (c == 0) break; + msg_buff[i] = c; + } + msg_buff[i] = 0; + Device_SN = System.Text.Encoding.Default.GetString(msg_buff, 0, i); + + } + return true; + } + } + try + { + if (Can_Is_Opened == false) Serial.Close(); + } catch { }; + return false; + + } + public bool Lin_Open(NetworkStream stream) + { + if (Serial != null) return false; + if (DataStream != null) + { + if (DataStream != stream) return false; + } + else + { + DataStream = stream; + } + + if (Lin_Is_Opened) return true; + + if (read_thread == null) + { + read_thread = new Thread(new ThreadStart(read_task)); + read_thread.Start(); + } + Queuemutex.WaitOne(); + Q_lin_ack.Clear(); + Queuemutex.ReleaseMutex(); + + Byte[] msg = new Byte[1024]; + msg[0] = 0x41; + msg[1] = LinkChar; + + write_msg(msg, 2); + Thread.Sleep(100); + + while (Q_lin_ack.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] buff = Q_lin_ack.Dequeue(); + Queuemutex.ReleaseMutex(); + if (buff[1] == 'U' && buff[2] == '2' && buff[3] == 'L') + { + Lin_Is_Opened = true; + Lin_Count = Q_lin_dat.Count; + Device_LinVersion = System.Text.Encoding.Default.GetString(buff, 1, 8); + + if (buff.Length > 9) + { + int i, pos; + for (i = 0; i < 32; i++) + { + byte c = buff[10 + i]; + if (c == 0) break; + msg_buff[i] = c; + } + msg_buff[i] = 0; + Device_Name = System.Text.Encoding.Default.GetString(msg_buff, 0, i); + + pos = i + 10 + 1; + for (i = 0; i < 32; i++) + { + if (pos + i >= buff.Length) break; + byte c = buff[pos + i]; + if (c == 0) break; + msg_buff[i] = c; + } + msg_buff[i] = 0; + Device_SN = System.Text.Encoding.Default.GetString(msg_buff, 0, i); + + } + return true; + } + } + + return false; + + } + public MFLin_DevInfo MFLinGetInfo() + { + MFLin_DevInfo dev; + dev = new MFLin_DevInfo(); + dev.Lin_Count = Lin_Count; + dev.ProtocolVersion = Device_LinVersion; + dev.Name = Device_Name; + dev.SN = Device_SN; + if (DataStream != null) dev.InterfaceName = "TCP"; + else if (Serial != null) dev.InterfaceName = Serial.PortName; + + return dev; + } + + public bool MFLinIsOpen() + { + return Lin_Is_Opened; + } + + public bool LinClose() + { + Lin_Is_Opened = false; + Device_LinVersion = ""; + if (Can_Is_Opened == false) + { + Device_Name = ""; + Device_SN = ""; + if (Serial != null) + { + try { Serial.Close(); } catch { } + Serial = null; + } + if (DataStream != null) DataStream = null; + if (read_thread != null) read_thread.Abort(); + read_thread = null; + + } + return true; + } + + public bool LinSet(MFLin_CFG cfg) + { + Byte[] buff = new Byte[1024]; + + if (Lin_Is_Opened == false) return false; + + Queuemutex.WaitOne(); + Q_lin_ack.Clear(); + Queuemutex.ReleaseMutex(); + + + buff[0] = 0x53; + buff[1] = cfg.Ch; + + if (cfg.Master) buff[2] = 0x01; + else buff[2] = 0x00; + if (cfg.Check == MFLin_CFG.CHECK_MODE_ENHANCE) buff[2] |= 1 << 2; + + write_msg(buff, 3); + + for(int i = 0; i< 200; i++) + { + if(Q_lin_ack.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] rmsg = Q_lin_ack.Dequeue(); + Queuemutex.ReleaseMutex(); + if (rmsg[0] == U2LIN_COM_SETMODE) goto setbps; + } + Thread.Sleep(1); + + } + + return false; + +setbps: + + buff[0] = 0xd6; + buff[1] = cfg.Ch; + buff[2] = (Byte)(cfg.Bps >> 0); + buff[3] = (Byte)(cfg.Bps >> 8); + buff[4] = (Byte)(cfg.Bps >> 16); + buff[5] = (Byte)(cfg.Bps >> 24); + + write_msg(buff, 6); + for (int i = 0; i < 200; i++) + { + if (Q_lin_ack.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] rmsg = Q_lin_ack.Dequeue(); + Queuemutex.ReleaseMutex(); + if (rmsg[0] == U2LIN_COM_SETMODE) + { + + Lin_Cfg.Ch = rmsg[1]; + if ((rmsg[2] & 0x01) != 0) Lin_Cfg.Master = true; + else Lin_Cfg.Master = false; + + if (((rmsg[2]>>2)& 0x03) == 0x01) Lin_Cfg.Check = MFLin_CFG.CHECK_MODE_ENHANCE; + else Lin_Cfg.Check = MFLin_CFG.CHECK_MODE_CLASSIC; + + Lin_Cfg.Bps = rmsg[5] << 0 | rmsg[6] << 8 | rmsg[7] << 16 | rmsg[8] << 24; + + return true; + } + } + Thread.Sleep(1); + + } + + return false; + } + + public bool LinSetIdLength(byte ch,MFLin_IDLength[] set) + { + Byte[] buff = new Byte[1024]; + + if(ch >= U2LIN_CH_MAX) return false; + if (Lin_Is_Opened == false) return false; + + Queuemutex.WaitOne(); + Q_lin_ack.Clear(); + Queuemutex.ReleaseMutex(); + + for (int i = 0; i < set.Length; i++) + { + byte id, len; + id = set[i].Id; + len = set[i].Len; + if (id < 0x40 && len < UL_RES_DAT_MAX) + { + IdLengthBuff[ch*64+id] = len; + } + } + + buff[0] = U2LIN_COM_SETLEN | 0x40; + buff[1] = ch; + Array.Copy(IdLengthBuff, ch * 64, buff, 2, 64); + + write_msg(buff, 66); + + for (int i = 0; i < 200; i++) + { + if (Q_lin_ack.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] rmsg = Q_lin_ack.Dequeue(); + Queuemutex.ReleaseMutex(); + if (rmsg[0] == U2LIN_COM_ENTER && rmsg[1] == U2LIN_COM_SETLEN) return true; + } + Thread.Sleep(1); + + } + return false; + } + public bool LinUpLoad(MFLin_MSG Msg,bool ack) + { + Byte[] buff = new Byte[1024]; + + if (Lin_Is_Opened == false) return false; + + Queuemutex.WaitOne(); + Q_lin_ack.Clear(); + Queuemutex.ReleaseMutex(); + + buff[0] = U2LIN_COM_SETRES ; + if (ack) buff[0] |= 0x40; + buff[1] = Msg.Ch; + buff[2] = 1; + buff[3] = Msg.Id; + if (Msg.Dlc > UL_RES_DAT_MAX) Msg.Dlc = (byte)UL_RES_DAT_MAX; + buff[4] = Msg.Dlc; + + Array.Copy(Msg.Dat, 0, buff, 5, Msg.Dlc); + + write_msg(buff, Msg.Dlc + 5); + + if (ack == false) return true; + + for (int i = 0; i < 200; i++) + { + if (Q_lin_ack.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] rmsg = Q_lin_ack.Dequeue(); + Queuemutex.ReleaseMutex(); + if (rmsg[0] == U2LIN_COM_ENTER && rmsg[1] == U2LIN_COM_SETRES) return true; + } + Thread.Sleep(1); + + } + return false; + } + public bool LinSlaveDisable( byte ch, byte id) + { + MFLin_MSG res_msg = new MFLin_MSG(); + res_msg.Ch = ch; + res_msg.Id = id; + res_msg.Dlc = 0; + return LinUpLoad(res_msg,true); + } + + public bool LinMasterSend( byte ch, byte id) + { + Byte[] buff = new Byte[64]; + + buff[0] = U2LIN_COM_SEND; + buff[1] = ch; + buff[2] = 13; + buff[3] = id; + write_msg(buff, 4); + + return true; + } + + + public bool LinMasterSendOne(MFLin_MSG Msg) + { + LinUpLoad(Msg,true); + LinMasterSend( Msg.Ch, Msg.Id); + return true; + } + + public int MFLinGetMsg(MFLin_MSG[] msg, int num) + { + int cnt = 0; + + while (Q_lin_dat.Count > 0) + { + Queuemutex.WaitOne(); + Byte[] buff = Q_lin_dat.Dequeue(); + Queuemutex.ReleaseMutex(); + if (buff[0] == U2LIN_COM_RECV) + { + msg[cnt].Ch = buff[1]; + msg[cnt].TimeStamp = (uint)(buff[2]) + (uint)(buff[3] << 8) + (uint)(buff[4] << 16) + (uint)(buff[5] << 24); + msg[cnt].Err = buff[6]; + msg[cnt].Id = buff[7]; + msg[cnt].Dlc = (byte)buff[8]; + for (int i = 0; i < buff[8]; i++) + { + msg[cnt].Dat[i] = buff[9 + i]; + } + } + else if (buff[0] == U2LIN_COM_EVENT) + { + msg[cnt].Ch = buff[1]; + msg[cnt].TimeStamp = (uint)(buff[2]) + (uint)(buff[3] << 8) + (uint)(buff[4] << 16) + (uint)(buff[5] << 24); + msg[cnt].Err = (byte)(buff[6] + 6); + msg[cnt].Id = 0; + msg[cnt].Dlc = 0; + } + if (Lin_Count > 0) Lin_Count--; + + if (++cnt >= num) break; + + } + return cnt; + + } + // for class self + public bool write_msg(Byte[] msg, int num) + { + int i; + int csum; + Writemutex.WaitOne(); + + send_buff[0] = MSG_HEAD; + send_buff[1] = (Byte)(num & 0xff); + send_buff[2] = (Byte)((num >> 8) & 0xff); + csum = send_buff[0] + send_buff[1] + send_buff[2]; + for (i = 0; i < num; i++) + { + send_buff[3 + i] = msg[i]; + csum += msg[i]; + } + send_buff[3 + i] = (Byte)(csum & 0xff); + send_buff[3 + i + 1] = MSG_TAIL; + num += 5; + + try + { + if (DataStream != null) DataStream.Write(send_buff, 0, num); + else if(Serial != null) Serial.Write(send_buff, 0, num); + + } + catch { }; + + Writemutex.ReleaseMutex(); + + return true; + } + + private void read_task() + { + int ret; + while (true) + { + ret = read_dat_proc(); + if(ret < 0) Thread.Sleep(2); + else Thread.Sleep(20); + } + + } + private int read_dat_proc() + { + int i; + int num; + try + { + if (DataStream != null) num = DataStream.Read(recv_buff, 0, 1024); + else if (Serial != null) num = Serial.Read(recv_buff, 0, 1024); + else return -1; + } + catch + { + return -1; + } + for (i = 0; i < num; i++) + { + switch (MsgState) + { + case MSG_STA_HEAD: + if (recv_buff[i] == MSG_HEAD) + { + MsgState = MSG_STA_LENGTH; + MsgRp = 0; + MsgLen = 2; + } + break; + case MSG_STA_LENGTH: + msg_buff[MsgRp] = recv_buff[i]; + MsgRp++; + MsgLen -= 1; + if (MsgLen == 0) + { + MsgLen = msg_buff[0] + msg_buff[1] * 256; + if (MsgLen > 1023) + { + MsgState = MSG_STA_HEAD; + } + else + { + MsgCs = (Byte)(MSG_HEAD + msg_buff[0] + msg_buff[1]); // + MsgState = MSG_STA_DAT; + MsgRp = 0; + } + } + break; + case MSG_STA_DAT: + msg_buff[MsgRp] = recv_buff[i]; + MsgRp += 1; + MsgCs += recv_buff[i]; + if (MsgRp == MsgLen) + { + MsgState = MSG_STA_CS; + } + break; + case MSG_STA_CS: + if ((MsgCs & 0xff) == recv_buff[i]) MsgState = MSG_STA_TAIL; + else MsgState = MSG_STA_HEAD; + break; + case MSG_STA_TAIL: + if (recv_buff[i] == MSG_TAIL) + { + msg_push(msg_buff, MsgLen); + } + MsgState = MSG_STA_HEAD; + break; + default: + MsgState = MSG_STA_HEAD; + break; + } + + } + return num; + + } + private void msg_push(Byte[] msg, int len) + { + if ((msg[0] & 0x80) == 0) return;//no dir flag + msg[0] = (Byte)(msg[0] & 0x3f); + Byte[] pbuff = new Byte[len]; + Array.Copy(msg, pbuff, len); + Queuemutex.WaitOne(); + if (MsgPushEvent != null) MsgPushEvent(pbuff, null); + switch (msg[0]) + { + case 0x00: + if (msg[1] >= 0x02 && msg[1] <= 0x0f) + { //can ack + Q_can_ack.Enqueue(pbuff); + } + else if (msg[1] >= 0x10 && msg[1] <= 0x1e) + { //lin ack + Q_lin_ack.Enqueue(pbuff); + } + else + { + //uni_ack + Q_uni_ack.Enqueue(pbuff); + } + break; + case 0x01: //link + if (msg[3] == 'C') + { + //can ack + Q_can_ack.Enqueue(pbuff); + + } + if (msg[3] == 'L') + { + //lin ack + Q_lin_ack.Enqueue(pbuff); + } + break; + case 0x03: //can data + { + int n = pbuff[2]; + int pos = 3; + for(int i = 0; i< n; i++) + { + if (Can_Recv_Count > Can_Recv_Count_Max) break; + byte[] mbuff = new Byte[23]; + mbuff[0] = pbuff[0]; + mbuff[1] = pbuff[1]; + mbuff[2] = 1; + if (pos+20 > pbuff.Length) break; + Array.Copy(pbuff, pos, mbuff ,3, 20); + Q_can_dat.Enqueue(mbuff); + + pos += 20; + Can_Recv_Count++; + } + + } + break; + case 0x0b: //adv can data + { + int n = pbuff[2]; + int pos = 3; + + for (int i = 0; i < n; i++) + { + if (Can_Recv_Count > Can_Recv_Count_Max) break; + + if (pos + 5 > pbuff.Length) break; + + int datlen = pbuff[pos + 9]; + if (datlen > 64) break; + + byte[] mbuff = new Byte[3 + 10 + datlen]; + + mbuff[0] = pbuff[0]; + mbuff[1] = pbuff[1]; + mbuff[2] = 1; + + if (pos + 10 + datlen > pbuff.Length) break; + + Array.Copy(pbuff, pos, mbuff, 3, 10 + datlen); + Q_can_dat.Enqueue(mbuff); + + pos += 10 + datlen; + Can_Recv_Count++; + + + } + + } + + break; + case 0x10: + case 0x14: + //lin dat + Q_lin_dat.Enqueue(pbuff); + break; + default: + if (msg[0] >= 0x02 && msg[0] <= 0x0f) + { //can ack + Q_can_ack.Enqueue(pbuff); + } + else if (msg[0] >= 0x10 && msg[0] <= 0x1e) + { //link ack + Q_lin_ack.Enqueue(pbuff); + } + else + {// other + Q_uni_ack.Enqueue(pbuff); + } + break; + } + Queuemutex.ReleaseMutex(); + } + + + + } + + class MFNetDevice + { + public string Name; + public string ServerPort; + public string Ip; + } + + struct UdpState + { + public UdpClient u; + public IPEndPoint e; + } + class MFNetDeviceScanner + { + public bool Scanning; + private UdpState sta; + List DevList; + + public delegate void ScanDevResult(MFNetDevice dev); + + public event ScanDevResult FoundADevice; + public MFNetDeviceScanner() + { + Scanning = false; + DevList = new List(); + } + public void ReceiveCallback(IAsyncResult ar) + { + UdpClient u = ((UdpState)(ar.AsyncState)).u; + IPEndPoint e = ((UdpState)(ar.AsyncState)).e; + byte[] receiveBytes; + try + { + receiveBytes = u.EndReceive(ar, ref e); + } + catch { + Scanning = false; + return; + } + + + string receiveString = System.Text.Encoding.ASCII.GetString(receiveBytes); + string[] strs = receiveString.Split(' '); + if(strs.Length == 2) + { + MFNetDevice dev = new MFNetDevice(); + dev.Ip = e.Address.ToString(); + dev.Name = strs[0]; + dev.ServerPort = strs[1]; + + MFNetDevice result = DevList.Find( + delegate (MFNetDevice d) + { + return d.Name == dev.Name; + } + ); + if (result == null) + { + DevList.Add(dev); + if (FoundADevice != null) FoundADevice(dev); + } + } + + + + try + { + u.BeginReceive(new AsyncCallback(ReceiveCallback), sta); + } + catch + { + Scanning = false; + } + + + } + public bool Start(int port) + { + if (Scanning) return true; + DevList.Clear(); + IPEndPoint e = new IPEndPoint(IPAddress.Any, port); + UdpClient u = new UdpClient(e); + + sta = new UdpState(); + sta.e = e; + sta.u = u; + try + { + u.BeginReceive(new AsyncCallback(ReceiveCallback), sta); + } + catch { return false; } + Scanning = true; + return true; + } + public bool Stop() + { + try + { + if(Scanning) sta.u.Close(); + } + catch { }; + Scanning = false; + return true; + } + } \ No newline at end of file