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