protected void initIMU() {
// The nav6 IMU serial port configuration is 8 data bits, no parity, one stop bit.
// No flow control is used.
// Conveniently, these are the defaults used by the WPILib's SerialPort class.
//
// In addition, the WPILib's SerialPort class also defaults to:
//
// Timeout period of 5 seconds
// Termination ('\n' character)
// Transmit immediately
initializeYawHistory();
user_yaw_offset = 0;
// set the nav6 into the desired update mode
byte stream_command_buffer[] = new byte[256];
int packet_length =
IMUProtocol.encodeStreamCommand(stream_command_buffer, update_type, update_rate_hz);
try {
serial_port.write(stream_command_buffer, packet_length);
} catch (RuntimeException ex) {
ex.printStackTrace();
}
}
protected int decodePacketHandler(byte[] received_data, int offset, int bytes_remaining) {
int packet_length =
IMUProtocol.decodeYPRUpdate(received_data, offset, bytes_remaining, ypr_update_data);
if (packet_length > 0) {
setYawPitchRoll(
ypr_update_data.yaw,
ypr_update_data.pitch,
ypr_update_data.roll,
ypr_update_data.compass_heading);
}
return packet_length;
}
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());
}
}
}
