private byte[] CreateCmd(CmdGroups sendGroup, int sendCmd, byte[] data = null) { int dataLen = (data == null) ? 0 : data.Length; // data + 5 byte header + 1 byte chksum int length = dataLen + 5; byte[] cmd = new byte[length + 1]; // + 1 byte for checksum cmd[0] = CMD_SOH; cmd[1] = (byte)sendGroup; cmd[2] = (byte)sendCmd; cmd[3] = (byte)(dataLen & 0xff); cmd[4] = (byte)(dataLen >> 8); if (dataLen > 0) { Buffer.BlockCopy(data, 0, cmd, 5, dataLen); } int chksum = 0; for (int i = 1; i < cmd.Length - 1; i++) { chksum ^= cmd[i]; } cmd[cmd.Length - 1] = (byte)(chksum); return(cmd); }
/// <summary> /// Send cmd with send and receive data /// </summary> /// <param name="sendGroup"></param> /// <param name="sendCmd"></param> /// <param name="sendData"></param> /// <param name="recvLen"></param> /// <param name="recvData"></param> /// <returns></returns> private int SendCmd(CmdGroups sendGroup, int sendCmd, byte[] sendData, int?recvLen, out byte[] recvData) { recvData = null; try { _serialPort.DiscardInBuffer(); byte[] data = CreateCmd(sendGroup, sendCmd, sendData); if (data.Length <= 32) { data = CreateCmd(sendGroup, sendCmd, sendData); _serialPort.Write(data, 0, data.Length); } else { // if data>32 byte we use a special SendBlock routine to send the data part data = CreateCmd(sendGroup, sendCmd, sendData); _serialPort.Write(data, 0, 5); // header SendBlock(data, 5, data.Length - 6); // data _serialPort.Write(data, data.Length - 1, 1); // checksum } int status = RecvResult(sendGroup, sendCmd, recvLen, out recvData); string str = _serialPort.ReadExisting(); return(status); } catch (Exception ex) { _logging.Error(MODUL_NAME, "SendCmd", $"Error sending command, sendCmd={sendCmd}"); return(0); } }
/// <summary> /// Receive a result message from the Arduino, with timeout /// Debugging messegaes from the arduino a filtered an written to the log /// </summary> /// <param name="sendGroup"></param> /// <param name="sendCmd"></param> /// <param name="length"></param> /// <param name="data"></param> /// <returns></returns> private int RecvResult(CmdGroups sendGroup, int sendCmd, int?length, out byte[] data) { int cmdPrefix; data = null; try { // wait for CMD_PREFIX Stopwatch sw = new Stopwatch(); sw.Start(); while (true) { if (sw.ElapsedMilliseconds > CurrentTimeout) { _logging.Warn(MODUL_NAME, "RecvResult", $"timout waiting for {CMD_SOH:X2}"); Debug.WriteLine($"timeout {(byte)sendGroup:X2} {sendCmd:X2}"); return(99); } if (_serialPort.BytesToRead == 0) { continue; } cmdPrefix = _serialPort.ReadByte(); if (cmdPrefix == CMD_SOH) { break; } if (cmdPrefix == (int)'*') { // debug output from arduino string debug = _serialPort.ReadLine().Trim(new char[] { '\r', '\n' }); Debug.WriteLine("*" + debug); _logging.Debug("Arduino", "", "*" + debug); sw.Restart(); } } int calcChksum = 0; int recvGroup = _serialPort.ReadByte(); if (recvGroup != (int)sendGroup) { _logging.Error(MODUL_NAME, "RecvResult", $"invalid cmd group {recvGroup:X2} received, expected {(int)sendGroup:X2}"); return(99); // invalid cmd group received } calcChksum ^= recvGroup; int recvCmd = _serialPort.ReadByte(); if (recvCmd != sendCmd) { _logging.Error(MODUL_NAME, "RecvResult", $"invalid cmd {recvCmd:X2} received, expected {sendCmd:X2}"); return(99); // invalid cmd group received } calcChksum ^= recvCmd; int recvResult = _serialPort.ReadByte(); if (recvResult != 0) { _logging.Error(MODUL_NAME, "RecvResult", $"error result {recvResult:X2} received"); return(recvResult); } calcChksum ^= recvResult; // 16 bit length int recvLength = _serialPort.ReadByte(); recvLength += (_serialPort.ReadByte() << 8); if (length != null && recvLength != length) { _logging.Error(MODUL_NAME, "RecvResult", $"invalid length {recvLength}, excpected {length}"); return(99); // wrong length } calcChksum ^= recvLength & 0xFF; calcChksum ^= (recvLength >> 8) & 0xFF; if (recvLength != 0) { // receive data data = new byte[recvLength]; int count = RecvBlock(data, recvLength); if (count != recvLength) { _logging.Error(MODUL_NAME, "RecvResult", $"received {count} bytes, excpected {recvLength} bytes"); return(99); // wrong length } for (int i = 0; i < recvLength; i++) { calcChksum ^= data[i]; } } int chkSum = _serialPort.ReadByte(); if (chkSum != calcChksum) { _logging.Error(MODUL_NAME, "RecvResult", $"checksum error, receiced {chkSum:X2}, excpected {calcChksum:X2}"); return(99); // wrong checksum } return(0); // ok } catch (Exception ex) { _logging.Error(MODUL_NAME, "RecvResult", $"ex={ex}"); return(99); } }
/// <summary> /// Result with out data /// </summary> /// <param name="sendGroup"></param> /// <param name="sendCmd"></param> /// <returns></returns> private int RecvResult(CmdGroups sendGroup, int sendCmd) { byte[] dummy; return(RecvResult(sendGroup, sendCmd, 0, out dummy)); }
/// <summary> /// Send cmd with send data, not receive data /// </summary> /// <param name="sendGroup"></param> /// <param name="sendCmd"></param> /// <param name="sendData"></param> /// <param name="recvLen"></param> /// <param name="recvData"></param> /// <returns></returns> private int SendCmd(CmdGroups sendGroup, int sendCmd, byte[] sendData) { byte[] dummy; return(SendCmd(sendGroup, sendCmd, sendData, 0, out dummy)); }
/// <summary> /// Send cmd without send and receive data /// </summary> /// <param name="sendGroup"></param> /// <param name="sendCmd"></param> /// <returns></returns> private int SendCmd(CmdGroups sendGroup, int sendCmd) { byte[] dummy; return(SendCmd(sendGroup, sendCmd, null, 0, out dummy)); }