public void Update(double elapsedMS)
{
if (!serialPortComm.IsOpen())
{
return;
}
serialPortComm.Update();
switch (State)
{
case EMgrState.IDLE:
{
if (MsgQueue.Count > 0)
{
// get message and execute (do not remove, will be removed when execution is successful)
MsgInExecution = MsgQueue.Peek();
// execute
serialPortComm.Send(MsgInExecution.Type, MsgInExecution.Data);
CommandTimoutTimerMS = COMMANDTIMEOUTMS;
ValidResponseReceivedFlag = false; // wait for response
State = EMgrState.WAIT_RESPONSE;
}
else
{
// queue empty, inject PING command!
SMsg msg = new SMsg();
msg.Type = 0x10; // PING command
msg.Data = new byte[] { 0 };
MsgQueue.Enqueue(msg);
}
break;
}
case EMgrState.WAIT_RESPONSE:
{
if (ValidResponseReceivedFlag)
{
// got response, remove message, go IDLE
MsgQueue.Dequeue();
State = EMgrState.IDLE;
}
else
{
// check timout
CommandTimoutTimerMS -= elapsedMS;
if (CommandTimoutTimerMS < 0)
{
TimeoutCounter++;
// resend command, go idle without removing from queue
State = EMgrState.IDLE;
}
}
break;
}
}
}
public void QueueMsg(byte type, byte[] msgToSend)
{
// Add MSG to queue, schedule for execution
SMsg msg = new SMsg();
msg.Type = type;
msg.Data = msgToSend;
MsgQueue.Enqueue(msg);
}
