diff --git a/libuavcan_drivers/linux/include/uavcan_linux/clock.hpp b/libuavcan_drivers/linux/include/uavcan_linux/clock.hpp index 6000ae2120..954269e84f 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/clock.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/clock.hpp @@ -60,12 +60,6 @@ class SystemClock : public uavcan::ISystemClock return adjtime(&tv, nullptr) == 0; } - static ClockAdjustmentMode detectPreferredClockAdjustmentMode() - { - const bool godmode = geteuid() == 0; - return godmode ? ClockAdjustmentMode::SystemWide : ClockAdjustmentMode::PerDriverPrivate; - } - public: SystemClock(ClockAdjustmentMode adj_mode = detectPreferredClockAdjustmentMode()) : gradual_adj_limit_(uavcan::UtcDuration::fromMSec(4000)) @@ -147,6 +141,12 @@ public: { return getStepAdjustmentCount() + getGradualAdjustmentCount(); } + + static ClockAdjustmentMode detectPreferredClockAdjustmentMode() + { + const bool godmode = geteuid() == 0; + return godmode ? ClockAdjustmentMode::SystemWide : ClockAdjustmentMode::PerDriverPrivate; + } }; } diff --git a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp index 23ad6f2d09..966c95c55f 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp @@ -12,10 +12,10 @@ namespace uavcan_linux class Exception : public std::runtime_error { - int errno_; + const int errno_; public: - Exception(const char* descr) + Exception(const std::string& descr) : std::runtime_error(descr) , errno_(errno) { } diff --git a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp new file mode 100644 index 0000000000..533a551a65 --- /dev/null +++ b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp @@ -0,0 +1,146 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include + +namespace uavcan_linux +{ +/** + * Contains all drivers needed for uavcan::Node. + */ +struct DriverPack +{ + SocketCanDriver can; + SystemClock clock; + + DriverPack(ClockAdjustmentMode clock_adjustment_mode) + : can() + , clock(clock_adjustment_mode) + { } +}; + +typedef std::shared_ptr DriverPackPtr; + +typedef std::shared_ptr TimerPtr; + +static constexpr std::size_t NodeMemPoolSize = 1024 * 512; // One size fits all + +/** + * Wrapper for uavcan::Node with some additional convenience functions. + */ +class Node : public uavcan::Node +{ + DriverPackPtr driver_pack_; + + static void enforce(int error, const char* msg) + { + if (error < 0) + { + throw Exception(msg); + } + } + +public: + /** + * Simple forwarding constructor, compatible with uavcan::Node + */ + Node(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) + : uavcan::Node(can_driver, clock) + { } + + /** + * Takes ownership of the driver container. + */ + Node(DriverPackPtr driver_pack) + : uavcan::Node(driver_pack->can, driver_pack->clock) + , driver_pack_(driver_pack) + { } + + template + std::shared_ptr> + makeSubscriber(const typename uavcan::Subscriber::Callback& cb) + { + std::shared_ptr> p(new uavcan::Subscriber(*this)); + enforce(p->start(cb), "Subscriber start"); + return p; + } + + template + std::shared_ptr> + makePublisher(uavcan::MonotonicDuration tx_timeout = uavcan::Publisher::getDefaultTxTimeout()) + { + std::shared_ptr> p(new uavcan::Publisher(*this)); + enforce(p->init(), "Publisher init"); + p->setTxTimeout(tx_timeout); + return p; + } + + template + std::shared_ptr> + makeServiceServer(const typename uavcan::ServiceServer::Callback& cb) + { + std::shared_ptr> p(new uavcan::ServiceServer(*this)); + enforce(p->start(cb), "ServiceServer start"); + return p; + } + + template + std::shared_ptr> + makeServiceClient(const typename uavcan::ServiceClient::Callback& cb) + { + std::shared_ptr> p(new uavcan::ServiceClient(*this)); + enforce(p->init(), "ServiceClient init"); + p->setCallback(cb); + return p; + } + + TimerPtr makeTimer(uavcan::MonotonicTime deadline, const typename uavcan::Timer::Callback& cb) + { + TimerPtr p(new uavcan::Timer(*this)); + p->setCallback(cb); + p->startOneShotWithDeadline(deadline); + return p; + } + + TimerPtr makeTimer(uavcan::MonotonicDuration period, const typename uavcan::Timer::Callback& cb) + { + TimerPtr p(new uavcan::Timer(*this)); + p->setCallback(cb); + p->startPeriodic(period); + return p; + } +}; + +typedef std::shared_ptr NodePtr; + +/** + * Constructs Node with explicitly specified ClockAdjustmentMode. + */ +static inline NodePtr makeNode(const std::vector& iface_names, ClockAdjustmentMode clock_adjustment_mode) +{ + DriverPackPtr dp(new DriverPack(clock_adjustment_mode)); + for (auto ifn : iface_names) + { + if (dp->can.addIface(ifn) < 0) + { + throw Exception("Failed to add iface " + ifn); + } + } + return NodePtr(new Node(dp)); +} + +/** + * This is the preferred way to make Node. + */ +static inline NodePtr makeNode(const std::vector& iface_names) +{ + return makeNode(iface_names, SystemClock::detectPreferredClockAdjustmentMode()); +} + +} diff --git a/libuavcan_drivers/linux/include/uavcan_linux/uavcan_linux.hpp b/libuavcan_drivers/linux/include/uavcan_linux/uavcan_linux.hpp index 32428411f2..3d68a42e99 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/uavcan_linux.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/uavcan_linux.hpp @@ -8,3 +8,4 @@ #include #include +#include