protected void DispatchStreamResponse(IMUProtocol.StreamResponse response) { board_state.CalStatus = (byte)(response.flags & IMUProtocol.NAV6_FLAG_MASK_CALIBRATION_STATE); board_state.CapabilityFlags = (short)(response.flags & ~IMUProtocol.NAV6_FLAG_MASK_CALIBRATION_STATE); board_state.OpStatus = 0x04; /* TODO: Create a symbol for this */ board_state.SelftestStatus = 0x07; /* TODO: Create a symbol for this */ board_state.AccelFsrG = response.accel_fsr_g; board_state.GyroFsrDps = response.gyro_fsr_dps; board_state.UpdateRateHz = (byte)response.update_rate_hz; notify_sink.SetBoardState(board_state); /* If AHRSPOS is update type is request, but board doesn't support it, */ /* retransmit the stream config, falling back to AHRS Update mode. */ if (this.update_type == AHRSProtocol.MSGID_AHRSPOS_UPDATE) { if (!board_capabilities.IsDisplacementSupported()) { this.update_type = AHRSProtocol.MSGID_AHRS_UPDATE; signal_retransmit_stream_config = true; } } }
public void Run() { m_stop = false; bool stream_response_received = false; double last_stream_command_sent_timestamp = 0.0; double last_data_received_timestamp = 0; double last_second_start_time = 0; int partial_binary_packet_count = 0; int stream_response_receive_count = 0; int timeout_count = 0; int discarded_bytes_count = 0; int port_reset_count = 0; int updates_in_last_second = 0; int integration_response_receive_count = 0; try { serial_port.ReadBufferSize = (256); serial_port.Timeout = (1.0); serial_port.EnableTermination('\n'); serial_port.Flush(); serial_port.Reset(); } catch (Exception ex) { Console.WriteLine(ex.StackTrace); } byte[] stream_command = new byte[256]; byte[] integration_control_command = new byte[256]; IMUProtocol.StreamResponse response = new IMUProtocol.StreamResponse(); AHRSProtocol.IntegrationControl integration_control = new AHRSProtocol.IntegrationControl(); AHRSProtocol.IntegrationControl integration_control_response = new AHRSProtocol.IntegrationControl(); int cmd_packet_length = IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz); try { serial_port.Reset(); serial_port.Write(stream_command, cmd_packet_length); cmd_packet_length = AHRSProtocol.encodeDataGetRequest(stream_command, AHRSProtocol.AHRS_DATA_TYPE.BOARD_IDENTITY, (byte)0); serial_port.Write(stream_command, cmd_packet_length); serial_port.Flush(); port_reset_count++; if (debug) { SmartDashboard.SmartDashboard.PutNumber("navX Port Resets", (double)port_reset_count); } last_stream_command_sent_timestamp = Timer.GetFPGATimestamp(); } catch (Exception ex) { Console.WriteLine(ex.StackTrace); } int remainder_bytes = 0; byte[] remainder_data = null; while (!m_stop) { try { // Wait, with delays to conserve CPU resources, until // bytes have arrived. if (signal_transmit_integration_control) { integration_control.action = next_integration_control_action; signal_transmit_integration_control = false; next_integration_control_action = 0; cmd_packet_length = AHRSProtocol.encodeIntegrationControlCmd(integration_control_command, integration_control); try { serial_port.Write(integration_control_command, cmd_packet_length); } catch (Exception ex2) { Console.WriteLine(ex2.StackTrace); } } if (!m_stop && (remainder_bytes == 0) && (serial_port.GetBytesReceived() < 1)) { double update_rate = 1.0 / ((double)((int)(this.update_rate_hz & 0xFF))); Timer.Delay(update_rate); } int packets_received = 0; byte[] received_data = serial_port.Read(256); int bytes_read = received_data.Length; byte_count += bytes_read; /* If a partial packet remains from last iteration, place that at */ /* the start of the data buffer, and append any new data available */ /* at the serial port. */ if (remainder_bytes > 0) { byte[] resized_array = new byte[remainder_bytes + bytes_read]; Array.Copy(remainder_data, 0, resized_array, 0, remainder_bytes); Array.Copy(received_data, 0, resized_array, remainder_bytes, bytes_read); received_data = resized_array; bytes_read += remainder_bytes; remainder_bytes = 0; remainder_data = null; } if (bytes_read > 0) { last_data_received_timestamp = Timer.GetFPGATimestamp(); int i = 0; // Scan the buffer looking for valid packets while (i < bytes_read) { // Attempt to decode a packet int bytes_remaining = bytes_read - i; if (received_data[i] != IMUProtocol.PACKET_START_CHAR) { /* Skip over received bytes until a packet start is detected. */ i++; discarded_bytes_count++; if (debug) { SmartDashboard.SmartDashboard.PutNumber("navX Discarded Bytes", (double)discarded_bytes_count); } continue; } else { if ((bytes_remaining > 2) && (received_data[i + 1] == AHRSProtocol.BINARY_PACKET_INDICATOR_CHAR)) { /* Binary packet received; next byte is packet length-2 */ byte total_expected_binary_data_bytes = received_data[i + 2]; total_expected_binary_data_bytes += 2; while (bytes_remaining < total_expected_binary_data_bytes) { /* This binary packet contains an embedded */ /* end-of-line character. Continue to receive */ /* more data until entire packet is received. */ byte[] additional_received_data = serial_port.Read(256); byte_count += additional_received_data.Length; bytes_remaining += additional_received_data.Length; /* Resize array to hold existing and new data */ byte[] c = new byte[received_data.Length + additional_received_data.Length]; if (c.Length > 0) { Array.Copy(received_data, 0, c, 0, received_data.Length); Array.Copy(additional_received_data, 0, c, received_data.Length, additional_received_data.Length); received_data = c; } else { /* Timeout waiting for remainder of binary packet */ i++; bytes_remaining--; partial_binary_packet_count++; if (debug) { SmartDashboard.SmartDashboard.PutNumber("navX Partial Binary Packets", (double)partial_binary_packet_count); } continue; } } } } int packet_length = DecodePacketHandler(received_data, i, bytes_remaining); if (packet_length > 0) { packets_received++; update_count++; last_valid_packet_time = Timer.GetFPGATimestamp(); updates_in_last_second++; if ((last_valid_packet_time - last_second_start_time) > 1.0) { if (debug) { SmartDashboard.SmartDashboard.PutNumber("navX Updates Per Sec", (double)updates_in_last_second); } updates_in_last_second = 0; last_second_start_time = last_valid_packet_time; } i += packet_length; } else { packet_length = IMUProtocol.decodeStreamResponse(received_data, i, bytes_remaining, response); if (packet_length > 0) { packets_received++; DispatchStreamResponse(response); stream_response_received = true; i += packet_length; stream_response_receive_count++; if (debug) { SmartDashboard.SmartDashboard.PutNumber("navX Stream Responses", (double)stream_response_receive_count); } } else { packet_length = AHRSProtocol.decodeIntegrationControlResponse(received_data, i, bytes_remaining, integration_control_response); if (packet_length > 0) { // Confirmation of integration control integration_response_receive_count++; if (debug) { SmartDashboard.SmartDashboard.PutNumber("navX Integration Control Response Count", integration_response_receive_count); } i += packet_length; if ((integration_control.action & AHRSProtocol.NAVX_INTEGRATION_CTL_RESET_YAW) != 0) { this.notify_sink.YawResetComplete(); } } else { /* Even though a start-of-packet indicator was found, the */ /* current index is not the start of a packet if interest. */ /* Scan to the beginning of the next packet, */ bool next_packet_start_found = false; int x; for (x = 0; x < bytes_remaining; x++) { if (received_data[i + x] != IMUProtocol.PACKET_START_CHAR) { x++; } else { i += x; bytes_remaining -= x; if (x != 0) { next_packet_start_found = true; } break; } } bool discard_remainder = false; if (!next_packet_start_found && x == bytes_remaining) { /* Remaining bytes don't include a start-of-packet */ discard_remainder = true; } bool partial_packet = false; if (discard_remainder) { /* Discard the remainder */ i = bytes_remaining; } else { if (!next_packet_start_found) { /* This occurs when packets are received that are not decoded. */ /* Bump over this packet and prepare for the next. */ if ((bytes_remaining > 2) && (received_data[i + 1] == AHRSProtocol.BINARY_PACKET_INDICATOR_CHAR)) { /* Binary packet received; next byte is packet length-2 */ int pkt_len = received_data[i + 2]; pkt_len += 2; if (bytes_remaining >= pkt_len) { bytes_remaining -= pkt_len; i += pkt_len; discarded_bytes_count += pkt_len; if (debug) { SmartDashboard.SmartDashboard.PutNumber("navX Discarded Bytes", (double)discarded_bytes_count); } } else { /* This is the initial portion of a partial binary packet. */ /* Keep this data and attempt to acquire the remainder. */ partial_packet = true; } } else { /* Ascii packet received. */ /* Scan up to and including next end-of-packet character */ /* sequence, or the beginning of a new packet. */ for (x = 0; x < bytes_remaining; x++) { if (received_data[i + x] == (byte)'\r') { i += x + 1; bytes_remaining -= (x + 1); discarded_bytes_count += x + 1; if ((bytes_remaining > 0) && received_data[i] == (byte)'\n') { bytes_remaining--; i++; discarded_bytes_count++; } if (debug) { SmartDashboard.SmartDashboard.PutNumber("navX Discarded Bytes", (double)discarded_bytes_count); } break; } /* If a new start-of-packet is found, discard */ /* the ascii packet bytes that precede it. */ if (received_data[i + x] == (byte)'!') { if (x > 0) { i += x; bytes_remaining -= x; discarded_bytes_count += x; break; } else { /* start of packet found, but no termination */ /* Time to get some more data, unless the bytes */ /* remaining are larger than a valid packet size */ if (bytes_remaining < IMUProtocol.IMU_PROTOCOL_MAX_MESSAGE_LENGTH) { /* Get more data */ partial_packet = true; } else { i++; bytes_remaining--; } break; } } } if (x == bytes_remaining) { /* Partial ascii packet - keep the remainder */ partial_packet = true; } } } } if (partial_packet) { remainder_data = new byte[bytes_remaining]; Array.Copy(received_data, i, remainder_data, 0, bytes_remaining); remainder_bytes = bytes_remaining; i = bytes_read; } } } } } if ((packets_received == 0) && (bytes_read == 256)) { // Workaround for issue found in SerialPort implementation: // No packets received and 256 bytes received; this // condition occurs in the SerialPort. In this case, // reset the serial port. serial_port.Flush(); serial_port.Reset(); port_reset_count++; if (debug) { SmartDashboard.SmartDashboard.PutNumber("navX Port Resets", (double)port_reset_count); } } bool retransmit_stream_config = false; if (signal_retransmit_stream_config) { retransmit_stream_config = true; signal_retransmit_stream_config = false; } // If a stream configuration response has not been received within three seconds // of operation, (re)send a stream configuration request if (retransmit_stream_config || (!stream_response_received && ((Timer.GetFPGATimestamp() - last_stream_command_sent_timestamp) > 3.0))) { cmd_packet_length = IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz); try { ResetSerialPort(); last_stream_command_sent_timestamp = Timer.GetFPGATimestamp(); serial_port.Write(stream_command, cmd_packet_length); cmd_packet_length = AHRSProtocol.encodeDataGetRequest(stream_command, AHRSProtocol.AHRS_DATA_TYPE.BOARD_IDENTITY, (byte)0); serial_port.Write(stream_command, cmd_packet_length); serial_port.Flush(); } catch (Exception ex2) { Console.WriteLine(ex2.StackTrace); } } else { // If no bytes remain in the buffer, and not awaiting a response, sleep a bit if (stream_response_received && (serial_port.GetBytesReceived() == 0)) { double update_rate = 1.0 / ((double)((int)(this.update_rate_hz & 0xFF))); Timer.Delay(update_rate); } } /* If receiving data, but no valid packets have been received in the last second */ /* the navX MXP may have been reset, but no exception has been detected. */ /* In this case , trigger transmission of a new stream_command, to ensure the */ /* streaming packet type is configured correctly. */ if ((Timer.GetFPGATimestamp() - last_valid_packet_time) > 1.0) { last_stream_command_sent_timestamp = 0.0; stream_response_received = false; } } else { /* No data received this time around */ if (Timer.GetFPGATimestamp() - last_data_received_timestamp > 1.0) { ResetSerialPort(); } } } catch (Exception ex) { // This exception typically indicates a Timeout, but can also be a buffer overrun error. stream_response_received = false; timeout_count++; if (debug) { SmartDashboard.SmartDashboard.PutNumber("navX Serial Port Timeout / Buffer Overrun", (double)timeout_count); SmartDashboard.SmartDashboard.PutString("navX Last Exception", ex.Message + "; " + ex.ToString()); } Console.WriteLine(ex.StackTrace); ResetSerialPort(); } } }