/// <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); }
/// <summary> /// Initialization Constructor /// </summary> public UpdateResponse(LegoResponse body) { this.Body = body; }
/// <summary> /// Send data immediately to the serial port. /// </summary> /// <param name="sendCommand"></param> /// <returns></returns> private IEnumerator <ITask> CommSendImmediateHandler(SendCommand sendCommand) { Fault fault = null; if (_commState.SerialPort == null || !_commState.SerialPort.IsOpen) { fault = Fault.FromException(new NullReferenceException("The Lego Serial Port is disconnected.")); } else if (sendCommand == null || sendCommand.Body == null || sendCommand.Body.LegoCommand == null || sendCommand.Body.LegoCommand.CommandData == null || sendCommand.Body.LegoCommand.CommandData.Length == 0) { fault = Fault.FromException(new NullReferenceException("Invalid Lego Command")); } if (fault != null) { if (sendCommand.Body.InternalResponsePort != null) { sendCommand.Body.InternalResponsePort.Post(fault); } else if (sendCommand.ResponsePort != null) { sendCommand.ResponsePort.Post(fault); } yield break; } byte[] data = sendCommand.Body.LegoCommand.CommandData; int packetLength = data.Length; try { if ((sendCommand.Body.LegoCommand.RequireResponse) && (sendCommand.Body.Stopwatch != null)) { sendCommand.Body.Stopwatch.Start(); } if (_state.ConnectOverBluetooth) { // Add the bluetooth packet length buffer[0] = (byte)(packetLength % 256); buffer[1] = (byte)(packetLength / 256); // Write the bluetooth header _commState.SerialPort.Write(buffer, 0, 2); } _commState.SerialPort.Write(data, 0, packetLength); if (sendCommand.Body.LegoCommand.RequireResponse) { // calculate the minimum expected response time for this command. sendCommand.Body.MinExpectedTimeSpan = CalcMinWaitTimeSpan(sendCommand.Body.LegoCommand.LegoCommandCode); _commState.PendingRequests.Push(sendCommand.Body); if (_commState.PendingRequests.Count == 1) { _commGetResponsePort.Post(GetResponse.Instance); } // Signal intermediate completion if (sendCommand.Body.InternalResponsePort != null) { sendCommand.Body.InternalResponsePort.Post(EmptyValue.SharedInstance); } } else { // Send an empty success response. LegoResponse response = sendCommand.Body.LegoCommand.GetResponse(null); sendCommand.ResponsePort.Post(response); } } catch (Exception ex) { if (ex is System.TimeoutException) { // A write timeout means the serial port is no longer connected. _state.Connected = false; SendNotification <ConnectionUpdate>(_subMgrPort, new ConnectionUpdate(_state.Connected)); } fault = Fault.FromException(ex); if (sendCommand.Body.LegoCommand.RequireResponse) { if (sendCommand.Body.InternalResponsePort != null) { sendCommand.Body.InternalResponsePort.Post(fault); } } else // respond directly to the source { sendCommand.ResponsePort.Post(fault); } } }
/// <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> /// Construct a LEGO NXT Response packet for the specified request /// </summary> /// <param name="responseData"></param> /// <returns>A Generic Lego Response</returns> /// <remarks>Override this method to return a custom response</remarks> public virtual LegoResponse GetResponse(byte[] responseData) { var expectedResponseSize = (RequireResponse) ? ExpectedResponseSize : 3; var response = new LegoResponse(expectedResponseSize, LegoCommandCode, responseData); return response; }
/// <summary> /// Determine if the LEGO response packet was a request for Button state. /// </summary> /// <param name="legoResponse"></param> /// <returns></returns> public static bool IsValidButtonStateResponse(LegoResponse legoResponse) { if ((legoResponse == null) || (legoResponse.CommandData == null) || legoResponse.CommandData.Length != 13 || legoResponse.CommandType != LegoCommandType.Response // Lego Return Code || legoResponse.LegoCommandCode != LegoCommandCode.ReadIOMap || legoResponse.ErrorCode != LegoErrorCode.Success // success || legoResponse.CommandData[3] != 0x01 // offset byte 1 || legoResponse.CommandData[4] != 0x00 // offset byte 2 || legoResponse.CommandData[5] != 0x04 // offset byte 3 || legoResponse.CommandData[6] != 0x00 // offset byte 4 || legoResponse.CommandData[7] != 0x04 // data length byte 1 || legoResponse.CommandData[8] != 0x00 // data length byte 2 ) return false; return true; }
/// <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); }