public virtual IEnumerator <ITask> SendCommandHandler(SendCommand sendCommand)
{
LegoCommand cmd = sendCommand.Body.LegoCommand;
if (buffer == null || _commState.SerialPort == null || !_commState.SerialPort.IsOpen)
{
sendCommand.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Not Connected")));
yield break;
}
if (sendCommand.Body == null || sendCommand.Body.LegoCommand == null)
{
sendCommand.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Invalid LEGO Command")));
yield break;
}
// Dispatch again to one of three different ports:
// If no response is required, send the command immediately
// Otherwise queue into a low or high priority queue to
// be handled one command/response at a time.
if (!sendCommand.Body.LegoCommand.RequireResponse)
{
_commSendImmediatePort.Post(sendCommand);
}
else if (sendCommand.Body.PriorityRequest)
{
_legoPriorityRequestResponsePort.Post(sendCommand);
}
else
{
_legoRequestResponsePort.Post(sendCommand);
}
yield break;
}
/// <summary>
/// Post Brick Send Command and return the response port.
/// </summary>
/// <param name="cmd"></param>
/// <param name="priorityRequest"></param>
/// <returns></returns>
public virtual PortSet <LegoResponse, Fault> SendCommand(LegoCommand cmd, bool priorityRequest)
{
SendCommand op = new SendCommand();
op.Body = new SendCommandRequest(cmd, priorityRequest);
this.PostUnknownType(op);
return(op.ResponsePort);
}
/// <summary>
/// DO NOT USE THIS COMMAND DIRECTLY.
/// In LegoNxt.cs, post a message to _legoCommWithAckPort or _legoCommNoAckPort.
/// </summary>
/// <param name="cmd"></param>
/// <returns></returns>
internal LegoResponse SendCommand(LegoCommand cmd)
{
LegoResponse response = null;
if (buffer != null && serialPort != null && serialPort.IsOpen)
{
// Add the bluetooth packet length
int ixBuffer = 0;
// dataLength = commandType + commandCode + Data
int packetLength = (2 + ((cmd.Data == null) ? 0 : cmd.Data.Length));
buffer[ixBuffer++] = (byte)(packetLength % 256);
buffer[ixBuffer++] = (byte)(packetLength / 256);
// Add the bluetooth header bytes to get an actual packet count
packetLength += 2;
buffer[ixBuffer++] = (byte)(cmd.CommandType + ((!cmd.RequireResponse) ? 0x80 : 0x00));
buffer[ixBuffer++] = (byte)cmd.LegoCommandCode;
// When requesting a response, clear the inbound buffer
if (cmd.RequireResponse && serialPort.BytesToRead > 0)
{
serialPort.DiscardInBuffer();
}
int dataLength = (cmd.Data == null) ? 0 : cmd.Data.Length;
int ixData = 0;
while (ixData < dataLength || ixBuffer > 0)
{
// Copy up to serialBufferLength from Data
while (ixBuffer < serialBufferLength && ixData < dataLength)
{
buffer[ixBuffer++] = cmd.Data[ixData++];
}
try
{
serialPort.Write(buffer, 0, ixBuffer);
ixBuffer = 0;
}
catch
{
_legoService.LogConsoleError("Serial Port Timeout.");
ShowLegoHelp(_legoService);
return(null);
}
}
response = GetCommandResponse(cmd);
}
return(response);
}
public SendLegoCommand(LegoCommand cmd)
{
this.Body = cmd;
}
/// <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;
}
/// <summary>
/// Lego Command Request
/// </summary>
/// <param name="legoCommand"></param>
/// <param name="priorityRequest"></param>
public SendCommandRequest(LegoCommand legoCommand, bool priorityRequest)
{
this.LegoCommand = legoCommand;
this.PriorityRequest = priorityRequest;
}
/// <summary>
/// Lego Command Request
/// </summary>
/// <param name="legoCommand"></param>
public SendCommandRequest(LegoCommand legoCommand)
{
this.LegoCommand = legoCommand;
}
/// <summary>
/// Read Serial Port for a LEGO response
/// </summary>
/// <param name="cmd"></param>
private LegoResponse GetCommandResponse(LegoCommand cmd)
{
if (cmd != null && cmd.RequireResponse && serialPort != null && serialPort.IsOpen)
{
int wait = 0;
while (serialPort.BytesToRead == 0 && wait < 10)
{
wait++;
if (wait < 10)
{
System.Threading.Thread.Sleep(100);
}
else
{
if (serialPort.CtsHolding && serialPort.DsrHolding)
{
// We might be connected in the wrong direction
_legoService.LogConsoleError("Have you connected from the LEGO NXT to the PC?");
ShowLegoHelp(_legoService);
}
}
}
try
{
int btLength = serialPort.ReadByte() + (serialPort.ReadByte() * 256);
byte commandType = (byte)serialPort.ReadByte();
LegoHelper.LegoCommandCode command = (LegoHelper.LegoCommandCode)serialPort.ReadByte();
btLength -= 2;
if (cmd.LegoCommandCode != command)
{
// Garbled response.
// Wait for remaining bytes
// return an error
System.Threading.Thread.Sleep(500);
serialPort.DiscardInBuffer();
IOException ex1 = new IOException("Garbled data received from LEGO NXT. LEGO response code does not match LEGO command.");
return(new LegoResponseException(cmd, ex1));
}
LegoResponse legoReceive = new LegoResponse(commandType, command, btLength);
if (btLength > 0)
{
legoReceive.Data = new byte[btLength];
serialPort.Read(legoReceive.Data, 0, btLength);
}
// Is this a valid starting type?
if (commandType != 0x00 && commandType != 0x01 &&
commandType != 0x02 && commandType != 0x80 &&
commandType != 0x81)
{
_legoService.LogConsoleInfo(string.Format("Invalid LEGO response command: {0}", commandType));
}
return(legoReceive);
}
catch (ArgumentException ex)
{
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.")
{
_legoService.LogConsoleError("A connection error occured which may be caused by an invalid Baud Rate");
IOException ex3 = new IOException("Invalid Baud Rate");
return(new LegoResponseException(cmd, ex3));
}
else
{
_legoService.LogConsoleError("A connection error occured while accessing the LEGO NXT serial port: " + ex.Message);
return(new LegoResponseException(cmd, ex));
}
}
catch (TimeoutException ex)
{
// Ignore timeouts for now.
_legoService.LogConsoleInfo("Timeout reading from LEGO NXT brick");
return(new LegoResponseException(cmd, ex));
}
catch (IOException ex)
{
_legoService.LogConsoleError(string.Format("Error reading from the serial port in CommBase(): {0}", ex.Message));
return(new LegoResponseException(cmd, ex));
}
catch (Exception ex)
{
_legoService.LogConsoleError(string.Format("Error reading from the serial port in CommBase(): {0}", ex.Message));
return(new LegoResponseException(cmd, ex));
}
}
LegoResponse response = new LegoResponse(2, cmd.LegoCommandCode, 1);
if (cmd == null || cmd.RequireResponse)
{
response.ErrorCode = LegoErrorCode.UnknownStatus;
if (cmd == null)
{
_legoService.LogConsoleError("Invalid LEGO Command: null");
}
else if (serialPort == null || !serialPort.IsOpen)
{
_legoService.LogConsoleWarning(string.Format("Unable to retrieve response from LEGO command {0} because the serial port is not open.", cmd.LegoCommandCode));
}
}
return(response);
}
/// <summary>
/// Send Any Command, including a two-phase LowSpeed command (LSWrite)
/// </summary>
/// <param name="cmd"></param>
/// <param name="priority"></param>
/// <returns></returns>
public PortSet<LegoResponse, Fault> SendAnyCommand(LegoCommand cmd, bool priority)
{
if (cmd.LegoCommandCode == LegoCommandCode.LSWrite)
{
LegoLSWrite cmdLsWrite = cmd as LegoLSWrite;
if (cmdLsWrite == null)
cmdLsWrite = new LegoLSWrite(cmd);
// Send Low Speed Command
SendLowSpeedCommand sendCmd = new SendLowSpeedCommand(cmdLsWrite);
_internalI2CPort.Post(sendCmd);
return sendCmd.ResponsePort;
}
return _brickPort.SendCommand(cmd, priority);
}
