/// <summary>
/// Close the serial port
/// </summary>
/// <param name="close"></param>
private void CommCloseHandler(Close close)
{
try
{
if (_commState.SerialPort != null)
{
if (_commState.SerialPort.IsOpen)
{
_commState.SerialPort.Close();
_state.Connected = false;
SendNotification <ConnectionUpdate>(_subMgrPort, new ConnectionUpdate(_state.Connected));
}
}
#region Respond to all pending requests and responses
Fault closingFault = Fault.FromException(new IOException("Connection is closing"));
SendCommand pendingCommand;
// respond to pending priority requests
while (_legoPriorityRequestResponsePort.ItemCount > 0)
{
_legoPriorityRequestResponsePort.Test(out pendingCommand);
if (pendingCommand != null && pendingCommand.ResponsePort != null)
{
pendingCommand.ResponsePort.Post(closingFault);
}
}
// respond to pending standard requests
while (_legoRequestResponsePort.ItemCount > 0)
{
_legoRequestResponsePort.Test(out pendingCommand);
if (pendingCommand != null && pendingCommand.ResponsePort != null)
{
pendingCommand.ResponsePort.Post(closingFault);
}
}
// respond to pending responses
while (_commState.PendingRequests.Count > 0)
{
SendCommandRequest pendingRequest = _commState.PendingRequests.Pop();
if (pendingRequest.InternalResponsePort != null)
{
pendingRequest.InternalResponsePort.Post(closingFault);
}
}
#endregion
_commState.ConsecutivePriorityRequests = 0;
if (close != null && close.ResponsePort != null)
{
close.ResponsePort.Post(DefaultSubmitResponseType.Instance);
}
}
catch (Exception ex)
{
if (close != null && close.ResponsePort != null)
{
close.ResponsePort.Post(Fault.FromException(ex));
}
}
}
/// <summary>
/// Wait for a response on the serial port.
/// </summary>
/// <param name="getResponse"></param>
/// <returns></returns>
private IEnumerator <ITask> CommGetResponseHandler(GetResponse getResponse)
{
string errorMessage = null;
SendCommandRequest cmdRequest = _commState.PendingRequests.Peek();
if (cmdRequest == null)
{
yield break;
}
#region If the data isn't ready yet, wait a little and try again.
if (_commState.SerialPort.BytesToRead == 0)
{
TimeSpan elapsed = (cmdRequest.Stopwatch == null) ? TimeSpan.MinValue : cmdRequest.Stopwatch.Elapsed;
TimeSpan remaining;
if (elapsed != TimeSpan.MinValue &&
elapsed < cmdRequest.MinExpectedTimeSpan)
{
// Wait until the minimum expected time for this command.
remaining = cmdRequest.MinExpectedTimeSpan.Subtract(elapsed);
}
else if (elapsed.TotalSeconds < 1.0)
{
// No data yet, wait 3 milliseconds
remaining = new TimeSpan(0, 0, 0, 0, 3);
}
else
{
// Timeout has occurred
// Remove from the pending list
_commState.PendingRequests.Pop();
_commState.ConsecutiveReadTimeouts++;
if (_commState.ConsecutiveReadTimeouts > 5)
{
// Several read timeouts in a row means the serial port is no longer connected.
_state.Connected = false;
SendNotification <ConnectionUpdate>(_subMgrPort, new ConnectionUpdate(_state.Connected));
}
errorMessage = "Timeout receiving data from the LEGO NXT.";
if (cmdRequest.InternalResponsePort != null)
{
Fault faultResponse = Fault.FromException(new IOException(errorMessage));
cmdRequest.InternalResponsePort.Post(faultResponse);
}
else
{
LogError(errorMessage);
}
yield break;
}
// Leave the exclusive handler,
// but wake up in a little bit and try again.
Activate(Arbiter.Receive(false, TimeoutPort(remaining),
delegate(DateTime timeout)
{
_commGetResponsePort.Post(GetResponse.Instance);
}));
yield break;
}
// When data starts to come in, clear the read timeout counter.
if (_commState.ConsecutiveReadTimeouts > 0)
{
_commState.ConsecutiveReadTimeouts = 0;
}
#endregion
// See if there is data on the serial port.
// if not, post back to _commGetResponsePort
// otherwise read it and send it back
LegoCommand cmd = cmdRequest.LegoCommand;
bool resetComm = false;
try
{
int packetSize = cmd.ExpectedResponseSize;
if (_state.ConnectOverBluetooth)
{
packetSize = _commState.SerialPort.ReadByte() + (_commState.SerialPort.ReadByte() * 256);
if (packetSize != cmd.ExpectedResponseSize)
{
errorMessage = "Bluetooth header does not match the expected LEGO Command response size.";
resetComm = true;
}
}
// Read the data and get a response packet.
byte[] receiveData = new byte[packetSize];
_commState.SerialPort.Read(receiveData, 0, packetSize);
#region Timing Stats
if (cmdRequest.Stopwatch != null)
{
cmdRequest.Stopwatch.Stop();
_CodeTimerStats.Add(new CodeTimer(cmd.LegoCommandCode, cmdRequest.Stopwatch.Elapsed.TotalMilliseconds * 1000.0));
if (_CodeTimerStats.Count > 20)
{
ProcessCommunicationStats();
}
}
#endregion
LegoResponse legoReceive = cmd.GetResponse(receiveData);
byte commandType = (byte)(receiveData[0] & 0x7F);
// Is this a valid starting type?
if (commandType != 0x00 && commandType != 0x01 && commandType != 0x02)
{
errorMessage = string.Format("Invalid LEGO response command: {0}", commandType);
resetComm = true;
}
else
{
// Data is received successfully
// Remove from the pending list
_commState.PendingRequests.Pop();
if (!_state.Connected)
{
_state.Connected = true;
SendNotification <ConnectionUpdate>(_subMgrPort, new ConnectionUpdate(_state.Connected));
}
if (cmdRequest.InternalResponsePort != null)
{
cmdRequest.InternalResponsePort.Post(legoReceive);
}
yield break;
}
}
catch (ArgumentException ex)
{
resetComm = true;
if (ex.Message == "Offset and length were out of bounds for the array or count is greater than the number of elements from index to the end of the source collection.")
{
errorMessage = "A connection error occured which may be caused by an invalid Baud Rate";
}
else
{
errorMessage = "A connection error occured while accessing the LEGO NXT serial port";
}
}
catch (TimeoutException)
{
// Ignore timeouts for now.
errorMessage = "Timeout reading from LEGO NXT brick";
resetComm = true;
}
catch (IOException ex)
{
errorMessage = string.Format("Error reading from the serial port in NxtComm(): {0}", ex);
resetComm = true;
}
catch (Exception ex)
{
errorMessage = string.Format("Error reading from the serial port in NxtComm(): {0}", ex);
resetComm = true;
}
// Some error has occurred
// Remove from the pending list
_commState.PendingRequests.Pop();
if (resetComm)
{
// A serious error occurred
LogInfo(LogGroups.Console, "Resetting LEGO Serial Port");
// Wait for remaining bytes
yield return(Arbiter.Receive(false, TimeoutPort(300), delegate(DateTime timeout) { }));
// Clear the serial port buffer
_commState.SerialPort.DiscardInBuffer();
}
if (string.IsNullOrEmpty(errorMessage))
{
errorMessage = "Invalid or missing response data from LEGO NXT.";
}
if (cmdRequest.InternalResponsePort != null)
{
Fault faultResponse = Fault.FromException(new IOException(errorMessage));
cmdRequest.InternalResponsePort.Post(faultResponse);
}
else
{
LogError(errorMessage);
}
yield break;
}