static void imuTask(IMU *imu)
{
char stream_type;
uint16_t gyro_fsr_dps, accel_fsr, update_rate_hz;
uint16_t q1_offset, q2_offset, q3_offset, q4_offset;
float yaw_offset_degrees;
uint16_t flags;
bool stream_response_received = false;
double last_valid_packet_time = 0.0;
int partial_binary_packet_count = 0;
int stream_response_receive_count = 0;
int stream_response_timeout_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;
SerialPort *pport = imu->GetSerialPort();
try {
pport->SetReadBufferSize(512);
pport->SetTimeout(1.0);
pport->EnableTermination('\n');
pport->Flush();
pport->Reset();
} catch(std::exception ex) {
}
int cmd_packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer,
imu->current_stream_type, imu->update_rate_hz );
try {
pport->Reset();
pport->Write( protocol_buffer, cmd_packet_length );
pport->Flush();
#ifdef DEBUG_IMU_RX
port_reset_count++;
SmartDashboard::PutNumber("nav6_PortResets", (double)port_reset_count);
#endif
last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
} catch (std::exception ex) {
}
while (!stop) {
try {
// Wait, with delays to conserve CPU resources, until
// bytes have arrived.
while ( !stop && ( pport->GetBytesReceived() < 1 ) ) {
delayMillis(1000/update_rate_hz);
}
int packets_received = 0;
uint32_t bytes_read = pport->Read( protocol_buffer, 256 );
if (bytes_read > 0) {
byte_count += bytes_read;
uint32_t 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 ( protocol_buffer[i] != PACKET_START_CHAR ) {
/* Skip over received bytes until a packet start is detected. */
i++;
#ifdef DEBUG_IMU_RX
discarded_bytes_count++;
SmartDashboard::PutNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
#endif
continue;
} else {
if ( ( bytes_remaining > 2 ) &&
( protocol_buffer[i+1] == '#' ) ) {
/* Binary packet received; next byte is packet length-2 */
uint8_t total_expected_binary_data_bytes = protocol_buffer[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. */
uint32_t additional_received_data_length = pport->Read(additional_received_data, 256);
if ( additional_received_data_length > 0 ) {
byte_count += additional_received_data_length;
memcpy(protocol_buffer + bytes_read, additional_received_data, additional_received_data_length);
bytes_read += additional_received_data_length;
bytes_remaining += additional_received_data_length;
} else {
/* Timeout waiting for remainder of binary packet */
i++;
bytes_remaining--;
#ifdef DEBUG_IMU_RX
partial_binary_packet_count++;
SmartDashboard::PutNumber("nav6 Partial Binary Packets", (double)partial_binary_packet_count);
#endif
continue;
}
}
}
}
int packet_length = imu->DecodePacketHandler(&protocol_buffer[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 ) {
#ifdef DEBUG_IMU_RX
SmartDashboard::PutNumber("nav6 UpdatesPerSec", (double)updates_in_last_second);
updates_in_last_second = 0;
#endif
last_second_start_time = last_valid_packet_time;
}
i += packet_length;
}
else
{
packet_length = IMUProtocol::decodeStreamResponse( &protocol_buffer[i], bytes_remaining, stream_type,
gyro_fsr_dps, accel_fsr, update_rate_hz,
yaw_offset_degrees,
q1_offset, q2_offset, q3_offset, q4_offset,
flags );
if ( packet_length > 0 )
{
packets_received++;
imu->SetStreamResponse( stream_type,
gyro_fsr_dps, accel_fsr, update_rate_hz,
yaw_offset_degrees,
q1_offset, q2_offset, q3_offset, q4_offset,
flags );
stream_response_received = true;
i += packet_length;
#ifdef DEBUG_IMU_RX
stream_response_receive_count++;
SmartDashboard::PutNumber("nav6 Stream Responses", (double)stream_response_receive_count);
#endif
}
else {
// current index is not the start of a valid packet; increment
i++;
#ifdef DEBUG_IMU_RX
discarded_bytes_count++;
SmartDashboard::PutNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
#endif
}
}
}
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.
pport->Reset();
#ifdef DEBUG_IMU_RX
port_reset_count++;
SmartDashboard::PutNumber("nav6_PortResets", (double)port_reset_count);
#endif
}
// 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 ) ) {
int cmd_packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer, imu->current_stream_type, imu->update_rate_hz );
try {
last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
pport->Write( protocol_buffer, cmd_packet_length );
pport->Flush();
} catch (std::exception ex2) {
#ifdef DEBUG_IMU_RX
stream_response_timeout_count++;
SmartDashboard::PutNumber("nav6 Stream Response Timeouts", (double)stream_response_timeout_count);
#endif
}
}
else {
// If no bytes remain in the buffer, and not awaiting a response, sleep a bit
if ( stream_response_received && ( pport->GetBytesReceived() == 0 ) ) {
delayMillis(1000/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 (std::exception ex) {
// This exception typically indicates a Timeout
stream_response_received = false;
#ifdef DEBUG_IMU_RX
timeout_count++;
SmartDashboard::PutNumber("nav6 Serial Port Timeouts", (double)timeout_count);
SmartDashboard::PutString("LastNavException", ex.what());
#endif
}
}
}
static void imuTask(IMU *imu)
{
bool stream_response_received = false;
double last_stream_command_sent_timestamp = 0.0;
stop = false;
SerialPort *pport = imu->GetSerialPort();
pport->SetReadBufferSize(512);
pport->SetTimeout(1.0);
pport->EnableTermination('\n');
pport->Flush();
pport->Reset();
char stream_type;
uint16_t gyro_fsr_dps, accel_fsr, update_rate_hz;
uint16_t q1_offset, q2_offset, q3_offset, q4_offset;
float yaw_offset_degrees;
uint16_t flags;
// Give the nav6 circuit a few seconds to initialize, then send the stream configuration command.
int cmd_packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer, imu->current_stream_type, imu->update_rate_hz );
pport->Reset();
pport->Write( protocol_buffer, cmd_packet_length );
pport->Flush();
last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
while (!stop)
{
uint32_t bytes_read = pport->Read( protocol_buffer, 256 );
if ( bytes_read > 0 )
{
int packets_received = 0;
byte_count += bytes_read;
uint32_t i = 0;
// Scan the buffer looking for valid packets
while ( i < bytes_read )
{
int bytes_remaining = bytes_read - i;
int packet_length = imu->DecodePacketHandler( &protocol_buffer[i], bytes_remaining );
if ( packet_length > 0 )
{
packets_received++;
update_count++;
i += packet_length;
}
else
{
packet_length = IMUProtocol::decodeStreamResponse( &protocol_buffer[i], bytes_remaining, stream_type,
gyro_fsr_dps, accel_fsr, update_rate_hz,
yaw_offset_degrees,
q1_offset, q2_offset, q3_offset, q4_offset,
flags );
if ( packet_length > 0 )
{
packets_received++;
imu->SetStreamResponse( stream_type,
gyro_fsr_dps, accel_fsr, update_rate_hz,
yaw_offset_degrees,
q1_offset, q2_offset, q3_offset, q4_offset,
flags );
stream_response_received = true;
i += packet_length;
}
else // current index is not the start of a valid packet we're interested in; increment
{
i++;
}
}
}
if ( ( packets_received == 0 ) && ( bytes_read == 256 ) ) {
// No packets received and 256 bytes received; this
// condition occurs in the SerialPort. In this case,
// reset the serial port.
pport->Reset();
}
if ( !stream_response_received && ((Timer::GetFPGATimestamp() - last_stream_command_sent_timestamp ) > 3.0 ) ) {
cmd_packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer, imu->current_stream_type, imu->update_rate_hz );
last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
pport->Write( protocol_buffer, cmd_packet_length );
pport->Flush();
}
}
else {
// This exception typically indicates a Timeout
stream_response_received = false;
}
}
}