private void Port_OnReceived(object sender, PortReceivedEventArgs args)
{
if (args.Data.Length > BUFFER_SIZE)
{
_wOffset = 0;
_rOffset = 0;
_length = 0;
return;
}
if (BUFFER_SIZE >= _wOffset + args.Length)
{
Array.Copy(args.Data, 0, _buffer, _wOffset % BUFFER_SIZE, args.Length);
}
else
{
Array.Copy(args.Data, 0, _buffer, _wOffset % BUFFER_SIZE, BUFFER_SIZE - _wOffset);
Array.Copy(args.Data, BUFFER_SIZE - _wOffset, _buffer, 0, (args.Length - (BUFFER_SIZE - _wOffset)) % BUFFER_SIZE);
}
_wOffset = (_wOffset + _length) % BUFFER_SIZE;
_length += args.Length;
if (BUFFER_SIZE < _length)
{
_rOffset = _wOffset;
_length = BUFFER_SIZE;
}
byte[] d;
int startOffset = -1;
int endOffset = -1;
for (int o = _rOffset, l = _length; (--l) > 0; o = (++o) % BUFFER_SIZE)
{
if ((byte)(0x80 + _AGVNo) == _buffer[o])
{
startOffset = o;
}
else if ((byte)(0xFF - (0x80 + _AGVNo)) == _buffer[o])
{
if (0 > startOffset)
{
continue;
}
endOffset = (o + 1) % BUFFER_SIZE;
if (startOffset < endOffset)
{
d = new byte[endOffset - startOffset];
Array.Copy(_buffer, startOffset, d, 0, d.Length);
}
else
{
d = new byte[(BUFFER_SIZE - startOffset + endOffset)];
Array.Copy(_buffer, startOffset, d, 0, BUFFER_SIZE - startOffset);
Array.Copy(_buffer, 0, d, BUFFER_SIZE - startOffset, endOffset);
}
_length -= _rOffset < endOffset ? endOffset - _rOffset : BUFFER_SIZE - _rOffset + endOffset;
_rOffset = endOffset;
startOffset = -1;
endOffset = -1;
//TODO:d
try
{
TPosition pos = default(TPosition);
TState state = default(TState);
var res = _serialize.Deserialize <StateResult>(d);
if (res.UPHead == (0x80 + _AGVNo) && eAgvResultWord.State == res.CommandWord)
{
Position = (TPosition)System.Enum.ToObject(typeof(TPosition), res.NodeNumber);
State = (TState)System.Enum.ToObject(typeof(TState), res.State);
}
}
catch
{
var code = BitConverter.ToString(d).Replace('-', ' '); //args.Data
Logger.LogInfo($"AGV{_AGVNo}:OnReceived, code:{code}");
}
}
}
}
//Incoming data event - wait for a prompt or gather/send a line and trigger events
void DataReceived(object sender, PortReceivedEventArgs e)
{
DoModeChange(CommMode.CLI);
mavPixReply += e.Data;
//Respond to boot message - "Press <Enter> 3 times for CLI."
if (waitForBoot)
{
if (mavPixReply.Contains("<Enter>"))
{
stopTimer();
crlfPress();
crlfPress();
crlfPress();
startTimer(ref waitForPrompt);
return;
}
else if (mavPixReply.EndsWith("#"))
{
waitForPrompt = true; //Drop through to prompt
}
}
//Respond to a prompt "#"
if (waitForPrompt)
{
if (mavPixReply.EndsWith("#"))
{
stopTimer();
if (commType == CommType.BOOTING)
{
OnProgressChanged(commType, 32, "Configuration: Mavpixel ready.");
OnBootCompleted();
OnCompleted(commType, true, 0);
}
else
{
OnComStarted(commType);
OnProgressChanged(commType, 0, "Configuration: Fetching " + dataName + "..");
serial.WriteLine(OnReadCommand(readCommandIndex));
mavPixReply = "";
startTimer(ref collectingReply);
}
}
}
//Gathering a reply
else if (collectingReply)
{
//Scan the (multiline) reply for command indexes and use to update progress.
int index = -1;
for (int c = mavPixReply.Length - 1; c >= 0; c--)
{
if (char.IsLower(mavPixReply, c))
{
index = c;
break;
}
}
if (index >= 0 && mavPixReply.Length >= index + 4)
{
int gotReply;
if (int.TryParse(mavPixReply.Substring(index + 1, 3), out gotReply) && lastReply != gotReply)
{
stopTimer();
lastReply = gotReply;
OnProgressChanged(commType, lastReply, "");
startTimer(ref collectingReply);
}
}
//Got a complete reply. Send it to be parsed.
if (mavPixReply.EndsWith("#"))
{
if (mavPixReply.Contains(":") || mavPixReply.Contains(","))
{
stopTimer();
OnGotData(readCommandIndex, mavPixReply);
int progress = (int)((float)readCommandIndex / (float)commandsToRead * 32);
MessageCounter++;
if (++readCommandIndex < commandsToRead)
{
serial.WriteLine(OnReadCommand(readCommandIndex));
mavPixReply = "";
OnProgressChanged(commType, progress, "");
startTimer(ref collectingReply);
}
else
{
OnProgressChanged(commType, progress, "Configuration: Read all data OK.");
OnCompleted(commType, true, readCommandIndex - 1);
}
}
else
{
checkEmpty = true;
}
}
}
//Sending a command
else if (sendingCommands)
{
if (mavPixReply.EndsWith("#")) //wait for the prompt
{
stopTimer();
//Check for errors or unrecognised responses
if (mavPixReply.Contains("error") || mavPixReply.Contains("Unknown"))
{
string error = mavPixReply.Split('\r')[1].Trim();
comAttempts++;
int progress = (int)((float)sentCommandIndex / (float)commandsToSend * 32);
if (comAttempts <= Properties.Settings.Default.comRetries)
{
string command;
OnProgressChanged(commType, progress, "Configuration - attempt " + comAttempts.ToString() + ": " + error + " error sending " + dataName + ".");
if (singleCommand != "")
{
command = singleCommand;
}
else
{
command = OnWriteCommand(sentCommandIndex);
}
serial.WriteLine(command);
startTimer(ref sendingCommands);
}
else
{
OnProgressChanged(commType, progress, "Configuration: " + error + " error sending " + dataName + ".");
OnCompleted(commType, false, sentCommandIndex);
}
}
//Send the next command if there is one
else if (commType == CommType.SENDING)
{
for (sentCommandIndex++; sentCommandIndex < commandsToSend; sentCommandIndex++)
{
if (serialWriteCommand(sentCommandIndex))
{
break;
}
}
}
//Single command completed
else
{
sentCommandIndex = commandsToSend;
}
mavPixReply = "";
}
//All commands sent?
if (sentCommandIndex == commandsToSend)
{
//All commands sent successfully
stopTimer();
OnProgressChanged(commType, 32, "Configuration: Sent all data OK.");
OnCompleted(commType, true, sentCommandIndex - 1);
}
}
}
