Руслан Урядинский / libuavcan

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   UAVCAN UAVCAN_Subscriber

libuavcan_drivers/linux/include/uavcan_linux/clock.hpp@0:dfe6edabb8ec, 2018-04-14 (annotated)

Committer:
RuslanUrya
Date:
Sat Apr 14 10:25:32 2018 +0000
Revision:
0:dfe6edabb8ec
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RuslanUrya 0:dfe6edabb8ec 1 /*
RuslanUrya 0:dfe6edabb8ec 2 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
RuslanUrya 0:dfe6edabb8ec 3 */
RuslanUrya 0:dfe6edabb8ec 4
RuslanUrya 0:dfe6edabb8ec 5 #pragma once
RuslanUrya 0:dfe6edabb8ec 6
RuslanUrya 0:dfe6edabb8ec 7 #include <cassert>
RuslanUrya 0:dfe6edabb8ec 8 #include <ctime>
RuslanUrya 0:dfe6edabb8ec 9 #include <cstdint>
RuslanUrya 0:dfe6edabb8ec 10
RuslanUrya 0:dfe6edabb8ec 11 #include <unistd.h>
RuslanUrya 0:dfe6edabb8ec 12 #include <sys/time.h>
RuslanUrya 0:dfe6edabb8ec 13 #include <sys/types.h>
RuslanUrya 0:dfe6edabb8ec 14
RuslanUrya 0:dfe6edabb8ec 15 #include <uavcan/driver/system_clock.hpp>
RuslanUrya 0:dfe6edabb8ec 16 #include <uavcan_linux/exception.hpp>
RuslanUrya 0:dfe6edabb8ec 17
RuslanUrya 0:dfe6edabb8ec 18 namespace uavcan_linux
RuslanUrya 0:dfe6edabb8ec 19 {
RuslanUrya 0:dfe6edabb8ec 20 /**
RuslanUrya 0:dfe6edabb8ec 21 * Different adjustment modes can be used for time synchronization
RuslanUrya 0:dfe6edabb8ec 22 */
RuslanUrya 0:dfe6edabb8ec 23 enum class ClockAdjustmentMode
RuslanUrya 0:dfe6edabb8ec 24 {
RuslanUrya 0:dfe6edabb8ec 25 SystemWide, ///< Adjust the clock globally for the whole system; requires root privileges
RuslanUrya 0:dfe6edabb8ec 26 PerDriverPrivate ///< Adjust the clock only for the current driver instance
RuslanUrya 0:dfe6edabb8ec 27 };
RuslanUrya 0:dfe6edabb8ec 28
RuslanUrya 0:dfe6edabb8ec 29 /**
RuslanUrya 0:dfe6edabb8ec 30 * Linux system clock driver.
RuslanUrya 0:dfe6edabb8ec 31 * Requires librt.
RuslanUrya 0:dfe6edabb8ec 32 */
RuslanUrya 0:dfe6edabb8ec 33 class SystemClock : public uavcan::ISystemClock
RuslanUrya 0:dfe6edabb8ec 34 {
RuslanUrya 0:dfe6edabb8ec 35 uavcan::UtcDuration private_adj_;
RuslanUrya 0:dfe6edabb8ec 36 uavcan::UtcDuration gradual_adj_limit_;
RuslanUrya 0:dfe6edabb8ec 37 const ClockAdjustmentMode adj_mode_;
RuslanUrya 0:dfe6edabb8ec 38 std::uint64_t step_adj_cnt_;
RuslanUrya 0:dfe6edabb8ec 39 std::uint64_t gradual_adj_cnt_;
RuslanUrya 0:dfe6edabb8ec 40
RuslanUrya 0:dfe6edabb8ec 41 static constexpr std::int64_t Int1e6 = 1000000;
RuslanUrya 0:dfe6edabb8ec 42 static constexpr std::uint64_t UInt1e6 = 1000000;
RuslanUrya 0:dfe6edabb8ec 43
RuslanUrya 0:dfe6edabb8ec 44 bool performStepAdjustment(const uavcan::UtcDuration adjustment)
RuslanUrya 0:dfe6edabb8ec 45 {
RuslanUrya 0:dfe6edabb8ec 46 step_adj_cnt_++;
RuslanUrya 0:dfe6edabb8ec 47 const std::int64_t usec = adjustment.toUSec();
RuslanUrya 0:dfe6edabb8ec 48 timeval tv;
RuslanUrya 0:dfe6edabb8ec 49 if (gettimeofday(&tv, NULL) != 0)
RuslanUrya 0:dfe6edabb8ec 50 {
RuslanUrya 0:dfe6edabb8ec 51 return false;
RuslanUrya 0:dfe6edabb8ec 52 }
RuslanUrya 0:dfe6edabb8ec 53 tv.tv_sec += usec / Int1e6;
RuslanUrya 0:dfe6edabb8ec 54 tv.tv_usec += usec % Int1e6;
RuslanUrya 0:dfe6edabb8ec 55 return settimeofday(&tv, nullptr) == 0;
RuslanUrya 0:dfe6edabb8ec 56 }
RuslanUrya 0:dfe6edabb8ec 57
RuslanUrya 0:dfe6edabb8ec 58 bool performGradualAdjustment(const uavcan::UtcDuration adjustment)
RuslanUrya 0:dfe6edabb8ec 59 {
RuslanUrya 0:dfe6edabb8ec 60 gradual_adj_cnt_++;
RuslanUrya 0:dfe6edabb8ec 61 const std::int64_t usec = adjustment.toUSec();
RuslanUrya 0:dfe6edabb8ec 62 timeval tv;
RuslanUrya 0:dfe6edabb8ec 63 tv.tv_sec = usec / Int1e6;
RuslanUrya 0:dfe6edabb8ec 64 tv.tv_usec = usec % Int1e6;
RuslanUrya 0:dfe6edabb8ec 65 return adjtime(&tv, nullptr) == 0;
RuslanUrya 0:dfe6edabb8ec 66 }
RuslanUrya 0:dfe6edabb8ec 67
RuslanUrya 0:dfe6edabb8ec 68 public:
RuslanUrya 0:dfe6edabb8ec 69 /**
RuslanUrya 0:dfe6edabb8ec 70 * By default, the clock adjustment mode will be selected automatically - global if root, private otherwise.
RuslanUrya 0:dfe6edabb8ec 71 */
RuslanUrya 0:dfe6edabb8ec 72 explicit SystemClock(ClockAdjustmentMode adj_mode = detectPreferredClockAdjustmentMode())
RuslanUrya 0:dfe6edabb8ec 73 : gradual_adj_limit_(uavcan::UtcDuration::fromMSec(4000))
RuslanUrya 0:dfe6edabb8ec 74 , adj_mode_(adj_mode)
RuslanUrya 0:dfe6edabb8ec 75 , step_adj_cnt_(0)
RuslanUrya 0:dfe6edabb8ec 76 , gradual_adj_cnt_(0)
RuslanUrya 0:dfe6edabb8ec 77 { }
RuslanUrya 0:dfe6edabb8ec 78
RuslanUrya 0:dfe6edabb8ec 79 /**
RuslanUrya 0:dfe6edabb8ec 80 * Returns monotonic timestamp from librt.
RuslanUrya 0:dfe6edabb8ec 81 * @throws uavcan_linux::Exception.
RuslanUrya 0:dfe6edabb8ec 82 */
RuslanUrya 0:dfe6edabb8ec 83 uavcan::MonotonicTime getMonotonic() const override
RuslanUrya 0:dfe6edabb8ec 84 {
RuslanUrya 0:dfe6edabb8ec 85 timespec ts;
RuslanUrya 0:dfe6edabb8ec 86 if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0)
RuslanUrya 0:dfe6edabb8ec 87 {
RuslanUrya 0:dfe6edabb8ec 88 throw Exception("Failed to get monotonic time");
RuslanUrya 0:dfe6edabb8ec 89 }
RuslanUrya 0:dfe6edabb8ec 90 return uavcan::MonotonicTime::fromUSec(std::uint64_t(ts.tv_sec) * UInt1e6 + ts.tv_nsec / 1000);
RuslanUrya 0:dfe6edabb8ec 91 }
RuslanUrya 0:dfe6edabb8ec 92
RuslanUrya 0:dfe6edabb8ec 93 /**
RuslanUrya 0:dfe6edabb8ec 94 * Returns wall time from gettimeofday().
RuslanUrya 0:dfe6edabb8ec 95 * @throws uavcan_linux::Exception.
RuslanUrya 0:dfe6edabb8ec 96 */
RuslanUrya 0:dfe6edabb8ec 97 uavcan::UtcTime getUtc() const override
RuslanUrya 0:dfe6edabb8ec 98 {
RuslanUrya 0:dfe6edabb8ec 99 timeval tv;
RuslanUrya 0:dfe6edabb8ec 100 if (gettimeofday(&tv, NULL) != 0)
RuslanUrya 0:dfe6edabb8ec 101 {
RuslanUrya 0:dfe6edabb8ec 102 throw Exception("Failed to get UTC time");
RuslanUrya 0:dfe6edabb8ec 103 }
RuslanUrya 0:dfe6edabb8ec 104 uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(std::uint64_t(tv.tv_sec) * UInt1e6 + tv.tv_usec);
RuslanUrya 0:dfe6edabb8ec 105 if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate)
RuslanUrya 0:dfe6edabb8ec 106 {
RuslanUrya 0:dfe6edabb8ec 107 utc += private_adj_;
RuslanUrya 0:dfe6edabb8ec 108 }
RuslanUrya 0:dfe6edabb8ec 109 return utc;
RuslanUrya 0:dfe6edabb8ec 110 }
RuslanUrya 0:dfe6edabb8ec 111
RuslanUrya 0:dfe6edabb8ec 112 /**
RuslanUrya 0:dfe6edabb8ec 113 * Adjusts the wall clock.
RuslanUrya 0:dfe6edabb8ec 114 * Behavior depends on the selected clock adjustment mode - @ref ClockAdjustmentMode.
RuslanUrya 0:dfe6edabb8ec 115 * Clock adjustment mode can be set only once via constructor.
RuslanUrya 0:dfe6edabb8ec 116 *
RuslanUrya 0:dfe6edabb8ec 117 * If the system wide adjustment mode is selected, two ways for performing adjustment exist:
RuslanUrya 0:dfe6edabb8ec 118 * - Gradual adjustment using adjtime(), if the phase error is less than gradual adjustment limit.
RuslanUrya 0:dfe6edabb8ec 119 * - Step adjustment using settimeofday(), if the phase error is above gradual adjustment limit.
RuslanUrya 0:dfe6edabb8ec 120 * The gradual adjustment limit can be configured at any time via the setter method.
RuslanUrya 0:dfe6edabb8ec 121 *
RuslanUrya 0:dfe6edabb8ec 122 * @throws uavcan_linux::Exception.
RuslanUrya 0:dfe6edabb8ec 123 */
RuslanUrya 0:dfe6edabb8ec 124 void adjustUtc(const uavcan::UtcDuration adjustment) override
RuslanUrya 0:dfe6edabb8ec 125 {
RuslanUrya 0:dfe6edabb8ec 126 if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate)
RuslanUrya 0:dfe6edabb8ec 127 {
RuslanUrya 0:dfe6edabb8ec 128 private_adj_ += adjustment;
RuslanUrya 0:dfe6edabb8ec 129 }
RuslanUrya 0:dfe6edabb8ec 130 else
RuslanUrya 0:dfe6edabb8ec 131 {
RuslanUrya 0:dfe6edabb8ec 132 assert(private_adj_.isZero());
RuslanUrya 0:dfe6edabb8ec 133 assert(!gradual_adj_limit_.isNegative());
RuslanUrya 0:dfe6edabb8ec 134
RuslanUrya 0:dfe6edabb8ec 135 bool success = false;
RuslanUrya 0:dfe6edabb8ec 136 if (adjustment.getAbs() < gradual_adj_limit_)
RuslanUrya 0:dfe6edabb8ec 137 {
RuslanUrya 0:dfe6edabb8ec 138 success = performGradualAdjustment(adjustment);
RuslanUrya 0:dfe6edabb8ec 139 }
RuslanUrya 0:dfe6edabb8ec 140 else
RuslanUrya 0:dfe6edabb8ec 141 {
RuslanUrya 0:dfe6edabb8ec 142 success = performStepAdjustment(adjustment);
RuslanUrya 0:dfe6edabb8ec 143 }
RuslanUrya 0:dfe6edabb8ec 144 if (!success)
RuslanUrya 0:dfe6edabb8ec 145 {
RuslanUrya 0:dfe6edabb8ec 146 throw Exception("Clock adjustment failed");
RuslanUrya 0:dfe6edabb8ec 147 }
RuslanUrya 0:dfe6edabb8ec 148 }
RuslanUrya 0:dfe6edabb8ec 149 }
RuslanUrya 0:dfe6edabb8ec 150
RuslanUrya 0:dfe6edabb8ec 151 /**
RuslanUrya 0:dfe6edabb8ec 152 * Sets the maximum phase error to use adjtime().
RuslanUrya 0:dfe6edabb8ec 153 * If the phase error exceeds this value, settimeofday() will be used instead.
RuslanUrya 0:dfe6edabb8ec 154 */
RuslanUrya 0:dfe6edabb8ec 155 void setGradualAdjustmentLimit(uavcan::UtcDuration limit)
RuslanUrya 0:dfe6edabb8ec 156 {
RuslanUrya 0:dfe6edabb8ec 157 if (limit.isNegative())
RuslanUrya 0:dfe6edabb8ec 158 {
RuslanUrya 0:dfe6edabb8ec 159 limit = uavcan::UtcDuration();
RuslanUrya 0:dfe6edabb8ec 160 }
RuslanUrya 0:dfe6edabb8ec 161 gradual_adj_limit_ = limit;
RuslanUrya 0:dfe6edabb8ec 162 }
RuslanUrya 0:dfe6edabb8ec 163
RuslanUrya 0:dfe6edabb8ec 164 uavcan::UtcDuration getGradualAdjustmentLimit() const { return gradual_adj_limit_; }
RuslanUrya 0:dfe6edabb8ec 165
RuslanUrya 0:dfe6edabb8ec 166 ClockAdjustmentMode getAdjustmentMode() const { return adj_mode_; }
RuslanUrya 0:dfe6edabb8ec 167
RuslanUrya 0:dfe6edabb8ec 168 /**
RuslanUrya 0:dfe6edabb8ec 169 * This is only applicable if the selected clock adjustment mode is private.
RuslanUrya 0:dfe6edabb8ec 170 * In system wide mode this method will always return zero duration.
RuslanUrya 0:dfe6edabb8ec 171 */
RuslanUrya 0:dfe6edabb8ec 172 uavcan::UtcDuration getPrivateAdjustment() const { return private_adj_; }
RuslanUrya 0:dfe6edabb8ec 173
RuslanUrya 0:dfe6edabb8ec 174 /**
RuslanUrya 0:dfe6edabb8ec 175 * Statistics that allows to evaluate clock sync preformance.
RuslanUrya 0:dfe6edabb8ec 176 */
RuslanUrya 0:dfe6edabb8ec 177 std::uint64_t getStepAdjustmentCount() const { return step_adj_cnt_; }
RuslanUrya 0:dfe6edabb8ec 178 std::uint64_t getGradualAdjustmentCount() const { return gradual_adj_cnt_; }
RuslanUrya 0:dfe6edabb8ec 179 std::uint64_t getAdjustmentCount() const
RuslanUrya 0:dfe6edabb8ec 180 {
RuslanUrya 0:dfe6edabb8ec 181 return getStepAdjustmentCount() + getGradualAdjustmentCount();
RuslanUrya 0:dfe6edabb8ec 182 }
RuslanUrya 0:dfe6edabb8ec 183
RuslanUrya 0:dfe6edabb8ec 184 /**
RuslanUrya 0:dfe6edabb8ec 185 * This static method decides what is the optimal clock sync adjustment mode for the current configuration.
RuslanUrya 0:dfe6edabb8ec 186 * It selects system wide mode if the application is running as root; otherwise it prefers
RuslanUrya 0:dfe6edabb8ec 187 * the private adjustment mode because the system wide mode requires root privileges.
RuslanUrya 0:dfe6edabb8ec 188 */
RuslanUrya 0:dfe6edabb8ec 189 static ClockAdjustmentMode detectPreferredClockAdjustmentMode()
RuslanUrya 0:dfe6edabb8ec 190 {
RuslanUrya 0:dfe6edabb8ec 191 const bool godmode = geteuid() == 0;
RuslanUrya 0:dfe6edabb8ec 192 return godmode ? ClockAdjustmentMode::SystemWide : ClockAdjustmentMode::PerDriverPrivate;
RuslanUrya 0:dfe6edabb8ec 193 }
RuslanUrya 0:dfe6edabb8ec 194 };
RuslanUrya 0:dfe6edabb8ec 195
RuslanUrya 0:dfe6edabb8ec 196 }