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 }