/** * Code for thread that reads range values from the serial port. * * <p>Values are in the form RxxxxCR where: R is character 'R' xxxx is 3 or 4 decimal digits CR is * a carriage return character */ @Override public void run() { // Reset serial port to empty buffers: serialPort.reset(); // Start with an empty character buffer: byte[] data = new byte[0]; int index = 0; int value = 0; boolean startCharacterFound = false; // Loop forever reading and processing characters from the serial port. while (true) { // If all previously read characters have been processed, read more characters: if (index >= data.length) { data = serialPort.read(serialPort.getBytesReceived()); if (0 == data.length) { continue; } index = 0; } // Process a single character: byte c = data[index++]; if (startCharacterFound) { // If we have seen a 'R', look for a CR or digit: if ('\r' == c) { serialRangeCm = value / serialRangeFactorCm; value = 0; startCharacterFound = false; serialSampleRate.addSample(); } else { // Add current digit to value being accumulated: value = value * 10 + (c - '0'); } } else { // See if character is the 'R' indicating the start of a value: startCharacterFound = 'R' == c; } } }
/** * Constructs the IMU class, overriding the default update rate with a custom rate which may be * from 4 to 100, representing the number of updates per second sent by the nav6 IMU. * * <p>Note that increasing the update rate may increase the CPU utilization. * * @param serial_port BufferingSerialPort object to use * @param update_rate_hz Custom Update Rate (Hz) */ public IMU(SerialPort serial_port, byte update_rate_hz) { ypr_update_data = new IMUProtocol.YPRUpdate(); this.update_rate_hz = update_rate_hz; flags = 0; accel_fsr_g = DEFAULT_ACCEL_FSR_G; gyro_fsr_dps = DEFAULT_GYRO_FSR_DPS; this.serial_port = serial_port; yaw_history = new float[YAW_HISTORY_LENGTH]; yaw = (float) 0.0; pitch = (float) 0.0; roll = (float) 0.0; try { serial_port.reset(); } catch (RuntimeException ex) { ex.printStackTrace(); } initIMU(); m_thread = new Thread(this); m_thread.start(); }
public void run() { stop = false; boolean stream_response_received = false; double last_valid_packet_time = 0.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; double last_stream_command_sent_timestamp = 0.0; int updates_in_last_second = 0; double last_second_start_time = 0; try { serial_port.setReadBufferSize(512); serial_port.setTimeout(1.0); serial_port.enableTermination('\n'); serial_port.flush(); serial_port.reset(); } catch (RuntimeException ex) { ex.printStackTrace(); } IMUProtocol.StreamResponse response = new IMUProtocol.StreamResponse(); byte[] stream_command = new byte[256]; 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); serial_port.flush(); port_reset_count++; // SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count); last_stream_command_sent_timestamp = Timer.getFPGATimestamp(); } catch (RuntimeException ex) { ex.printStackTrace(); } while (!stop) { try { // Wait, with delays to conserve CPU resources, until // bytes have arrived. while (!stop && (serial_port.getBytesReceived() < 1)) { Timer.delay(1.0 / update_rate_hz); } int packets_received = 0; byte[] received_data = serial_port.read(256); int bytes_read = received_data.length; if (bytes_read > 0) { byte_count += bytes_read; 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++; // SmartDashboard.putNumber("nav6 Discarded Bytes", (double)discarded_bytes_count); continue; } else { if ((bytes_remaining > 2) && (received_data[i + 1] == (byte) '#')) { /* 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) { System.arraycopy(received_data, 0, c, 0, received_data.length); System.arraycopy( 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++; // SmartDashboard.putNumber("nav6 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) { // SmartDashboard.putNumber("nav6 UpdatesPerSec", (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++; setStreamResponse(response); stream_response_received = true; i += packet_length; stream_response_receive_count++; // SmartDashboard.putNumber("nav6 Stream Responses", // (double)stream_response_receive_count); } else { // current index is not the start of a valid packet; increment i++; } } } 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.reset(); port_reset_count++; // SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count); } // If a stream configuration response has not been received within three seconds // of operation, (re)send a stream configuration request if (!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 { last_stream_command_sent_timestamp = Timer.getFPGATimestamp(); serial_port.write(stream_command, cmd_packet_length); serial_port.flush(); } catch (RuntimeException ex2) { ex2.printStackTrace(); } } else { // If no bytes remain in the buffer, and not awaiting a response, sleep a bit if (stream_response_received && (serial_port.getBytesReceived() == 0)) { Timer.delay(1.0 / update_rate_hz); } } /* 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) { stream_response_received = false; } } } catch (RuntimeException ex) { // This exception typically indicates a Timeout stream_response_received = false; timeout_count++; // SmartDashboard.putNumber("nav6 Serial Port Timeouts", (double)timeout_count); // SmartDashboard.putString("LastNavExceptionBacktrace",ex.getStackTrace().toString()); // SmartDashboard.putString("LastNavException", ex.getMessage() + "; " + ex.toString()); } } }