void SendData( const void *buf, size_t bytesToWrite )
{
if ( gDebug )
{
DumpMem( "Send", 0, buf, bytesToWrite );
}
gSerialPort.Write( buf, bytesToWrite );
} // SendData
void SendByte( unsigned char ch )
{
if ( gDebug )
{
Log( "Sending: 0x%02x\n", ch );
}
gSerialPort.Write( &ch, 1 );
} // SendByte
int main(int argc, char *argv[])
{
const char default_path[] = "/dev/ttyUSB2";
const char *path;
if(argc < 2)
path = default_path;
else
path = argv[1];
char buf[1024];
SerialPort sp;
sp.Init();
sp.Open(path, 9600, 8, 1, 'n');
while(1)
{
cout<<"Please key in data"<<endl;
cin>>buf;
sp.Write(buf, strlen(buf)+1);
}
return 0;
}
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;
}
}
}
void *ReadStdinThread( void *param )
{
(void)param;
while ( 1 )
{
char ch;
if ( read( fileno( stdin ), &ch, 1 ) < 1 )
{
LogDebug( "read of stdin failed\n" );
break;
}
gKeyboardInputReceived = true;
#if USE_I2C
if ( gDongle )
{
if ( gSerialDongleAddr == 0 )
{
Log( "Retrieving i2c address of serial dongle ...\n" );
if ( !gSerialDongle.GetI2CAddress( &gSerialDongleAddr ))
{
LogError( "Unable to retrieve serial dongle i2c address\n" );
break;
}
Log( " Serial Dongle i2c Address: 0x%02x\n", gSerialDongleAddr );
}
switch ( ch )
{
case 'c':
{
LogDebug( "Call\n" );
I2C_Adapter::Buffer writeBuf;
I2C_Adapter::Buffer readBuf;
char writeMem[ 40 ];
char readMem[ 40 ];
writeMem[ 0 ] = gSerialDongleAddr;
sprintf( &writeMem[ 1 ], "-c-%d-c-", gCounter++ );
writeBuf.data = (unsigned char *)writeMem;
writeBuf.dataLen = strlen( writeMem ) + 2; // +1 for i2c addr +1 for terminating null
readBuf.data = (unsigned char *)readMem;
readBuf.dataLen = sizeof( readMem );
if ( gSerialDongle.Call( gSlaveAddr, BL_CMD_TEST_CALL, &writeBuf, &readBuf ))
{
printf( "Call: Wrote: '%s' to 0x%02x, read '%s' from 0x%02x\n", &writeMem[ 1 ], writeMem[ 0 ], &readMem[ 1 ], readMem[ 0 ] );
}
else
{
printf( "Call: Wrote: '%s' to 0x%02x, read failed\n", &writeMem[ 1 ], writeMem[ 0 ] );
}
break;
}
case 'd':
{
LogDebug( "Download\n" );
if ( gDownloadFileName == NULL )
{
LogError( "No filename specified to download\n" );
break;
}
I2C_BootLoader bootLoader( &gSerialDongle, gSlaveAddr );
if ( !bootLoader.DownloadFile( gDownloadInfo ))
{
LogError( "Unable to download file\n" );
}
break;
}
case 'i':
{
LogDebug( "GetBootInfo\n" );
I2C_BootLoader bootLoader( &gSerialDongle, gSlaveAddr );
BootLoaderInfo_t bootInfo;
if ( bootLoader.GetBootLoaderInfo( &bootInfo ))
{
bootLoader.PrintBootLoaderInfo( bootInfo );
}
else
{
LogError( "Unable to retrieve bootloader info\n" );
}
break;
}
case 'r':
{
LogDebug( "Read\n" );
I2C_Adapter::Buffer readBuf;
char readMem[ 40 ];
readBuf.data = (unsigned char *)readMem;
readBuf.dataLen = sizeof( readMem );
if ( gSerialDongle.Read( gSlaveAddr, BL_CMD_TEST_READ, &readBuf ))
{
printf( "Read: '%s' from 0x%02x\n", &readMem[ 1 ], readMem[ 0 ] );
}
else
{
printf( "Read: failed\n" );
}
break;
}
case 'w':
{
LogDebug( "Write\n" );
I2C_Adapter::Buffer writeBuf;
char writeMem[ 40 ];
writeMem[ 0 ] = gSerialDongleAddr;
sprintf( &writeMem[ 1 ], "-w-%d-w-", gCounter++ );
writeBuf.data = (unsigned char *)writeMem;
writeBuf.dataLen = strlen( writeMem ) + 2; // +1 for i2c addr, +1 for temrinating null
printf( "Write: Wrote: '%s' to 0x%02x\n", &writeMem[ 1 ], writeMem[ 0 ] );
if ( !gSerialDongle.Write( gSlaveAddr, BL_CMD_TEST_WRITE, &writeBuf ))
{
printf( "Write: failed\n" );
}
break;
}
default:
{
printf( "Rcvd: 0x%02x\n", ch );
//gSerialPort.Write( &ch, 1 );
break;
}
}
}
else
#endif
{
gSerialPort.Write( &ch, 1 );
}
}
} // ReadStdinThread
