char *StrMaxCat( char *dst, const char *src, size_t maxLen )
{
size_t dstLen = strlen( dst );
if ( dstLen < maxLen )
{
StrMaxCpy( &dst[ dstLen ], src, maxLen - dstLen );
}
return dst;
} /* StrMaxCat */
/**
* Prints the error code and prints the results.
*/
bool BioloidCommandLine::PrintError( Bioloid::Error err )
{
char *str = (char *)&gErrorBuf[0];
str[0] = '\0';
if ( err == Bioloid::ERROR_NONE )
{
return false;
}
if ( err > 0xff )
{
const char *errStr;
switch ( err )
{
case Bioloid::ERROR_NOT_DONE: errStr = "Not Done"; break;
case Bioloid::ERROR_TIMEOUT: errStr = "Timeout"; break;
case Bioloid::ERROR_TOO_MUCH_DATA: errStr = "Too Much Data"; break;
default: errStr = "***Unknown***"; break;
}
StrMaxCpy( str, errStr, sizeof( gErrorBuf ));
}
else
{
AddErrorStr( err, Bioloid::ERROR_RESERVED, str, sizeof( gErrorBuf ), "Reserved" );
AddErrorStr( err, Bioloid::ERROR_INSTRUCTION, str, sizeof( gErrorBuf ), "Instruction" );
AddErrorStr( err, Bioloid::ERROR_OVERLOAD, str, sizeof( gErrorBuf ), "Overload" );
AddErrorStr( err, Bioloid::ERROR_CHECKSUM, str, sizeof( gErrorBuf ), "Checksum" );
AddErrorStr( err, Bioloid::ERROR_RANGE, str, sizeof( gErrorBuf ), "Range" );
AddErrorStr( err, Bioloid::ERROR_OVERHEATING, str, sizeof( gErrorBuf ), "Over Heating" );
AddErrorStr( err, Bioloid::ERROR_ANGLE_LIMIT, str, sizeof( gErrorBuf ), "Angle Limit" );
AddErrorStr( err, Bioloid::ERROR_INPUT_VOLTAGE, str, sizeof( gErrorBuf ), "Input Voltage" );
}
Log( "%s\n", str );
return true;
}
bool SerialPort::Open( const char *inDevName, const char *param )
{
char devName[ 40 ];
unsigned baudRate;
devName[ 0 ] = '\0';
#if 1
if ( inDevName[ 0 ] != '/' )
{
StrMaxCpy( devName, "/dev/", sizeof( devName ));
}
#endif
StrMaxCat( devName, inDevName, sizeof( devName ));
// Translate the params, if any
speed_t speed = B0;
if ( param == NULL )
{
speed = B38400;
baudRate = 38400;
}
else
{
baudRate = atoi( param );
for ( unsigned i = 0; i < ARRAY_LEN( gBaudTable ); i++ )
{
if ( gBaudTable[ i ].baudRate == baudRate )
{
speed = gBaudTable[ i ].speed;
break;
}
}
if ( speed == B0 )
{
LogError( "Unrecognized baud rate: '%s'\n", param );
return false;
}
}
LogVerbose( "Port: '%s' Baud: %d\n", devName, baudRate );
if (( m_fd = open( devName, O_RDWR | O_EXCL )) < 0 )
{
LogError( "Unable to open serial port '%s': %s\n", devName, strerror( errno ));
return false;
}
// Setup the serial port
struct termios attr;
if ( tcgetattr( m_fd, &attr ) < 0 )
{
LogError( "A: Call to tcgetattr failed: %s\n", strerror( errno ));
return false;
}
attr.c_iflag = 0;
attr.c_oflag = 0;
attr.c_cflag = CLOCAL | CREAD | CS8;
attr.c_lflag = 0;
attr.c_cc[ VTIME ] = 0; // timeout in tenths of a second
attr.c_cc[ VMIN ] = 1;
cfsetispeed( &attr, speed );
cfsetospeed( &attr, speed );
if ( tcsetattr( m_fd, TCSAFLUSH, &attr ) < 0 )
{
LogError( "Call to tcsetattr failed: %s\n", strerror( errno ));
return false;
}
return true;
} // Open
int main( int argc, char **argv )
{
int sig;
int rc;
int opt;
char devName[ 40 ];
const char *baudStr = NULL;
const char *portStr = "ttyS2";
speed_t baudRate;
sigset_t termSig;
pthread_t readSerialThreadId;
pthread_t readStdinThreadId;
struct termios stdin_tio;
struct termios stdin_tio_org;
struct termios attr;
// Parse the command line options
while (( opt = getopt_long( argc, argv, "b:dhp:v", gLongOption, NULL )) > 0 )
{
switch ( opt )
{
case 'b':
{
baudStr = optarg;
break;
}
case 'd':
{
gDebug = 1;
break;
}
case 'p':
{
portStr = optarg;
break;
}
case 'v':
{
gVerbose = 1;
break;
}
case '?':
case 'h':
{
Usage();
return 1;
}
}
}
devName[ 0 ] = '\0';
if ( portStr[ 0 ] != '/' )
{
StrMaxCpy( devName, "/dev/", sizeof( devName ));
}
StrMaxCat( devName, portStr, sizeof( devName ));
baudRate = B0;
if ( baudStr == NULL )
{
baudRate = B9600;
}
else
{
int baudIdx;
int testBaud = atoi( baudStr );
for ( baudIdx = 0; baudIdx < ARRAY_LEN( gBaudTable ); baudIdx++ )
{
if ( gBaudTable[ baudIdx ].baudRate == testBaud )
{
baudRate = gBaudTable[ baudIdx ].speed;
break;
}
}
if ( baudRate == B0 )
{
fprintf( stderr, "Unrecognized baud rate: '%s'\n", baudStr );
exit( 1 );
}
}
if (( gPortFd = open( devName, O_RDWR | O_EXCL )) < 0 )
{
fprintf( stderr, "Unable to open serial port '%s': %s\n", devName, strerror( errno ));
exit( 2 );
}
if ( tcgetattr( gPortFd, &attr ) < 0 )
{
fprintf( stderr, "Call to tcgetattr failed: %s\n", strerror( errno ));
exit( 3 );
}
cfmakeraw( &attr );
// CLOCAL - Disable modem control lines
// CREAD - Enable Receiver
attr.c_cflag |= ( CLOCAL | CREAD );
cfsetispeed( &attr, baudRate );
cfsetospeed( &attr, baudRate );
if ( tcsetattr( gPortFd, TCSAFLUSH, &attr ) < 0 )
{
fprintf( stderr, "Call to tcsetattr failed: %s\n", strerror( errno ));
exit( 4 );
}
// Put stdin & stdout in unbuffered mode.
setbuf( stdin, NULL );
setbuf( stdout, NULL );
sigemptyset( &termSig );
sigaddset( &termSig, SIGINT );
sigaddset( &termSig, SIGTERM );
pthread_sigmask( SIG_BLOCK, &termSig, NULL );
// Put stdin in raw mode (i.e. turn off canonical mode). Canonical mode
// causes the driver to wait for the RETURN character so that line editing
// can take place. We also want to turn off ECHO.
if ( tcgetattr( fileno( stdin ), &stdin_tio_org ) < 0 )
{
fprintf( stderr, "Unable to retrieve terminal settings: %s\n", strerror( errno ));
exit( 5 );
}
stdin_tio = stdin_tio_org;
stdin_tio.c_lflag &= ~( ICANON | ECHO );
stdin_tio.c_cc[VTIME] = 0;
stdin_tio.c_cc[VMIN] = 1;
if ( tcsetattr( fileno( stdin ), TCSANOW, &stdin_tio ) < 0 )
{
fprintf( stderr, "Unable to update terminal settings: %s\n", strerror( errno ));
exit( 6 );
}
// Kick off the serial port reader thread.
rc = pthread_create( &readSerialThreadId, NULL, ReadSerialThread, NULL );
if ( rc != 0 )
{
fprintf( stderr, "Error creating ReadSerialThread: %s\n", strerror( rc ));
exit( 7 );
}
// Kick off the stdin reader thread
rc = pthread_create( &readStdinThreadId, NULL, ReadStdinThread, NULL );
if ( rc != 0 )
{
fprintf( stderr, "Error creating ReadStdinThread: %s\n", strerror( rc ));
exit( 7 );
}
// Wait for a termmination signal
if (( rc = sigwait( &termSig, &sig )) != 0 )
{
fprintf( stderr, "sigwait failed\n" );
}
else
{
fprintf( stderr, "Exiting...\n" );
}
pthread_cancel( readSerialThreadId );
pthread_cancel( readStdinThreadId );
// Restore stdin back to the way it was when we started
if ( tcsetattr( fileno( stdin ), TCSANOW, &stdin_tio_org ) < 0 )
{
fprintf( stderr, "Unable to update terminal settings: %s\n", strerror( errno ));
exit( 6 );
}
// Unblock the termination signals so the user can kill us if we hang up
// waiting for the reader threads to exit.
pthread_sigmask( SIG_UNBLOCK, &termSig, NULL );
pthread_join( readSerialThreadId, NULL );
pthread_join( readStdinThreadId, NULL );
close( gPortFd );
if ( gVerbose )
{
fprintf( stderr, "Done\n" );
}
exit( 0 );
return 0; // Get rid of warning about not returning anything
}
int main( int argc, char **argv )
{
int opt;
char devName[ 40 ];
const char *baudStr = NULL;
const char *portStr = "ttyS2";
speed_t baudRate;
struct termios stdin_tio;
struct termios stdin_tio_org;
fd_set read_fds;
struct termios attr;
// Parse the command line options
while (( opt = getopt_long( argc, argv, "b:dhp:v", gLongOption, NULL )) > 0 )
{
switch ( opt )
{
case 'b':
{
baudStr = optarg;
break;
}
case 'd':
{
gDebug = 1;
break;
}
case 'p':
{
portStr = optarg;
break;
}
case 'v':
{
gVerbose = 1;
break;
}
case '?':
case 'h':
{
Usage();
return 1;
}
}
}
devName[ 0 ] = '\0';
if ( portStr[ 0 ] != '/' )
{
StrMaxCpy( devName, "/dev/", sizeof( devName ));
}
StrMaxCat( devName, portStr, sizeof( devName ));
baudRate = B0;
if ( baudStr == NULL )
{
baudRate = B9600;
}
else
{
int baudIdx;
int testBaud = atoi( baudStr );
for ( baudIdx = 0; baudIdx < ARRAY_LEN( gBaudTable ); baudIdx++ )
{
if ( gBaudTable[ baudIdx ].baudRate == testBaud )
{
baudRate = gBaudTable[ baudIdx ].speed;
break;
}
}
if ( baudRate == B0 )
{
fprintf( stderr, "Unrecognized baud rate: '%s'\n", baudStr );
exit( 1 );
}
}
signal( SIGINT, ControlC );
signal( SIGTERM, ControlC );
// Open the serial port initially using O_NONBLOCK so that we won't block waiting for
// carrier detect.
if (( gPortFd = open( devName, O_RDWR | O_EXCL | O_NONBLOCK )) < 0 )
{
fprintf( stderr, "Unable to open serial port '%s': %s\n", devName, strerror( errno ));
exit( 2 );
}
// Now that the serial port is open, we can turn off the non-blocking behaviour (for us we want
// the reads to have blocking semantics).
fcntl( gPortFd, F_SETFL, fcntl( gPortFd, F_GETFL ) & ~O_NONBLOCK );
if ( tcgetattr( gPortFd, &attr ) < 0 )
{
fprintf( stderr, "Call to tcgetattr failed: %s\n", strerror( errno ));
exit( 3 );
}
cfmakeraw( &attr );
// CLOCAL - Disable modem control lines
// CREAD - Enable Receiver
attr.c_cflag |= ( CLOCAL | CREAD );
cfsetispeed( &attr, baudRate );
cfsetospeed( &attr, baudRate );
if ( tcsetattr( gPortFd, TCSAFLUSH, &attr ) < 0 )
{
fprintf( stderr, "Call to tcsetattr failed: %s\n", strerror( errno ));
exit( 4 );
}
// Put stdin & stdout in unbuffered mode.
setbuf( stdin, NULL );
setbuf( stdout, NULL );
// Put stdin in raw mode (i.e. turn off canonical mode). Canonical mode
// causes the driver to wait for the RETURN character so that line editing
// can take place. We also want to turn off ECHO.
if ( tcgetattr( fileno( stdin ), &stdin_tio_org ) < 0 )
{
fprintf( stderr, "Unable to retrieve terminal settings: %s\n", strerror( errno ));
exit( 5 );
}
stdin_tio = stdin_tio_org;
stdin_tio.c_lflag &= ~( ICANON | ECHO );
stdin_tio.c_cc[VTIME] = 0;
stdin_tio.c_cc[VMIN] = 1;
if ( tcsetattr( fileno( stdin ), TCSANOW, &stdin_tio ) < 0 )
{
fprintf( stderr, "Unable to update terminal settings: %s\n", strerror( errno ));
exit( 6 );
}
while (!gQuit) {
int nfds;
FD_ZERO(&read_fds);
FD_SET(fileno(stdin), &read_fds);
FD_SET(gPortFd, &read_fds);
nfds = gPortFd + 1;
if (select(nfds, &read_fds, NULL, NULL, NULL) < 0) {
if (errno != EINTR)
{
fprintf( stderr, "select failed: %s (%d)\n", strerror(errno), errno);
}
break;
}
if (FD_ISSET(fileno(stdin), &read_fds)) {
// Data available on stdin
char ch;
int bytesRead;
if (( bytesRead = read(fileno(stdin), &ch, 1 )) < 0 )
{
fprintf( stderr, "Read of stdin failed: %s\n", strerror( errno ));
exit( 1 );
}
if ( gDebug )
{
if (( ch < ' ' ) || ( ch > '~' ))
{
fprintf( stderr, "Stdin Read: 0x%02x '.'\n", ch );
}
else
{
fprintf( stderr, "Stdin Read: 0x%02x '%c'\n", ch, ch );
}
}
if (write(gPortFd, &ch, 1) < 0) {
fprintf(stderr, "write to serial failed: %s (%d)\n", strerror(errno), errno);
break;
}
}
if (FD_ISSET(gPortFd, &read_fds)) {
// Data is available on the serial port
char ch;
int bytesRead;
if (( bytesRead = read( gPortFd, &ch, 1 )) < 0 )
{
fprintf( stderr, "Serial port read failed: %s\n", strerror( errno ));
exit( 1 );
}
if ( gDebug )
{
if (( ch < ' ' ) || ( ch > '~' ))
{
fprintf( stderr, "Serial Read: 0x%02x '.'\n", ch );
}
else
{
fprintf( stderr, "Serial Read: 0x%02x '%c'\n", ch, ch );
}
}
if (write(fileno(stdout), &ch, 1) < 0) {
fprintf(stderr, "write to stdout failed: %s (%d)\n", strerror(errno), errno);
break;
}
}
}
// Restore stdin back to the way it was when we started
if ( tcsetattr( fileno( stdin ), TCSANOW, &stdin_tio_org ) < 0 )
{
fprintf( stderr, "Unable to update terminal settings: %s\n", strerror( errno ));
exit( 6 );
}
close( gPortFd );
if ( gVerbose )
{
fprintf( stderr, "Done\n" );
}
exit( 0 );
return 0; // Get rid of warning about not returning anything
}
