Commit 061c184d authored by William Woodall's avatar William Woodall
Browse files

Merging boostless into master

parents 3d6581ba 81fc77dd
......@@ -7,11 +7,11 @@
*.pyc
*.pyo
*.zip
*.cc
*/files/*
*/tmp/*
*.hwm*
*.cfg
*.out
.svn
build
bin
......
find_path(serial_INCLUDE_DIRS serial.h /usr/include/serial "$ENV{NAMER_ROOT}")
find_path(serial_INCLUDE_DIRS serial/serial.h /usr/include
/usr/local/include "$ENV{NAMER_ROOT}")
find_library(serial_LIBRARIES serial /usr/lib "$ENV{NAMER_ROOT}")
find_library(serial_LIBRARIES serial /usr/lib /usr/local/lib
"$ENV{NAMER_ROOT}")
set(serial_FOUND TRUE)
......
ifdef ROS_ROOT
include $(shell rospack find mk)/cmake.mk
else
include serial.mk
include serial.makefile
endif
......@@ -7,7 +7,6 @@ Coming Soon!
## Dependencies
* CMake, for the build system: http://www.cmake.org/
* Boost, for threading: http://www.boost.org/
* (Optional) ROS
## Stand Alone Installation
......
This diff is collapsed.
/***
* This example expects the serial port has a loopback on it.
*
* Alternatively, you could use an Arduino:
*
* <pre>
* setup() {
* Serial.begin(<insert your baudrate here>);
* }
*
* loop() {
* if (Serial.available()) {
* Serial.write(Serial.read());
* }
* }
* </pre>
*/
#include <string>
#include <iostream>
#include <cstdio>
// OS Specific sleep
#ifdef __WIN32__
#include <windows.h>
#else
#include <unistd.h>
#endif
#include "serial/serial.h"
using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
void my_sleep(unsigned long milliseconds) {
#ifdef __WIN32__
Sleep(milliseconds); // 100 ms
#else
usleep(milliseconds*1000); // 100 ms
#endif
}
int run(int argc, char **argv)
{
if(argc < 3) {
cerr << "Usage: test_serial <serial port address> ";
cerr << "<baudrate> [test string]" << endl;
return 0;
}
// Argument 1 is the serial port
string port(argv[1]);
// Argument 2 is the baudrate
unsigned long baud = 0;
sscanf(argv[2], "%lu", &baud);
// port, baudrate, timeout in milliseconds
serial::Serial my_serial(port, baud, 1000);
cout << "Is the serial port open?";
if(my_serial.isOpen())
cout << " Yes." << endl;
else
cout << " No." << endl;
// Get the Test string
int count = 0;
string test_string;
if (argc == 4) {
test_string = argv[3];
} else {
test_string = "Testing.";
}
// Test the timeout, there should be 1 second between prints
cout << "Timeout == 1000ms, asking for 1 more byte than written." << endl;
while (count < 10) {
size_t bytes_wrote = my_serial.write(test_string);
string result = my_serial.read(test_string.length()+1);
cout << "Iteration: " << count << ", Bytes written: ";
cout << bytes_wrote << ", Bytes read: ";
cout << result.length() << ", String read: " << result << endl;
count += 1;
}
// Test the timeout at 250ms
my_serial.setTimeout(250);
count = 0;
cout << "Timeout == 250ms, asking for 1 more byte than written." << endl;
while (count < 10) {
size_t bytes_wrote = my_serial.write(test_string);
string result = my_serial.read(test_string.length()+1);
cout << "Iteration: " << count << ", Bytes written: ";
cout << bytes_wrote << ", Bytes read: ";
cout << result.length() << ", String read: " << result << endl;
count += 1;
}
// Test the timeout at 250ms, but asking exactly for what was written
count = 0;
cout << "Timeout == 250ms, asking for exactly what was written." << endl;
while (count < 10) {
size_t bytes_wrote = my_serial.write(test_string);
string result = my_serial.read(test_string.length());
cout << "Iteration: " << count << ", Bytes written: ";
cout << bytes_wrote << ", Bytes read: ";
cout << result.length() << ", String read: " << result << endl;
count += 1;
}
// Test the timeout at 250ms, but asking for 1 less than what was written
count = 0;
cout << "Timeout == 250ms, asking for 1 less than was written." << endl;
while (count < 10) {
size_t bytes_wrote = my_serial.write(test_string);
string result = my_serial.read(test_string.length()-1);
cout << "Iteration: " << count << ", Bytes written: ";
cout << bytes_wrote << ", Bytes read: ";
cout << result.length() << ", String read: " << result << endl;
count += 1;
}
return 0;
}
int main(int argc, char **argv) {
try {
return run(argc, argv);
} catch (exception &e) {
cerr << "Unhandled Exception: " << e.what() << endl;
}
}
#include <string>
#include <iostream>
#include "serial/serial.h"
int main(int argc, char **argv)
{
if(argc < 2) {
std::cerr << "Usage: test_serial <serial port address>" << std::endl;
return 0;
}
std::string port(argv[1]);
serial::Serial serial(port, 115200, 250);
std::cout << "Is the serial port open?";
if(serial.isOpen())
std::cout << " Yes." << std::endl;
else
std::cout << " No." << std::endl;
int count = 0;
while (count >= 0) {
int bytes_wrote = serial.write("Testing.");
std::string result = serial.read(8);
if(count % 10 == 0)
std::cout << ">" << count << ">" << bytes_wrote << ">" << result << std::endl;
count += 1;
}
return 0;
}
/*!
* \file serial/impl/unix.h
* \author William Woodall <wjwwood@gmail.com>
* \author John Harrison <ash@greaterthaninfinity.com>
* \version 0.1
*
* \section LICENSE
*
* The MIT License
*
* Copyright (c) 2011 William Woodall, John Harrison
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
* DEALINGS IN THE SOFTWARE.
*
* \section DESCRIPTION
*
* This provides a unix based pimpl for the Serial class. This implementation is
* based off termios.h and uses select for multiplexing the IO ports.
*
*/
#ifndef SERIAL_IMPL_UNIX_H
#define SERIAL_IMPL_UNIX_H
#include "serial/serial.h"
#include <pthread.h>
namespace serial {
using std::string;
using std::invalid_argument;
using serial::SerialExecption;
using serial::IOException;
class serial::Serial::SerialImpl {
public:
SerialImpl (const string &port,
unsigned long baudrate,
long timeout,
bytesize_t bytesize,
parity_t parity,
stopbits_t stopbits,
flowcontrol_t flowcontrol);
virtual ~SerialImpl ();
void
open ();
void
close ();
bool
isOpen () const;
size_t
available ();
size_t
read (unsigned char* buf, size_t size = 1);
size_t
write (const string &data);
void
flush ();
void
flushInput ();
void
flushOutput ();
void
sendBreak(int duration);
void
setBreak(bool level);
void
setRTS(bool level);
void
setDTR(bool level);
bool
getCTS();
bool
getDSR();
bool
getRI();
bool
getCD();
void
setPort (const string &port);
string
getPort () const;
void
setTimeout (long timeout);
long
getTimeout () const;
void
setBaudrate (unsigned long baudrate);
unsigned long
getBaudrate () const;
void
setBytesize (bytesize_t bytesize);
bytesize_t
getBytesize () const;
void
setParity (parity_t parity);
parity_t
getParity () const;
void
setStopbits (stopbits_t stopbits);
stopbits_t
getStopbits () const;
void
setFlowcontrol (flowcontrol_t flowcontrol);
flowcontrol_t
getFlowcontrol () const;
void
readLock();
void
readUnlock();
void
writeLock();
void
writeUnlock();
protected:
void reconfigurePort ();
private:
string port_; // Path to the file descriptor
int fd_; // The current file descriptor
bool is_open_;
bool xonxoff_;
bool rtscts_;
long timeout_; // Timeout for read operations
unsigned long baudrate_; // Baudrate
parity_t parity_; // Parity
bytesize_t bytesize_; // Size of the bytes
stopbits_t stopbits_; // Stop Bits
flowcontrol_t flowcontrol_; // Flow Control
// Mutex used to lock the read functions
pthread_mutex_t read_mutex;
// Mutex used to lock the write functions
pthread_mutex_t write_mutex;
};
}
#endif // SERIAL_IMPL_UNIX_H
/*!
* \file serial/impl/windows.h
* \author William Woodall <wjwwood@gmail.com>
* \author John Harrison <ash@greaterthaninfinity.com>
* \version 0.1
*
* \section LICENSE
*
* The MIT License
*
* Copyright (c) 2011 William Woodall, John Harrison
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
* DEALINGS IN THE SOFTWARE.
*
* \section DESCRIPTION
*
* This provides a windows implementation of the Serial class interface.
*
*/
#ifndef SERIAL_IMPL_WINDOWS_H
#define SERIAL_IMPL_WINDOWS_H
#include "serial/serial.h"
namespace serial {
using std::string;
using std::invalid_argument;
using serial::SerialExecption;
using serial::IOException;
class serial::Serial::SerialImpl {
public:
SerialImpl (const string &port,
unsigned long baudrate,
long timeout,
bytesize_t bytesize,
parity_t parity,
stopbits_t stopbits,
flowcontrol_t flowcontrol);
virtual ~SerialImpl ();
void
open ();
void
close ();
bool
isOpen () const;
size_t
available ();
size_t
read (char* buf, size_t size = 1);
size_t
write (const string &data);
void
flush ();
void
flushInput ();
void
flushOutput ();
void
sendBreak(int duration);
void
setBreak(bool level);
void
setRTS(bool level);
void
setDTR(bool level);
bool
getCTS();
bool
getDSR();
bool
getRI();
bool
getCD();
void
setPort (const string &port);
string
getPort () const;
void
setTimeout (long timeout);
long
getTimeout () const;
void
setBaudrate (unsigned long baudrate);
unsigned long
getBaudrate () const;
void
setBytesize (bytesize_t bytesize);
bytesize_t
getBytesize () const;
void
setParity (parity_t parity);
parity_t
getParity () const;
void
setStopbits (stopbits_t stopbits);
stopbits_t
getStopbits () const;
void
setFlowcontrol (flowcontrol_t flowcontrol);
flowcontrol_t
getFlowcontrol () const;
protected:
void reconfigurePort ();
private:
string port_; // Path to the file descriptor
int fd_; // The current file descriptor
bool isOpen_;
bool xonxoff_;
bool rtscts_;
long timeout_; // Timeout for read operations
unsigned long baudrate_; // Baudrate
parity_t parity_; // Parity
bytesize_t bytesize_; // Size of the bytes
stopbits_t stopbits_; // Stop Bits
flowcontrol_t flowcontrol_; // Flow Control
};
}
#endif // SERIAL_IMPL_WINDOWS_H
This diff is collapsed.
macro(build_serial)
## Project Setup
cmake_minimum_required(VERSION 2.4.6)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
project(Serial)
## Configurations
## Project Setup
cmake_minimum_required(VERSION 2.4.6)
option(SERIAL_BUILD_TESTS "Build all of the Serial tests." OFF)
option(SERIAL_BUILD_EXAMPLES "Build all of the Serial examples." OFF)
# Allow for building shared libs override
IF(NOT BUILD_SHARED_LIBS)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
project(Serial)
## Configurations
# Enable warnings
# Assuming unix means a gcc style compiler, eg. g++ or clang++.
IF(UNIX)
set(CMAKE_CXX_FLAGS "-Wall -Weffc++ -pedantic -pedantic-errors -Wextra -Wall -Waggregate-return -Wcast-align -Wcast-qual -Wchar-subscripts -Wcomment -Wconversion -Wdisabled-optimization -Wfloat-equal -Wformat -Wformat=2 -Wformat-nonliteral -Wformat-security -Wformat-y2k -Wimplicit -Wimport -Winit-self -Winline -Winvalid-pch -Wlong-long -Wmissing-braces -Wmissing-field-initializers -Wmissing-format-attribute -Wmissing-include-dirs -Wmissing-noreturn -Wpacked -Wparentheses -Wpointer-arith -Wredundant-decls -Wreturn-type -Wsequence-point -Wshadow -Wsign-compare -Wstack-protector -Wstrict-aliasing -Wstrict-aliasing=2 -Wswitch -Wswitch-default -Wswitch-enum -Wtrigraphs -Wuninitialized -Wunknown-pragmas -Wunreachable-code -Wunused -Wunused-function -Wunused-label -Wunused-parameter -Wunused-value -Wunused-variable -Wvariadic-macros -Wvolatile-register-var -Wwrite-strings")
ELSEIF(WIN32)
# Force to always compile with W4
if(CMAKE_CXX_FLAGS MATCHES "/W[0-4]")
string(REGEX REPLACE "/W[0-4]" "/W4" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /W4")
endif()
endif(UNIX)
# Use clang if available
IF(EXISTS /usr/bin/clang)
set(CMAKE_CXX_COMPILER /usr/bin/clang++)
set(CMAKE_OSX_DEPLOYMENT_TARGET "")
set(SERIAL_BUILD_WARNINGS TRUE)
set(CMAKE_BUILD_TYPE Debug)
ENDIF(EXISTS /usr/bin/clang)
option(SERIAL_BUILD_TESTS "Build all of the Serial tests." OFF)
option(SERIAL_BUILD_EXAMPLES "Build all of the Serial examples." OFF)
# Allow for building shared libs override
IF(NOT BUILD_SHARED_LIBS)
set(BUILD_SHARED_LIBS OFF)
ENDIF(NOT BUILD_SHARED_LIBS)
ENDIF(NOT BUILD_SHARED_LIBS)
# Set the default path for built executables to the "bin" directory
IF(NOT DEFINED(EXECUTABLE_OUTPUT_PATH))
# Threading libraries added for mutexs
FIND_PACKAGE (Threads)
# Set the default path for built executables to the "bin" directory
IF(NOT DEFINED(EXECUTABLE_OUTPUT_PATH))
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
ENDIF(NOT DEFINED(EXECUTABLE_OUTPUT_PATH))
# set the default path for built libraries to the "lib" directory
IF(NOT DEFINED(LIBRARY_OUTPUT_PATH))
ENDIF(NOT DEFINED(EXECUTABLE_OUTPUT_PATH))
# set the default path for built libraries to the "lib" directory
IF(NOT DEFINED(LIBRARY_OUTPUT_PATH))
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
ENDIF(NOT DEFINED(LIBRARY_OUTPUT_PATH))
## Configure the build system
# Add the include folder to the include path
include_directories(${PROJECT_SOURCE_DIR}/include)
ENDIF(NOT DEFINED(LIBRARY_OUTPUT_PATH))
# Add default source files
set(SERIAL_SRCS src/serial.cpp)
# Add default header files
set(SERIAL_HEADERS include/serial/serial.h)
## Configure the build system
# Find Boost, if it hasn't already been found
IF(NOT Boost_FOUND OR NOT Boost_SYSTEM_FOUND OR NOT Boost_FILESYSTEM_FOUND OR NOT Boost_THREAD_FOUND)
find_package(Boost COMPONENTS system filesystem thread REQUIRED)
ENDIF(NOT Boost_FOUND OR NOT Boost_SYSTEM_FOUND OR NOT Boost_FILESYSTEM_FOUND OR NOT Boost_THREAD_FOUND)
# Add the include folder to the include path
include_directories(${PROJECT_SOURCE_DIR}/include)
link_directories(${Boost_LIBRARY_DIRS})
include_directories(${Boost_INCLUDE_DIRS})
# Add default source files
set(SERIAL_SRCS src/serial.cc)
IF(WIN32)
list(APPEND SERIAL_SRCS src/impl/windows.cc)
ELSE(WIN32)
list(APPEND SERIAL_SRCS src/impl/unix.cc)
ENDIF(WIN32)
# Add default header files
set(SERIAL_HEADERS include/serial/serial.h)
set(SERIAL_LINK_LIBS ${Boost_SYSTEM_LIBRARY}
${Boost_FILESYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY})
## Build the Serial Library
set(OTHER_LIBS "")
if(UNIX)
set(OTHER_LIBS util)
endif(UNIX)
# Compile the Library
add_library(serial ${SERIAL_SRCS} ${SERIAL_HEADERS})
target_link_libraries(serial ${SERIAL_LINK_LIBS})
IF( WIN32 )
target_link_libraries(serial wsock32)
ENDIF( )
## Build the Serial Library
# Check for OS X and if so disable kqueue support in asio
IF(CMAKE_SYSTEM_NAME MATCHES Darwin)
add_definitions(-DBOOST_ASIO_DISABLE_KQUEUE)
ENDIF(CMAKE_SYSTEM_NAME MATCHES Darwin)
# Compile the Library
add_library(serial ${SERIAL_SRCS})
target_link_libraries(serial ${CMAKE_THREAD_LIBS_INIT} ${OTHER_LIBS})
## Build Examples
## Build Examples
# If asked to
IF(SERIAL_BUILD_EXAMPLES)
# Compile the Test program
add_executable(serial_example examples/serial_example.cpp)
# If asked to
IF(SERIAL_BUILD_EXAMPLES)
# Compile the Serial Test program
add_executable(serial_example examples/serial_example.cc)
# Link the Test program to the Serial library
target_link_libraries(serial_example serial)
ENDIF(SERIAL_BUILD_EXAMPLES)
ENDIF(SERIAL_BUILD_EXAMPLES)
## Build tests
# If asked to
IF(SERIAL_BUILD_TESTS)
# Find Google Test
enable_testing()
find_package(GTest REQUIRED)
include_directories(${GTEST_INCLUDE_DIRS})
## Build tests
# Compile the Serial Test program
add_executable(serial_tests tests/serial_tests.cc)
# Link the Test program to the serial library
target_link_libraries(serial_tests ${GTEST_BOTH_LIBRARIES}
serial)
# If asked to
IF(SERIAL_BUILD_TESTS)
# none yet...
ENDIF(SERIAL_BUILD_TESTS)
add_test(AllTestsIntest_serial serial_tests)
ENDIF(SERIAL_BUILD_TESTS)
## Setup install and uninstall
## Setup install and uninstall
# Unless asked not to...
IF(NOT SERIAL_DONT_CONFIGURE_INSTALL)
# Unless asked not to...
IF(NOT SERIAL_DONT_CONFIGURE_INSTALL)
# Configure make install
IF(NOT CMAKE_INSTALL_PREFIX)
SET(CMAKE_INSTALL_PREFIX /usr/local)
......@@ -95,13 +121,15 @@ IF(NOT SERIAL_DONT_CONFIGURE_INSTALL)
ARCHIVE DESTINATION lib
)
INSTALL(FILES include/serial/serial.h DESTINATION include/serial)
INSTALL(FILES include/serial/serial.h
DESTINATION include/serial)
IF(NOT CMAKE_FIND_INSTALL_PATH)
set(CMAKE_FIND_INSTALL_PATH ${CMAKE_ROOT})
ENDIF(NOT CMAKE_FIND_INSTALL_PATH)
INSTALL(FILES Findserial.cmake DESTINATION ${CMAKE_FIND_INSTALL_PATH}/Modules/)
INSTALL(FILES Findserial.cmake
DESTINATION ${CMAKE_FIND_INSTALL_PATH}/Modules/)
ADD_CUSTOM_TARGET(uninstall @echo uninstall package)
......@@ -118,5 +146,5 @@ IF(NOT SERIAL_DONT_CONFIGURE_INSTALL)
TARGET uninstall
)
ENDIF(UNIX)
ENDIF(NOT SERIAL_DONT_CONFIGURE_INSTALL)
ENDIF(NOT SERIAL_DONT_CONFIGURE_INSTALL)
endmacro(build_serial)
......@@ -20,6 +20,13 @@ clean:
-cd build && make clean
rm -rf build bin lib
.PHONY: doc
doc:
@doxygen doc/Doxyfile
ifeq ($(UNAME),Darwin)
@open doc/html/index.html
endif
.PHONY: test
test:
@mkdir -p build
......@@ -30,4 +37,4 @@ ifneq ($(MAKE),)
else
cd build && make
endif
# cd bin && ./serial_tests
\ No newline at end of file
cd bin && ./serial_tests
\ No newline at end of file
macro(build_serial)
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
# Check for OS X and if so disable kqueue support in asio
IF(CMAKE_SYSTEM_NAME MATCHES Darwin)
add_definitions(-DBOOST_ASIO_DISABLE_KQUEUE)
ENDIF(CMAKE_SYSTEM_NAME MATCHES Darwin)
# Build the serial library
rosbuild_add_library(serial src/serial.cpp include/serial/serial.h)
# Add boost dependencies
rosbuild_add_boost_directories()
rosbuild_link_boost(serial system filesystem thread)
# Build example
rosbuild_add_executable(serial_example examples/serial_example.cpp)
target_link_libraries(serial_example serial)
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
set(ROS_BUILD_TYPE Debug)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
include_directories(include)
include_directories(vendor)
set(SERIAL_SRCS src/serial.cc)
if(UNIX)
list(APPEND SERIAL_SRCS src/impl/unix.cc)
else(UNIX)
list(APPEND SERIAL_SRCS src/impl/winows.cc)
endif(UNIX)
# Build the serial library
rosbuild_add_library(${PROJECT_NAME} ${SERIAL_SRCS})
# Build example
rosbuild_add_executable(serial_example examples/serial_example.cc)
target_link_libraries(serial_example ${PROJECT_NAME})
# Create unit tests
rosbuild_add_gtest(serial_tests tests/serial_tests.cc)
target_link_libraries(serial_tests ${PROJECT_NAME})
endmacro(build_serial)
/* Copyright 2012 William Woodall and John Harrison */
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <paths.h>
#include <sysexits.h>
#include <termios.h>
#include <sys/param.h>
#include <pthread.h>
#if defined(__linux__)
# include <linux/serial.h>
#endif
#include <sys/select.h>
#include <sys/time.h>
#include <time.h>
#ifdef __MACH__
#include <mach/clock.h>
#include <mach/mach.h>
#endif
#include "serial/impl/unix.h"
#ifndef TIOCINQ
#ifdef FIONREAD
#define TIOCINQ FIONREAD
#else
#define TIOCINQ 0x541B
#endif
#endif
using std::string;
using std::invalid_argument;
using serial::Serial;
using serial::SerialExecption;
using serial::PortNotOpenedException;
using serial::IOException;
Serial::SerialImpl::SerialImpl (const string &port, unsigned long baudrate,
long timeout, bytesize_t bytesize,
parity_t parity, stopbits_t stopbits,
flowcontrol_t flowcontrol)
: port_ (port), fd_ (-1), is_open_ (false), xonxoff_ (true), rtscts_ (false),
timeout_ (timeout), baudrate_ (baudrate), parity_ (parity),
bytesize_ (bytesize), stopbits_ (stopbits), flowcontrol_ (flowcontrol)
{
pthread_mutex_init(&this->read_mutex, NULL);
pthread_mutex_init(&this->write_mutex, NULL);
if (port_.empty () == false)
open ();
}
Serial::SerialImpl::~SerialImpl ()
{
close();
pthread_mutex_destroy(&this->read_mutex);
pthread_mutex_destroy(&this->write_mutex);
}
void
Serial::SerialImpl::open ()
{
if (port_.empty ())
{
throw invalid_argument ("Empty port is invalid.");
}
if (is_open_ == true)
{
throw SerialExecption ("Serial port already open.");
}
fd_ = ::open (port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
if (fd_ == -1)
{
switch (errno)
{
case EINTR:
// Recurse because this is a recoverable error.
open ();
return;
case ENFILE:
case EMFILE:
THROW (IOException, "Too many file handles open.");
default:
THROW (IOException, errno);
}
}
reconfigurePort();
is_open_ = true;
}
void
Serial::SerialImpl::reconfigurePort ()
{
if (fd_ == -1)
{
// Can only operate on a valid file descriptor
THROW (IOException, "Invalid file descriptor, is the serial port open?");
}
struct termios options; // The options for the file descriptor
if (tcgetattr(fd_, &options) == -1)
{
THROW (IOException, "::tcgetattr");
}
// set up raw mode / no echo / binary
options.c_cflag |= (unsigned long) (CLOCAL|CREAD);
options.c_lflag &= (unsigned long) ~(ICANON|ECHO|ECHOE|ECHOK|ECHONL|
ISIG|IEXTEN); //|ECHOPRT
options.c_oflag &= (unsigned long) ~(OPOST);
options.c_iflag &= (unsigned long) ~(INLCR|IGNCR|ICRNL|IGNBRK);
#ifdef IUCLC
options.c_iflag &= (unsigned long) ~IUCLC;
#endif
#ifdef PARMRK
options.c_iflag &= (unsigned long) ~PARMRK;
#endif
// setup baud rate
bool custom_baud = false;
speed_t baud;
switch (baudrate_)
{
#ifdef B0
case 0: baud = B0; break;
#endif
#ifdef B50
case 50: baud = B50; break;
#endif
#ifdef B75
case 75: baud = B75; break;
#endif
#ifdef B110
case 110: baud = B110; break;
#endif
#ifdef B134
case 134: baud = B134; break;
#endif
#ifdef B150
case 150: baud = B150; break;
#endif
#ifdef B200
case 200: baud = B200; break;
#endif
#ifdef B300
case 300: baud = B300; break;
#endif
#ifdef B600
case 600: baud = B600; break;
#endif
#ifdef B1200
case 1200: baud = B1200; break;
#endif
#ifdef B1800
case 1800: baud = B1800; break;
#endif
#ifdef B2400
case 2400: baud = B2400; break;
#endif
#ifdef B4800
case 4800: baud = B4800; break;
#endif
#ifdef B7200
case 7200: baud = B7200; break;
#endif
#ifdef B9600
case 9600: baud = B9600; break;
#endif
#ifdef B14400
case 14400: baud = B14400; break;
#endif
#ifdef B19200
case 19200: baud = B19200; break;
#endif
#ifdef B28800
case 28800: baud = B28800; break;
#endif
#ifdef B57600
case 57600: baud = B57600; break;
#endif
#ifdef B76800
case 76800: baud = B76800; break;
#endif
#ifdef B38400
case 38400: baud = B38400; break;
#endif
#ifdef B115200
case 115200: baud = B115200; break;
#endif
#ifdef B128000
case 128000: baud = B128000; break;
#endif
#ifdef B153600
case 153600: baud = B153600; break;
#endif
#ifdef B230400
case 230400: baud = B230400; break;
#endif
#ifdef B256000
case 256000: baud = B256000; break;
#endif
#ifdef B460800
case 460800: baud = B460800; break;
#endif
#ifdef B921600
case 921600: baud = B921600; break;
#endif
default:
custom_baud = true;
// Mac OS X 10.x Support
#if defined(__APPLE__) && defined(__MACH__)
#define IOSSIOSPEED _IOW('T', 2, speed_t)
int new_baud = static_cast<int> (baudrate_);
if (ioctl (fd_, IOSSIOSPEED, &new_baud, 1) < 0)
{
THROW (IOException, errno);
}
// Linux Support
#elif defined(__linux__)
struct serial_struct ser;
ioctl (fd_, TIOCGSERIAL, &ser);
// set custom divisor
ser.custom_divisor = ser.baud_base / baudrate_;
// update flags
ser.flags &= ~ASYNC_SPD_MASK;
ser.flags |= ASYNC_SPD_CUST;
if (ioctl (fd_, TIOCSSERIAL, ser) < 0)
{
THROW (IOException, errno);
}
#else
throw invalid_argument ("OS does not currently support custom bauds");
#endif
}
if (custom_baud == false)
{
#ifdef _BSD_SOURCE
::cfsetspeed(&options, baud);
#else
::cfsetispeed(&options, baud);
::cfsetospeed(&options, baud);
#endif
}
// setup char len
options.c_cflag &= (unsigned long) ~CSIZE;
if (bytesize_ == eightbits)
options.c_cflag |= CS8;
else if (bytesize_ == sevenbits)
options.c_cflag |= CS7;
else if (bytesize_ == sixbits)
options.c_cflag |= CS6;
else if (bytesize_ == fivebits)
options.c_cflag |= CS5;
else
throw invalid_argument ("invalid char len");
// setup stopbits
if (stopbits_ == stopbits_one)
options.c_cflag &= (unsigned long) ~(CSTOPB);
else if (stopbits_ == stopbits_one_point_five)
// ONE POINT FIVE same as TWO.. there is no POSIX support for 1.5
options.c_cflag |= (CSTOPB);
else if (stopbits_ == stopbits_two)
options.c_cflag |= (CSTOPB);
else
throw invalid_argument ("invalid stop bit");
// setup parity
options.c_iflag &= (unsigned long) ~(INPCK|ISTRIP);
if (parity_ == parity_none)
{
options.c_cflag &= (unsigned long) ~(PARENB|PARODD);
}
else if (parity_ == parity_even)
{
options.c_cflag &= (unsigned long) ~(PARODD);
options.c_cflag |= (PARENB);
}
else if (parity_ == parity_odd)
{
options.c_cflag |= (PARENB|PARODD);
}
else
{
throw invalid_argument ("invalid parity");
}
// setup flow control
// xonxoff
#ifdef IXANY
if (xonxoff_)
options.c_iflag |= (IXON|IXOFF); //|IXANY)
else
options.c_iflag &= (unsigned long) ~(IXON|IXOFF|IXANY);
#else
if (xonxoff_)
options.c_iflag |= (IXON|IXOFF);
else
options.c_iflag &= (unsigned long) ~(IXON|IXOFF);
#endif
// rtscts
#ifdef CRTSCTS
if (rtscts_)
options.c_cflag |= (CRTSCTS);
else
options.c_cflag &= (unsigned long) ~(CRTSCTS);
#elif defined CNEW_RTSCTS
if (rtscts_)
options.c_cflag |= (CNEW_RTSCTS);
else
options.c_cflag &= (unsigned long) ~(CNEW_RTSCTS);
#else
#error "OS Support seems wrong."
#endif
// http://www.unixwiz.net/techtips/termios-vmin-vtime.html
// this basically sets the read call up to be a polling read,
// but we are using select to ensure there is data available
// to read before each call, so we should never needlessly poll
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 0;
// activate settings
::tcsetattr (fd_, TCSANOW, &options);
}
void
Serial::SerialImpl::close ()
{
if (is_open_ == true)
{
if (fd_ != -1)
{
::close (fd_); // Ignoring the outcome
fd_ = -1;
}
is_open_ = false;
}
}
bool
Serial::SerialImpl::isOpen () const
{
return is_open_;
}
size_t
Serial::SerialImpl::available ()
{
if (!is_open_)
{
return 0;
}
int count = 0;
int result = ioctl (fd_, TIOCINQ, &count);
if (result == 0)
{
return static_cast<size_t> (count);
}
else
{
THROW (IOException, errno);
}
}
inline void get_time_now(struct timespec &time) {
# ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time
clock_serv_t cclock;
mach_timespec_t mts;
host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
clock_get_time(cclock, &mts);
mach_port_deallocate(mach_task_self(), cclock);
time.tv_sec = mts.tv_sec;
time.tv_nsec = mts.tv_nsec;
# else
clock_gettime(CLOCK_REALTIME, &time);
# endif
}
size_t
Serial::SerialImpl::read (unsigned char* buf, size_t size)
{
if (!is_open_)
{
throw PortNotOpenedException ("Serial::read");
}
fd_set readfds;
size_t bytes_read = 0;
struct timeval timeout;
timeout.tv_sec = timeout_ / 1000;
timeout.tv_usec = static_cast<int> (timeout_ % 1000) * 1000;
while (bytes_read < size)
{
FD_ZERO (&readfds);
FD_SET (fd_, &readfds);
// On Linux the timeout struct is updated by select to contain the time
// left on the timeout to make looping easier, but on other platforms this
// does not occur.
#if !defined(__linux__)
// Begin timing select
struct timespec start, end;
get_time_now(start);
#endif
// Do the select
int r = select (fd_ + 1, &readfds, NULL, NULL, &timeout);
#if !defined(__linux__)
// Calculate difference and update the structure
get_time_now(end);
// Calculate the time select took
struct timeval diff;
diff.tv_sec = end.tv_sec-start.tv_sec;
diff.tv_usec = static_cast<int> ((end.tv_nsec-start.tv_nsec)/1000);
// Update the timeout
if (timeout.tv_sec <= diff.tv_sec) {
timeout.tv_sec = 0;
} else {
timeout.tv_sec -= diff.tv_sec;
}
if (timeout.tv_usec <= diff.tv_usec) {
timeout.tv_usec = 0;
} else {
timeout.tv_usec -= diff.tv_usec;
}
#endif
// Figure out what happened by looking at select's response 'r'
/** Error **/
if (r < 0) {
// Select was interrupted, try again
if (errno == EINTR) {
continue;
}
// Otherwise there was some error
THROW (IOException, errno);
}
/** Timeout **/
if (r == 0) {
break;
}
/** Something ready to read **/
if (r > 0) {
// Make sure our file descriptor is in the ready to read list
if (FD_ISSET (fd_, &readfds)) {
// This should be non-blocking returning only what is avaialble now
// Then returning so that select can block again.
ssize_t bytes_read_now =
::read (fd_, buf+bytes_read, size-bytes_read);
// read should always return some data as select reported it was
// ready to read when we get to this point.
if (bytes_read_now < 1)
{
// Disconnected devices, at least on Linux, show the
// behavior that they are always ready to read immediately
// but reading returns nothing.
throw SerialExecption ("device reports readiness to read but "
"returned no data (device disconnected?)");
}
// Update bytes_read
bytes_read += static_cast<size_t> (bytes_read_now);
// If bytes_read == size then we have read everything we need
if (bytes_read == size) {
break;
}
// If bytes_read < size then we have more to read
if (bytes_read < size) {
continue;
}
// If bytes_read > size then we have over read, which shouldn't happen
if (bytes_read > size) {
throw SerialExecption ("read over read, too many bytes where "
"read, this shouldn't happen, might be "
"a logical error!");
}
}
// This shouldn't happen, if r > 0 our fd has to be in the list!
THROW (IOException, "select reports ready to read, but our fd isn't"
" in the list, this shouldn't happen!");
}
}
return bytes_read;
}
size_t
Serial::SerialImpl::write (const string &data)
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::write");
}
return static_cast<size_t> (::write (fd_, data.c_str (), data.length ()));
}
void
Serial::SerialImpl::setPort (const string &port)
{
port_ = port;
}
string
Serial::SerialImpl::getPort () const
{
return port_;
}
void
Serial::SerialImpl::setTimeout (long timeout)
{
timeout_ = timeout;
}
long
Serial::SerialImpl::getTimeout () const
{
return timeout_;
}
void
Serial::SerialImpl::setBaudrate (unsigned long baudrate)
{
baudrate_ = baudrate;
if (is_open_)
reconfigurePort ();
}
unsigned long
Serial::SerialImpl::getBaudrate () const
{
return baudrate_;
}
void
Serial::SerialImpl::setBytesize (serial::bytesize_t bytesize)
{
bytesize_ = bytesize;
if (is_open_)
reconfigurePort ();
}
serial::bytesize_t
Serial::SerialImpl::getBytesize () const
{
return bytesize_;
}
void
Serial::SerialImpl::setParity (serial::parity_t parity)
{
parity_ = parity;
if (is_open_)
reconfigurePort ();
}
serial::parity_t
Serial::SerialImpl::getParity () const
{
return parity_;
}
void
Serial::SerialImpl::setStopbits (serial::stopbits_t stopbits)
{
stopbits_ = stopbits;
if (is_open_)
reconfigurePort ();
}
serial::stopbits_t
Serial::SerialImpl::getStopbits () const
{
return stopbits_;
}
void
Serial::SerialImpl::setFlowcontrol (serial::flowcontrol_t flowcontrol)
{
flowcontrol_ = flowcontrol;
if (is_open_)
reconfigurePort ();
}
serial::flowcontrol_t
Serial::SerialImpl::getFlowcontrol () const
{
return flowcontrol_;
}
void
Serial::SerialImpl::flush ()
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::flush");
}
tcdrain (fd_);
}
void
Serial::SerialImpl::flushInput ()
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::flushInput");
}
tcflush (fd_, TCIFLUSH);
}
void
Serial::SerialImpl::flushOutput ()
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::flushOutput");
}
tcflush (fd_, TCOFLUSH);
}
void
Serial::SerialImpl::sendBreak (int duration)
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::sendBreak");
}
tcsendbreak (fd_, static_cast<int> (duration/4));
}
void
Serial::SerialImpl::setBreak (bool level)
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::setBreak");
}
if (level)
{
ioctl (fd_, TIOCSBRK);
}
else {
ioctl (fd_, TIOCCBRK);
}
}
void
Serial::SerialImpl::setRTS (bool level)
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::setRTS");
}
if (level)
{
ioctl (fd_, TIOCMBIS, TIOCM_RTS);
}
else {
ioctl (fd_, TIOCMBIC, TIOCM_RTS);
}
}
void
Serial::SerialImpl::setDTR (bool level)
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::setDTR");
}
if (level)
{
ioctl (fd_, TIOCMBIS, TIOCM_DTR);
}
else
{
ioctl (fd_, TIOCMBIC, TIOCM_DTR);
}
}
bool
Serial::SerialImpl::getCTS ()
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::getCTS");
}
int s = ioctl (fd_, TIOCMGET, 0);
return (s & TIOCM_CTS) != 0;
}
bool
Serial::SerialImpl::getDSR()
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::getDSR");
}
int s = ioctl (fd_, TIOCMGET, 0);
return (s & TIOCM_DSR) != 0;
}
bool
Serial::SerialImpl::getRI()
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::getRI");
}
int s = ioctl (fd_, TIOCMGET, 0);
return (s & TIOCM_RI) != 0;
}
bool
Serial::SerialImpl::getCD()
{
if (is_open_ == false)
{
throw PortNotOpenedException ("Serial::getCD");
}
int s = ioctl (fd_, TIOCMGET, 0);
return (s & TIOCM_CD) != 0;
}
void
Serial::SerialImpl::readLock() {
int result = pthread_mutex_lock(&this->read_mutex);
if (result) {
THROW (IOException, result);
}
}
void
Serial::SerialImpl::readUnlock() {
int result = pthread_mutex_unlock(&this->read_mutex);
if (result) {
THROW (IOException, result);
}
}
void
Serial::SerialImpl::writeLock() {
int result = pthread_mutex_lock(&this->write_mutex);
if (result) {
THROW (IOException, result);
}
}
void
Serial::SerialImpl::writeUnlock() {
int result = pthread_mutex_unlock(&this->write_mutex);
if (result) {
THROW (IOException, result);
}
}
/* Copyright 2012 William Woodall and John Harrison */
#include "serial/impl/windows.h"
using std::string;
using std::invalid_argument;
using serial::Serial;
using serial::SerialExecption;
using serial::PortNotOpenedException;
using serial::IOException;
Serial::SerialImpl::SerialImpl (const string &port, unsigned long baudrate,
long timeout, bytesize_t bytesize,
parity_t parity, stopbits_t stopbits,
flowcontrol_t flowcontrol)
: port_ (port), fd_ (-1), isOpen_ (false), xonxoff_ (true), rtscts_ (false),
timeout_ (timeout), baudrate_ (baudrate), parity_ (parity), bytesize_ (bytesize),
stopbits_ (stopbits), flowcontrol_ (flowcontrol)
{
if (port_.empty () == false)
open ();
}
Serial::SerialImpl::~SerialImpl ()
{
close();
}
void
Serial::SerialImpl::open ()
{
if (port_.empty())
{
throw invalid_argument ("bad port specified");
}
if (isOpen_ == true)
{
throw SerialExecption ("port already open");
}
fd_ = ::open (port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
if (fd_ == -1)
{
switch (errno)
{
case EINTR:
// Recurse because this is a recoverable error.
open ();
return;
case ENFILE:
case EMFILE:
throw IOException ("to many file handles open");
break;
default:
throw IOException (errno);
}
}
reconfigurePort();
isOpen_ = true;
}
void
Serial::SerialImpl::reconfigurePort ()
{
if (fd_ == -1)
{
// Can only operate on a valid file descriptor
throw IOException ("invalid file descriptor");
}
struct termios options; // The options for the file descriptor
if (tcgetattr(fd_, &options) == -1)
{
throw IOException ("::tcgetattr");
}
// set up raw mode / no echo / binary
options.c_cflag |= (unsigned long) (CLOCAL|CREAD);
options.c_lflag &= (unsigned long) ~(ICANON|ECHO|ECHOE|ECHOK|ECHONL|
ISIG|IEXTEN); //|ECHOPRT
options.c_oflag &= (unsigned long) ~(OPOST);
options.c_iflag &= (unsigned long) ~(INLCR|IGNCR|ICRNL|IGNBRK);
#ifdef IUCLC
options.c_iflag &= (unsigned long) ~IUCLC;
#endif
#ifdef PARMRK
options.c_iflag &= (unsigned long) ~PARMRK;
#endif
// setup baud rate
bool custom_baud = false;
speed_t baud;
switch (baudrate_)
{
#ifdef B0
case 0: baud = B0; break;
#endif
#ifdef B50
case 50: baud = B50; break;
#endif
#ifdef B75
case 75: baud = B75; break;
#endif
#ifdef B110
case 110: baud = B110; break;
#endif
#ifdef B134
case 134: baud = B134; break;
#endif
#ifdef B150
case 150: baud = B150; break;
#endif
#ifdef B200
case 200: baud = B200; break;
#endif
#ifdef B300
case 300: baud = B300; break;
#endif
#ifdef B600
case 600: baud = B600; break;
#endif
#ifdef B1200
case 1200: baud = B1200; break;
#endif
#ifdef B1800
case 1800: baud = B1800; break;
#endif
#ifdef B2400
case 2400: baud = B2400; break;
#endif
#ifdef B4800
case 4800: baud = B4800; break;
#endif
#ifdef B7200
case 7200: baud = B7200; break;
#endif
#ifdef B9600
case 9600: baud = B9600; break;
#endif
#ifdef B14400
case 14400: baud = B14400; break;
#endif
#ifdef B19200
case 19200: baud = B19200; break;
#endif
#ifdef B28800
case 28800: baud = B28800; break;
#endif
#ifdef B57600
case 57600: baud = B57600; break;
#endif
#ifdef B76800
case 76800: baud = B76800; break;
#endif
#ifdef B38400
case 38400: baud = B38400; break;
#endif
#ifdef B115200
case 115200: baud = B115200; break;
#endif
#ifdef B128000
case 128000: baud = B128000; break;
#endif
#ifdef B153600
case 153600: baud = B153600; break;
#endif
#ifdef B230400
case 230400: baud = B230400; break;
#endif
#ifdef B256000
case 256000: baud = B256000; break;
#endif
#ifdef B460800
case 460800: baud = B460800; break;
#endif
#ifdef B921600
case 921600: baud = B921600; break;
#endif
default:
custom_baud = true;
// Mac OS X 10.x Support
#if defined(__APPLE__) && defined(__MACH__)
#define IOSSIOSPEED _IOW('T', 2, speed_t)
int new_baud = static_cast<int> (baudrate_);
if (ioctl (fd_, IOSSIOSPEED, &new_baud, 1) < 0)
{
throw IOException (errno);
}
// Linux Support
#elif defined(__linux__)
struct serial_struct ser;
ioctl(fd_, TIOCGSERIAL, &ser);
// set custom divisor
ser.custom_divisor = ser.baud_base / baudrate_;
// update flags
ser.flags &= ~ASYNC_SPD_MASK;
ser.flags |= ASYNC_SPD_CUST;
if (ioctl(fd_, TIOCSSERIAL, ser) < 0)
{
throw IOException (errno);
}
#else
throw invalid_argument ("OS does not currently support custom bauds");
#endif
}
if (custom_baud == false)
{
#ifdef _BSD_SOURCE
::cfsetspeed(&options, baud);
#else
::cfsetispeed(&options, baud);
::cfsetospeed(&options, baud);
#endif
}
// setup char len
options.c_cflag &= (unsigned long) ~CSIZE;
if (bytesize_ == EIGHTBITS)
options.c_cflag |= CS8;
else if (bytesize_ == SEVENBITS)
options.c_cflag |= CS7;
else if (bytesize_ == SIXBITS)
options.c_cflag |= CS6;
else if (bytesize_ == FIVEBITS)
options.c_cflag |= CS5;
else
throw invalid_argument ("invalid char len");
// setup stopbits
if (stopbits_ == STOPBITS_ONE)
options.c_cflag &= (unsigned long) ~(CSTOPB);
else if (stopbits_ == STOPBITS_ONE_POINT_FIVE)
options.c_cflag |= (CSTOPB); // XXX same as TWO.. there is no POSIX support for 1.5
else if (stopbits_ == STOPBITS_TWO)
options.c_cflag |= (CSTOPB);
else
throw invalid_argument ("invalid stop bit");
// setup parity
options.c_iflag &= (unsigned long) ~(INPCK|ISTRIP);
if (parity_ == PARITY_NONE)
{
options.c_cflag &= (unsigned long) ~(PARENB|PARODD);
}
else if (parity_ == PARITY_EVEN)
{
options.c_cflag &= (unsigned long) ~(PARODD);
options.c_cflag |= (PARENB);
}
else if (parity_ == PARITY_ODD)
{
options.c_cflag |= (PARENB|PARODD);
}
else
{
throw invalid_argument ("invalid parity");
}
// setup flow control
// xonxoff
#ifdef IXANY
if (xonxoff_)
options.c_iflag |= (IXON|IXOFF); //|IXANY)
else
options.c_iflag &= (unsigned long) ~(IXON|IXOFF|IXANY);
#else
if (xonxoff_)
options.c_iflag |= (IXON|IXOFF);
else
options.c_iflag &= (unsigned long) ~(IXON|IXOFF);
#endif
// rtscts
#ifdef CRTSCTS
if (rtscts_)
options.c_cflag |= (CRTSCTS);
else
options.c_cflag &= (unsigned long) ~(CRTSCTS);
#elif defined CNEW_RTSCTS
if (rtscts_)
options.c_cflag |= (CNEW_RTSCTS);
else
options.c_cflag &= (unsigned long) ~(CNEW_RTSCTS);
#else
#error "OS Support seems wrong."
#endif
options.c_cc[VMIN] = 1; // Minimum of 1 character in the buffer
options.c_cc[VTIME] = 0; // timeout on waiting for new data
// activate settings
::tcsetattr (fd_, TCSANOW, &options);
}
void
Serial::SerialImpl::close ()
{
if (isOpen_ == true)
{
if (fd_ != -1)
{
::close (fd_); // Ignoring the outcome
fd_ = -1;
}
isOpen_ = false;
}
}
bool
Serial::SerialImpl::isOpen () const
{
return isOpen_;
}
size_t
Serial::SerialImpl::available ()
{
if (!isOpen_)
{
return 0;
}
int count = 0;
int result = ioctl (fd_, TIOCINQ, &count);
if (result == 0)
{
return static_cast<size_t> (count);
}
else
{
throw IOException (errno);
}
}
size_t
Serial::SerialImpl::read (char* buf, size_t size)
{
if (!isOpen_)
{
throw PortNotOpenedException ("Serial::read");
}
fd_set readfds;
ssize_t bytes_read = 0;
int count = 0;
while (true)
{
count++;
if (timeout_ != -1)
{
FD_ZERO (&readfds);
FD_SET (fd_, &readfds);
struct timeval timeout;
timeout.tv_sec = timeout_ / 1000;
timeout.tv_usec = static_cast<int> (timeout_ % 1000) * 1000;
int r = select (fd_ + 1, &readfds, NULL, NULL, &timeout);
if (r == -1 && errno == EINTR)
continue;
if (r == -1)
{
throw IOException (errno);
}
}
if (timeout_ == -1 || FD_ISSET (fd_, &readfds))
{
bytes_read = ::read (fd_, buf, size);
// read should always return some data as select reported it was
// ready to read when we get to this point.
if (bytes_read < 1)
{
// Disconnected devices, at least on Linux, show the
// behavior that they are always ready to read immediately
// but reading returns nothing.
throw SerialExecption ("device reports readiness to read but "
"returned no data (device disconnected?)");
}
break;
}
else
{
break;
}
}
return static_cast<size_t> (bytes_read);
}
size_t
Serial::SerialImpl::write (const string &data)
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::write");
}
fd_set writefds;
ssize_t bytes_written = 0;
while (true)
{
if (timeout_ != -1)
{
FD_ZERO (&writefds);
FD_SET (fd_, &writefds);
struct timeval timeout;
timeout.tv_sec = timeout_ / 1000;
timeout.tv_usec = static_cast<int> (timeout_ % 1000) * 1000;
int r = select (fd_ + 1, NULL, &writefds, NULL, &timeout);
if (r == -1 && errno == EINTR)
continue;
if (r == -1)
{
throw IOException (errno);
}
}
if (timeout_ == -1 || FD_ISSET (fd_, &writefds))
{
bytes_written = ::write (fd_, data.c_str (), data.length ());
// read should always return some data as select reported it was
// ready to read when we get to this point.
if (bytes_written < 1)
{
// Disconnected devices, at least on Linux, show the
// behavior that they are always ready to read immediately
// but reading returns nothing.
throw SerialExecption ("device reports readiness to read but "
"returned no data (device disconnected?)");
}
break;
}
else
{
break;
}
}
return static_cast<size_t> (bytes_written);
}
void
Serial::SerialImpl::setPort (const string &port)
{
port_ = port;
}
string
Serial::SerialImpl::getPort () const
{
return port_;
}
void
Serial::SerialImpl::setTimeout (long timeout)
{
timeout_ = timeout;
}
long
Serial::SerialImpl::getTimeout () const
{
return timeout_;
}
void
Serial::SerialImpl::setBaudrate (unsigned long baudrate)
{
baudrate_ = baudrate;
if (isOpen_)
reconfigurePort ();
}
unsigned long
Serial::SerialImpl::getBaudrate () const
{
return baudrate_;
}
void
Serial::SerialImpl::setBytesize (serial::bytesize_t bytesize)
{
bytesize_ = bytesize;
if (isOpen_)
reconfigurePort ();
}
serial::bytesize_t
Serial::SerialImpl::getBytesize () const
{
return bytesize_;
}
void
Serial::SerialImpl::setParity (serial::parity_t parity)
{
parity_ = parity;
if (isOpen_)
reconfigurePort ();
}
serial::parity_t
Serial::SerialImpl::getParity () const
{
return parity_;
}
void
Serial::SerialImpl::setStopbits (serial::stopbits_t stopbits)
{
stopbits_ = stopbits;
if (isOpen_)
reconfigurePort ();
}
serial::stopbits_t
Serial::SerialImpl::getStopbits () const
{
return stopbits_;
}
void
Serial::SerialImpl::setFlowcontrol (serial::flowcontrol_t flowcontrol)
{
flowcontrol_ = flowcontrol;
if (isOpen_)
reconfigurePort ();
}
serial::flowcontrol_t
Serial::SerialImpl::getFlowcontrol () const
{
return flowcontrol_;
}
void
Serial::SerialImpl::flush ()
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::flush");
}
tcdrain (fd_);
}
void
Serial::SerialImpl::flushInput ()
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::flushInput");
}
tcflush (fd_, TCIFLUSH);
}
void
Serial::SerialImpl::flushOutput ()
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::flushOutput");
}
tcflush (fd_, TCOFLUSH);
}
void
Serial::SerialImpl::sendBreak (int duration)
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::sendBreak");
}
tcsendbreak (fd_, static_cast<int> (duration/4));
}
void
Serial::SerialImpl::setBreak (bool level)
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::setBreak");
}
if (level)
{
ioctl (fd_, TIOCSBRK);
}
else {
ioctl (fd_, TIOCCBRK);
}
}
void
Serial::SerialImpl::setRTS (bool level)
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::setRTS");
}
if (level)
{
ioctl (fd_, TIOCMBIS, TIOCM_RTS);
}
else {
ioctl (fd_, TIOCMBIC, TIOCM_RTS);
}
}
void
Serial::SerialImpl::setDTR (bool level)
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::setDTR");
}
if (level)
{
ioctl (fd_, TIOCMBIS, TIOCM_DTR);
}
else
{
ioctl (fd_, TIOCMBIC, TIOCM_DTR);
}
}
bool
Serial::SerialImpl::getCTS ()
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::getCTS");
}
int s = ioctl (fd_, TIOCMGET, 0);
return (s & TIOCM_CTS) != 0;
}
bool
Serial::SerialImpl::getDSR()
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::getDSR");
}
int s = ioctl(fd_, TIOCMGET, 0);
return (s & TIOCM_DSR) != 0;
}
bool
Serial::SerialImpl::getRI()
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::getRI");
}
int s = ioctl (fd_, TIOCMGET, 0);
return (s & TIOCM_RI) != 0;
}
bool
Serial::SerialImpl::getCD()
{
if (isOpen_ == false)
{
throw PortNotOpenedException ("Serial::getCD");
}
int s = ioctl (fd_, TIOCMGET, 0);
return (s & TIOCM_CD) != 0;
}
This diff is collapsed.
This diff is collapsed.
#include ""
\ No newline at end of file
#!/usr/bin/env python
import serial, sys
if len(sys.argv) != 2:
print "python: Usage_serial_test <port name like: /dev/ttyUSB0>"
sys.exit(1)
sio = serial.Serial(sys.argv[1], 115200)
sio.timeout = 250
while True:
sio.write("Testing.")
print sio.read(8)
#include <iostream>
#include <string>
#include <vector>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/foreach.hpp>
void
_delimeter_tokenizer (std::string &data, std::vector<std::string> &tokens,
std::string delimeter)
{
boost::split(tokens, data, boost::is_any_of(delimeter));
}
typedef boost::function<void(std::string&,std::vector<std::string>&)> TokenizerType;
int main(void) {
std::string data = "a\rb\rc\r";
std::vector<std::string> tokens;
std::string delimeter = "\r";
TokenizerType f = boost::bind(_delimeter_tokenizer, _1, _2, delimeter);
f(data, tokens);
BOOST_FOREACH(std::string token, tokens)
std::cout << token << std::endl;
return 0;
}
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment