diff --git a/libuavcan/include/uavcan/time.hpp b/libuavcan/include/uavcan/time.hpp new file mode 100644 index 0000000000..29b27547d6 --- /dev/null +++ b/libuavcan/include/uavcan/time.hpp @@ -0,0 +1,227 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +template +class DurationBase +{ + int64_t usec_; + +public: + DurationBase() + : usec_(0) + { + StaticAssert<(sizeof(D) == 8)>::check(); + } + + static D fromUSec(int64_t us) + { + D d; + d.usec_ = us; + return d; + } + static D fromMSec(int64_t ms) { return fromUSec(ms * 1000); } + + int64_t toUSec() const { return usec_; } + int64_t toMSec() const { return usec_ / 1000; } + + D getAbs() const { return D::fromUSec(std::abs(usec_)); } + + bool isPositive() const { return usec_ > 0; } + bool isNegative() const { return usec_ < 0; } + bool isZero() const { return usec_ == 0; } + + bool operator==(const D& r) const { return usec_ == r.usec_; } + bool operator!=(const D& r) const { return !operator==(r); } + + bool operator<(const D& r) const { return usec_ < r.usec_; } + bool operator>(const D& r) const { return usec_ > r.usec_; } + bool operator<=(const D& r) const { return usec_ <= r.usec_; } + bool operator>=(const D& r) const { return usec_ >= r.usec_; } + + D operator+(const D &r) const { return fromUSec(usec_ + r.usec_); } // TODO: overflow check + D operator-(const D &r) const { return fromUSec(usec_ - r.usec_); } // ditto + + D operator-() const { return fromUSec(-usec_); } + + D& operator+=(const D &r) + { + *this = *this + r; + return *static_cast(this); + } + D& operator-=(const D &r) + { + *this = *this - r; + return *static_cast(this); + } + + template + D operator*(Scale scale) const { return fromUSec(usec_ * scale); } + + template + D& operator*=(Scale scale) + { + *this = *this * scale; + return *static_cast(this); + } + + std::string toString() const; +}; + + +template +class TimeBase +{ + uint64_t usec_; + +public: + TimeBase() + : usec_(0) + { + StaticAssert<(sizeof(T) == 8)>::check(); + StaticAssert<(sizeof(D) == 8)>::check(); + } + + static T fromUSec(uint64_t us) + { + T d; + d.usec_ = us; + return d; + } + static T fromMSec(uint64_t ms) { return fromUSec(ms * 1000); } + + uint64_t toUSec() const { return usec_; } + uint64_t toMSec() const { return usec_ / 1000; } + + bool isZero() const { return usec_ == 0; } + + bool operator==(const T& r) const { return usec_ == r.usec_; } + bool operator!=(const T& r) const { return !operator==(r); } + + bool operator<(const T& r) const { return usec_ < r.usec_; } + bool operator>(const T& r) const { return usec_ > r.usec_; } + bool operator<=(const T& r) const { return usec_ <= r.usec_; } + bool operator>=(const T& r) const { return usec_ >= r.usec_; } + + T operator+(const D& r) const + { + if (r.isNegative()) + { + if (uint64_t(r.getAbs().usec_) > usec_) + return fromUSec(0); + } + else + { + if (uint64_t(usec_ + r.usec_) < usec_) + return fromUSec(std::numeric_limits::max()); + } + return fromUSec(usec_ + r.usec_); + } + + T operator-(const D& r) const + { + return *static_cast(this) + (-r); + } + D operator-(const T& r) const + { + return D::fromUSec((usec_ > r.usec_) ? (usec_ - r.usec_) : -(r.usec_ - usec_)); + } + + T& operator+=(const D& r) + { + *this = *this + r; + return *static_cast(this); + } + T& operator-=(const D& r) + { + *this = *this - r; + return *static_cast(this); + } + + std::string toString() const; +}; + + +class MonotonicDuration : public DurationBase { }; + +class MonotonicTime : public TimeBase { }; + + +class UtcDuration : public DurationBase { }; + +class UtcTime : public TimeBase +{ +public: + UtcTime() { } + + UtcTime(const Timestamp& ts) // Implicit + { + operator=(ts); + } + + UtcTime& operator=(const Timestamp& ts) + { + *this = UtcTime::fromUSec(ts.husec * Timestamp::USEC_PER_LSB); + return *this; + } + + operator Timestamp() const + { + Timestamp ts; + ts.husec = toUSec() / Timestamp::USEC_PER_LSB; + return ts; + } +}; + + +template +inline Stream& operator<<(Stream& s, DurationBase d) +{ + char buf[8]; + std::snprintf(buf, sizeof(buf), "%06lu", static_cast(std::abs(d.toUSec() % 1000000L))); + if (d.isNegative()) + s << '-'; + s << std::abs(d.toUSec() / 1000000L) << '.' << buf; + return s; +} + +template +inline Stream& operator<<(Stream& s, TimeBase t) +{ + char buf[8]; + std::snprintf(buf, sizeof(buf), "%06lu", static_cast(t.toUSec() % 1000000L)); + s << (t.toUSec() / 1000000L) << '.' << buf; + return s; +} + + +template +inline std::string DurationBase::toString() const +{ + std::ostringstream os; + os << *static_cast(this); + return os.str(); +} + +template +inline std::string TimeBase::toString() const +{ + std::ostringstream os; + os << *static_cast(this); + return os.str(); +} + +} diff --git a/libuavcan/test/time.cpp b/libuavcan/test/time.cpp new file mode 100644 index 0000000000..831c9fbbf9 --- /dev/null +++ b/libuavcan/test/time.cpp @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + + +TEST(Time, Monotonic) +{ + using uavcan::MonotonicDuration; + using uavcan::MonotonicTime; + + MonotonicTime m1; + MonotonicTime m2 = MonotonicTime::fromMSec(1); + MonotonicDuration md1 = m2 - m1; // 1000 + MonotonicDuration md2 = m1 - m2; // -1000 + + ASSERT_EQ(0, m1.toUSec()); + ASSERT_EQ(1000, m2.toUSec()); + ASSERT_EQ(1000, md1.toUSec()); + ASSERT_EQ(-1000, md2.toUSec()); + + ASSERT_LT(m1, m2); + ASSERT_LE(m1, m2); + ASSERT_NE(m1, m2); + ASSERT_TRUE(m1.isZero()); + ASSERT_FALSE(m2.isZero()); + + ASSERT_GT(md1, md2); + ASSERT_GE(md1, md2); + ASSERT_NE(md1, md2); + ASSERT_FALSE(md1.isZero()); + ASSERT_TRUE(md1.isPositive()); + ASSERT_TRUE(md2.isNegative()); + + ASSERT_EQ(0, (md1 + md2).toUSec()); + ASSERT_EQ(2000, (md1 - md2).toUSec()); + + md1 *= 2; // 2000 + ASSERT_EQ(2000, md1.toUSec()); + + md2 += md1; // md2 = -1000 + 2000 + ASSERT_EQ(1000, md2.toUSec()); + + ASSERT_EQ(-1000, (-md2).toUSec()); + + /* + * To string + */ + ASSERT_EQ("0.000000", m1.toString()); + ASSERT_EQ("0.001000", m2.toString()); + + ASSERT_EQ("0.002000", md1.toString()); + ASSERT_EQ("-0.001000", (-md2).toString()); + + ASSERT_EQ("1001.000001", MonotonicTime::fromUSec(1001000001).toString()); + ASSERT_EQ("-1001.000001", MonotonicDuration::fromUSec(-1001000001).toString()); +} + + +TEST(Time, Utc) +{ + using uavcan::UtcDuration; + using uavcan::UtcTime; + using uavcan::Timestamp; + + Timestamp ts; + ts.husec = 90; + + UtcTime u1(ts); + ASSERT_EQ(9000, u1.toUSec()); + + ts.husec *= 2; + u1 = ts; + ASSERT_EQ(18000, u1.toUSec()); + + ts = UtcTime::fromUSec(12345678900); + ASSERT_EQ(123456789, ts.husec); + + /* + * To string + */ + ASSERT_EQ("0.018000", u1.toString()); + ASSERT_EQ("12345.678900", UtcTime(ts).toString()); +}