clock.hpp

00001 /*
00002  * Copyright (C) 2014 <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #pragma once
00006 
00007 #include <cassert>
00008 #include <time.h>
00009 #include <sys/time.h>
00010 #include <uavcan/driver/system_clock.hpp>
00011 
00012 class SystemClockMock : public uavcan::ISystemClock
00013 {
00014 public:
00015     mutable uint64_t monotonic;
00016     mutable uint64_t utc;
00017     uavcan::UtcDuration last_adjustment;
00018     uint64_t monotonic_auto_advance;
00019     bool preserve_utc;
00020 
00021     SystemClockMock(uint64_t initial = 0)
00022         : monotonic(initial)
00023         , utc(initial)
00024         , monotonic_auto_advance(0)
00025         , preserve_utc(false)
00026     { }
00027 
00028     void advance(uint64_t usec) const
00029     {
00030         monotonic += usec;
00031         if (!preserve_utc)
00032         {
00033             utc += usec;
00034         }
00035     }
00036 
00037     virtual uavcan::MonotonicTime getMonotonic() const
00038     {
00039         assert(this);
00040         const uint64_t res = monotonic;
00041         advance(monotonic_auto_advance);
00042         return uavcan::MonotonicTime::fromUSec(res);
00043     }
00044 
00045     virtual uavcan::UtcTime getUtc() const
00046     {
00047         assert(this);
00048         return uavcan::UtcTime::fromUSec(utc);
00049     }
00050 
00051     virtual void adjustUtc(uavcan::UtcDuration adjustment)
00052     {
00053         assert(this);
00054         const uint64_t prev_utc = utc;
00055         utc = uint64_t(int64_t(utc) + adjustment.toUSec());
00056         last_adjustment = adjustment;
00057         std::cout << "Clock adjustment " << prev_utc << " --> " << utc << std::endl;
00058     }
00059 };
00060 
00061 
00062 class SystemClockDriver : public uavcan::ISystemClock
00063 {
00064 public:
00065     uavcan::UtcDuration utc_adjustment;
00066 
00067     virtual uavcan::MonotonicTime getMonotonic() const
00068     {
00069         struct timespec ts;
00070         const int ret = clock_gettime(CLOCK_MONOTONIC, &ts);
00071         if (ret != 0)
00072         {
00073             assert(0);
00074             return uavcan::MonotonicTime();
00075         }
00076         return uavcan::MonotonicTime::fromUSec(uint64_t(int64_t(ts.tv_sec) * 1000000L + int64_t(ts.tv_nsec / 1000L)));
00077     }
00078 
00079     virtual uavcan::UtcTime getUtc() const
00080     {
00081         struct timeval tv;
00082         const int ret = gettimeofday(&tv, UAVCAN_NULLPTR);
00083         if (ret != 0)
00084         {
00085             assert(0);
00086             return uavcan::UtcTime();
00087         }
00088         return uavcan::UtcTime::fromUSec(uint64_t(int64_t(tv.tv_sec) * 1000000L + tv.tv_usec)) + utc_adjustment;
00089     }
00090 
00091     virtual void adjustUtc(uavcan::UtcDuration adjustment)
00092     {
00093         utc_adjustment += adjustment;
00094     }
00095 };
00096 
00097 inline uavcan::MonotonicTime tsMono(uint64_t usec) { return uavcan::MonotonicTime::fromUSec(usec); }
00098 inline uavcan::UtcTime tsUtc(uint64_t usec) { return uavcan::UtcTime::fromUSec(usec); }
00099 
00100 inline uavcan::MonotonicDuration durMono(int64_t usec) { return uavcan::MonotonicDuration::fromUSec(usec); }
00101 
00102 template <typename T>
00103 static bool areTimestampsClose(const T& a, const T& b, int64_t precision_usec = 100000)
00104 {
00105     return (a - b).getAbs().toUSec() < precision_usec;
00106 }