diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index fe7c6c07b5..bd6a95280e 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -20,3 +20,4 @@ set(CMAKE_CXX_FLAGS "-Wall -Wextra -Werror -pedantic -std=c++0x -g3 -O1") # GCC add_executable(log_subscriber test/log_subscriber.cpp) target_link_libraries(log_subscriber ${UAVCAN_LIB}) +target_link_libraries(log_subscriber rt) diff --git a/libuavcan_drivers/linux/include/uavcan_linux/clock.hpp b/libuavcan_drivers/linux/include/uavcan_linux/clock.hpp index 8d1c8b69c3..c6f7f89202 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/clock.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/clock.hpp @@ -1 +1,139 @@ - +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include + +namespace uavcan_linux +{ + +enum class ClockAdjustmentMode +{ + SystemWide, + PerDriverPrivate +}; + + +class SystemClockDriver : public uavcan::ISystemClock +{ + uavcan::UtcDuration private_adj_; + uavcan::UtcDuration gradual_adj_limit_; + const ClockAdjustmentMode adj_mode_; + std::uint64_t step_adj_cnt_; + std::uint64_t gradual_adj_cnt_; + + static constexpr int64_t Int1e6 = 1000000; + + bool performStepAdjustment(uavcan::UtcDuration adjustment) const + { + step_adj_cnt_++; + const int64_t usec = adjustment.toUSec(); + timeval tv; + if (gettimeofday(&tv, NULL) != 0) + { + return false; + } + tv.tv_sec += usec / Int1e6; + tv.tv_usec += usec % Int1e6; + return settimeofday(&tv, nullptr) == 0; + } + + bool performGradualAdjustment(uavcan::UtcDuration adjustment) const + { + gradual_adj_cnt_++; + const int64_t usec = adjustment.toUSec(); + timeval tv; + tv.tv_sec = usec / Int1e6; + tv.tv_usec = usec % Int1e6; + return adjtime(&tv, nullptr) == 0; + } + +public: + SystemClockDriver(ClockAdjustmentMode adj_mode = ClockAdjustmentMode::SystemWide) + : gradual_adj_limit_(uavcan::UtcDuration::fromMSec(4000)) + , adj_mode_(adj_mode) + , step_adj_cnt_(0) + , gradual_adj_cnt_(0) + { } + + uavcan::MonotonicTime getMonotonic() const + { + struct timespec ts; + if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) + { + throw Exception("Failed to get monotonic time"); + } + return uavcan::MonotonicTime::fromUSec(uint64_t(ts.tv_sec) * 1000000UL + ts.tv_nsec / 1000UL); + } + + uavcan::UtcTime getUtc() const + { + timeval tv; + if (gettimeofday(&tv, NULL) != 0) + { + throw Exception("Failed to get UTC time"); + } + uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(uint64_t(tv.tv_sec) * 1000000UL + tv.tv_usec); + if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate) + { + utc += private_adj_; + } + return utc; + } + + void adjustUtc(uavcan::UtcDuration adjustment) + { + if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate) + { + private_adj_ += adjustment; + } + else + { + assert(private_adj_.isZero()); + assert(!gradual_adj_limit_.isNegative()); + + bool success = false; + if (adjustment.getAbs() < gradual_adj_limit_) + { + success = performGradualAdjustment(adjustment); + } + else + { + success = performStepAdjustment(adjustment); + } + if (!success) + { + throw Exception("Clock adjustment failed"); + } + } + } + + void setGradualAdjustmentLimit(uavcan::UtcDuration limit) + { + if (limit.isNegative()) + { + limit = uavcan::UtcDuration(); + } + gradual_adj_limit_ = limit; + } + + uavcan::UtcDuration getGradualAdjustmentLimit() const { return gradual_adj_limit_; } + + ClockAdjustmentMode getAdjustmentMode() const { return adj_mode_; } + + uavcan::UtcDuration getPrivateAdjustment() const { return private_adj_; } + + std::uint64_t getStepAdjustmentCount() const { return step_adj_cnt_; } + std::uint64_t getGradualAdjustmentCount() const { return gradual_adj_cnt_; } + std::uint64_t getAdjustmentCount() const + { + return getStepAdjustmentCount() + getGradualAdjustmentCount(); + } +}; + +} diff --git a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp new file mode 100644 index 0000000000..23ad6f2d09 --- /dev/null +++ b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp @@ -0,0 +1,26 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include + +namespace uavcan_linux +{ + +class Exception : public std::runtime_error +{ + int errno_; + +public: + Exception(const char* descr) + : std::runtime_error(descr) + , errno_(errno) + { } + + int getErrno() const { return errno_; } +}; + +} diff --git a/libuavcan_drivers/linux/test/log_subscriber.cpp b/libuavcan_drivers/linux/test/log_subscriber.cpp index f004c9fd1d..2408265577 100644 --- a/libuavcan_drivers/linux/test/log_subscriber.cpp +++ b/libuavcan_drivers/linux/test/log_subscriber.cpp @@ -3,12 +3,28 @@ */ #include +#include #include #include #include int main() { - std::cout << "Hello owl." << std::endl; + double sec = 0; + std::cout << "Enter system time adjustment in seconds: " << std::endl; + std::cin >> sec; + + uavcan_linux::SystemClockDriver clock; + + try + { + clock.adjustUtc(uavcan::UtcDuration::fromUSec(sec * 1e6)); + } + catch (const uavcan_linux::Exception& ex) + { + std::cout << ex.what() << std::endl; + std::cout << strerror(ex.getErrno()) << std::endl; + } + return 0; }