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();
}
}
}
