diff --git a/.travis.yml b/.travis.yml index 628c7e04..60d45e11 100644 --- a/.travis.yml +++ b/.travis.yml @@ -6,7 +6,7 @@ install: - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update - - sudo apt-get install ros-groovy-catkin + - sudo apt-get install ros-groovy-catkin libboost-dev - source /opt/ros/groovy/setup.bash script: - - make + - make && make test diff --git a/CMakeLists.txt b/CMakeLists.txt index 627e5ad7..f7f6a7ca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,9 +59,5 @@ install(FILES include/serial/serial.h include/serial/v8stdint.h ## Tests if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(${PROJECT_NAME}-test tests/serial_tests.cc) - target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${Boost_LIBRARIES}) - if(UNIX AND NOT APPLE) - target_link_libraries(${PROJECT_NAME}-test util) - endif() + add_subdirectory(tests) endif() diff --git a/include/serial/impl/unix.h b/include/serial/impl/unix.h index a7985da3..df73e2d5 100644 --- a/include/serial/impl/unix.h +++ b/include/serial/impl/unix.h @@ -53,6 +53,16 @@ using std::invalid_argument; using serial::SerialException; using serial::IOException; +class MillisecondTimer { +public: + MillisecondTimer(const uint32_t millis); + int64_t remaining(); + +private: + static timespec timespec_now(); + timespec expiry; +}; + class serial::Serial::SerialImpl { public: SerialImpl (const string &port, diff --git a/src/impl/unix.cc b/src/impl/unix.cc index 321995a7..561db21c 100755 --- a/src/impl/unix.cc +++ b/src/impl/unix.cc @@ -48,12 +48,62 @@ using std::string; using std::stringstream; using std::invalid_argument; +using serial::MillisecondTimer; using serial::Serial; using serial::SerialException; using serial::PortNotOpenedException; using serial::IOException; +MillisecondTimer::MillisecondTimer (const uint32_t millis) + : expiry(timespec_now()) +{ + int64_t tv_nsec = expiry.tv_nsec + (millis * 1e6); + if (tv_nsec >= 1e9) { + int64_t sec_diff = tv_nsec / static_cast (1e9); + expiry.tv_nsec = tv_nsec - static_cast (1e9 * sec_diff); + expiry.tv_sec += sec_diff; + } else { + expiry.tv_nsec = tv_nsec; + } +} + +int64_t +MillisecondTimer::remaining () +{ + timespec now(timespec_now()); + int64_t millis = (expiry.tv_sec - now.tv_sec) * 1e3; + millis += (expiry.tv_nsec - now.tv_nsec) / 1e6; + return millis; +} + +timespec +MillisecondTimer::timespec_now () +{ + 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 + return time; +} + +timespec +timespec_from_ms (const uint32_t millis) +{ + timespec time; + time.tv_sec = millis / 1e3; + time.tv_nsec = (millis - (time.tv_sec * 1e3)) * 1e6; + return time; +} + Serial::SerialImpl::SerialImpl (const string &port, unsigned long baudrate, bytesize_t bytesize, parity_t parity, stopbits_t stopbits, @@ -253,7 +303,7 @@ Serial::SerialImpl::reconfigurePort () // other than those specified by POSIX. The driver for the underlying serial hardware // ultimately determines which baud rates can be used. This ioctl sets both the input // and output speed. - speed_t new_baud = static_cast(baudrate_); + speed_t new_baud = static_cast (baudrate_); if (-1 == ioctl (fd_, IOSSIOSPEED, &new_baud, 1)) { THROW (IOException, errno); } @@ -266,7 +316,7 @@ Serial::SerialImpl::reconfigurePort () } // set custom divisor - ser.custom_divisor = ser.baud_base / (int) baudrate_; + ser.custom_divisor = ser.baud_base / static_cast (baudrate_); // update flags ser.flags &= ~ASYNC_SPD_MASK; ser.flags |= ASYNC_SPD_CUST; @@ -404,35 +454,6 @@ Serial::SerialImpl::available () } } -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 -} - -inline void -diff_timespec (timespec &start, timespec &end, timespec &result) { - if (start.tv_sec > end.tv_sec) { - throw SerialException ("Timetravel, start time later than end time."); - } - result.tv_sec = end.tv_sec - start.tv_sec; - result.tv_nsec = end.tv_nsec - start.tv_nsec; - if (result.tv_nsec < 0) { - result.tv_nsec = 1e9 - result.tv_nsec; - result.tv_sec -= 1; - } -} - size_t Serial::SerialImpl::read (uint8_t *buf, size_t size) { @@ -442,58 +463,37 @@ Serial::SerialImpl::read (uint8_t *buf, size_t size) } fd_set readfds; size_t bytes_read = 0; - // Setup the total_timeout timeval - // This timeout is maximum time before a timeout after read is called - struct timeval total_timeout; + // Calculate total timeout in milliseconds t_c + (t_m * N) long total_timeout_ms = timeout_.read_timeout_constant; - total_timeout_ms += timeout_.read_timeout_multiplier*static_cast(size); - total_timeout.tv_sec = total_timeout_ms / 1000; - total_timeout.tv_usec = static_cast(total_timeout_ms % 1000); - total_timeout.tv_usec *= 1000; // To convert to micro seconds - // Setup the inter byte timeout - struct timeval inter_byte_timeout; - inter_byte_timeout.tv_sec = timeout_.inter_byte_timeout / 1000; - inter_byte_timeout.tv_usec = - static_cast (timeout_.inter_byte_timeout % 1000); - inter_byte_timeout.tv_usec *= 1000; // To convert to micro seconds + total_timeout_ms += timeout_.read_timeout_multiplier * static_cast (size); + MillisecondTimer total_timeout(total_timeout_ms); + + // Pre-fill buffer with available bytes + { + ssize_t bytes_read_now = ::read (fd_, buf, size); + if (bytes_read_now > 0) { + bytes_read = bytes_read_now; + } + } + while (bytes_read < size) { - // Setup the select timeout timeval - struct timeval timeout; - // If the total_timeout is less than the inter_byte_timeout - if (total_timeout.tv_sec < inter_byte_timeout.tv_sec - || (total_timeout.tv_sec == inter_byte_timeout.tv_sec - && total_timeout.tv_usec < inter_byte_timeout.tv_sec)) - { - // Then set the select timeout to use the total time - timeout = total_timeout; - } else { - // Else set the select timeout to use the inter byte time - timeout = inter_byte_timeout; + int64_t timeout_remaining_ms = total_timeout.remaining(); + if (timeout_remaining_ms <= 0) { + // Timed out + break; } + + // Timeout for the next select is whichever is less of the remaining + // total read timeout and the inter-byte timeout. + timespec timeout(timespec_from_ms(std::min(static_cast (timeout_remaining_ms), + timeout_.inter_byte_timeout))); + FD_ZERO (&readfds); FD_SET (fd_, &readfds); - // Begin timing select - struct timespec start, end; - get_time_now (start); + // Call select to block for serial data or a timeout - int r = select (fd_ + 1, &readfds, NULL, NULL, &timeout); - // Calculate difference and update the structure - get_time_now (end); - // Calculate the time select took - struct timespec diff; - diff_timespec (start, end, diff); - // Update the timeout - if (total_timeout.tv_sec <= diff.tv_sec) { - total_timeout.tv_sec = 0; - } else { - total_timeout.tv_sec -= diff.tv_sec; - } - if (total_timeout.tv_usec <= (diff.tv_nsec / 1000)) { - total_timeout.tv_usec = 0; - } else { - total_timeout.tv_usec -= (diff.tv_nsec / 1000); - } + int r = pselect (fd_ + 1, &readfds, NULL, NULL, &timeout, NULL); // Figure out what happened by looking at select's response 'r' /** Error **/ @@ -559,41 +559,25 @@ Serial::SerialImpl::write (const uint8_t *data, size_t length) } fd_set writefds; size_t bytes_written = 0; - struct timeval timeout; - timeout.tv_sec = timeout_.write_timeout_constant / 1000; - timeout.tv_usec = static_cast (timeout_.write_timeout_multiplier % 1000); - timeout.tv_usec *= 1000; // To convert to micro seconds + + // Calculate total timeout in milliseconds t_c + (t_m * N) + long total_timeout_ms = timeout_.write_timeout_constant; + total_timeout_ms += timeout_.write_timeout_multiplier * static_cast (length); + MillisecondTimer total_timeout(total_timeout_ms); + while (bytes_written < length) { + int64_t timeout_remaining_ms = total_timeout.remaining(); + if (timeout_remaining_ms <= 0) { + // Timed out + break; + } + timespec timeout(timespec_from_ms(timeout_remaining_ms)); + FD_ZERO (&writefds); FD_SET (fd_, &writefds); - // 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, NULL, &writefds, NULL, &timeout); -#if !defined(__linux__) - // Calculate difference and update the structure - get_time_now(end); - // Calculate the time select took - struct timespec diff; - diff_timespec(start, end, diff); - // 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_nsec / 1000)) { - timeout.tv_usec = 0; - } else { - timeout.tv_usec -= (diff.tv_nsec / 1000); - } -#endif + int r = pselect (fd_ + 1, NULL, &writefds, NULL, &timeout, NULL); // Figure out what happened by looking at select's response 'r' /** Error **/ diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 00000000..38718e6c --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,10 @@ +if(UNIX) + catkin_add_gtest(${PROJECT_NAME}-test unix_serial_tests.cc) + target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${Boost_LIBRARIES}) + if(NOT APPLE) + target_link_libraries(${PROJECT_NAME}-test util) + endif() + + catkin_add_gtest(${PROJECT_NAME}-test-timer unit/unix_timer_tests.cc) + target_link_libraries(${PROJECT_NAME}-test-timer ${PROJECT_NAME}) +endif() diff --git a/tests/unit/unix_timer_tests.cc b/tests/unit/unix_timer_tests.cc new file mode 100644 index 00000000..5bbd1edc --- /dev/null +++ b/tests/unit/unix_timer_tests.cc @@ -0,0 +1,63 @@ +#include "gtest/gtest.h" +#include "serial/impl/unix.h" + +#include +#include + +using serial::MillisecondTimer; + +namespace { + +/** + * Do 100 trials of timing gaps between 0 and 19 milliseconds. + * Expect accuracy within one millisecond. + */ +TEST(timer_tests, short_intervals) { + for (int trial = 0; trial < 100; trial++) + { + uint32_t ms = rand() % 20; + MillisecondTimer mt(ms); + usleep(1000 * ms); + int32_t r = mt.remaining(); + + // 1ms slush, for the cost of calling usleep. + EXPECT_NEAR(r+1, 0, 1); + } +} + +TEST(timer_tests, overlapping_long_intervals) { + MillisecondTimer* timers[10]; + + // Experimentally determined. Corresponds to the extra time taken by the loops, + // the big usleep, and the test infrastructure itself. + const int slush_factor = 14; + + // Set up the timers to each time one second, 1ms apart. + for (int t = 0; t < 10; t++) + { + timers[t] = new MillisecondTimer(1000); + usleep(1000); + } + + // Check in on them after 500ms. + usleep(500000); + for (int t = 0; t < 10; t++) + { + EXPECT_NEAR(timers[t]->remaining(), 500 - slush_factor + t, 5); + } + + // Check in on them again after another 500ms and free them. + usleep(500000); + for (int t = 0; t < 10; t++) + { + EXPECT_NEAR(timers[t]->remaining(), -slush_factor + t, 5); + delete timers[t]; + } +} + +} // namespace + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tests/serial_tests.cc b/tests/unix_serial_tests.cc similarity index 100% rename from tests/serial_tests.cc rename to tests/unix_serial_tests.cc