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));
}
