Add sleep implementations to SGTimeStamp.

Implement sleeping to an absolute timestamp
as well as sleeping for a relative time in
SGtimeStamp.
The reason for including this into SGTimeStamp
is that the timestamps internals are required to
do accurate sleeps to and absolute timestamp
for the posix timer implementation.

diff --git a/simgear/timing/timestamp.cxx b/simgear/timing/timestamp.cxx
index 83eff2fc10ea485c8039634132b958f1ac866cef..a2eae9eef6b2b02af58cee940cd71e072526bc12 100644 (file)
--- a/simgear/timing/timestamp.cxx
+++ b/simgear/timing/timestamp.cxx
@@ -58,6 +58,29 @@
 
 #include "timestamp.hxx"
 
+#if defined(_POSIX_TIMERS) && (0 < _POSIX_TIMERS)
+static clockid_t getClockId()
+{
+#if defined(_POSIX_MONOTONIC_CLOCK)
+    static clockid_t clockid = CLOCK_MONOTONIC;
+    static bool firstTime = true;
+    if (!firstTime)
+        return clockid;
+
+    firstTime = false;
+    // For the first time test if the monotonic clock is available.
+    // If so use this, if not use the realtime clock.
+    struct timespec ts;
+    if (-1 == clock_gettime(clockid, &ts) && errno == EINVAL)
+        clockid = CLOCK_REALTIME;
+    return clockid;
+#else
+    return CLOCK_REALTIME;
+#endif
+}
+
+#endif
+
 void SGTimeStamp::stamp() {
 #ifdef _WIN32
     unsigned int t;
@@ -66,27 +89,12 @@ void SGTimeStamp::stamp() {
     _nsec = ( t - ( _sec * 1000 ) ) * 1000 * 1000;
 #elif defined(_POSIX_TIMERS) && (0 < _POSIX_TIMERS)
     struct timespec ts;
-#if defined(_POSIX_MONOTONIC_CLOCK)
-    static clockid_t clockid = CLOCK_MONOTONIC;
-    static bool firstTime = true;
-    if (firstTime) {
-        firstTime = false;
-        // For the first time test if the monotonic clock is available.
-        // If so use this if not use the realtime clock.
-        if (-1 == clock_gettime(clockid, &ts) && errno == EINVAL)
-            clockid = CLOCK_REALTIME;
-    }
-    clock_gettime(clockid, &ts);
-#else
-    clock_gettime(CLOCK_REALTIME, &ts);
-#endif
+    clock_gettime(getClockId(), &ts);
     _sec = ts.tv_sec;
     _nsec = ts.tv_nsec;
 #elif defined( HAVE_GETTIMEOFDAY )
     struct timeval current;
-    struct timezone tz;
-    // sg_timestamp currtime;
-    gettimeofday(&current, &tz);
+    gettimeofday(&current, NULL);
     _sec = current.tv_sec;
     _nsec = current.tv_usec * 1000;
 #elif defined( HAVE_GETLOCALTIME )
@@ -104,6 +112,181 @@ void SGTimeStamp::stamp() {
 #endif
 }
 
+// sleep based timing loop.
+//
+// Calling sleep, even usleep() on linux is less accurate than
+// we like, but it does free up the cpu for other tasks during
+// the sleep so it is desirable.  Because of the way sleep()
+// is implemented in consumer operating systems like windows
+// and linux, you almost always sleep a little longer than the
+// requested amount.
+//
+// To combat the problem of sleeping too long, we calculate the
+// desired wait time and shorten it by 2000us (2ms) to avoid
+// [hopefully] over-sleep'ing.  The 2ms value was arrived at
+// via experimentation.  We follow this up at the end with a
+// simple busy-wait loop to get the final pause timing exactly
+// right.
+//
+// Assuming we don't oversleep by more than 2000us, this
+// should be a reasonable compromise between sleep based
+// waiting, and busy waiting.
+//
+// Usually posix timer resolutions are low enough that we
+// could just leave this to the operating system today.
+// The day where the busy loop was introduced in flightgear,
+// the usual kernels still had just about 10ms (=HZ for
+// the timer tick) accuracy which is too bad to catch 60Hz...
+bool SGTimeStamp::sleepUntil(const SGTimeStamp& abstime)
+{
+#if defined(_POSIX_TIMERS) && (0 < _POSIX_TIMERS)
+    SGTimeStamp abstimeForSleep = abstime;
+
+    // Always undersleep by resolution of the clock
+    struct timespec ts;
+    if (-1 != clock_getres(getClockId(), &ts)) {
+        abstimeForSleep -= SGTimeStamp::fromSecNSec(ts.tv_sec, ts.tv_nsec);
+    } else {
+        abstimeForSleep -= SGTimeStamp::fromSecMSec(0, 2);
+    }
+
+    ts.tv_sec = abstimeForSleep._sec;
+    ts.tv_nsec = abstimeForSleep._nsec;
+    for (;;) {
+        int ret = clock_nanosleep(getClockId(), TIMER_ABSTIME, &ts, NULL);
+        if (-1 == ret && errno != EINTR)
+            return false;
+        if (ret == 0)
+            break;
+    }
+
+    // The busy loop for the rest
+    SGTimeStamp currentTime;
+    do {
+        currentTime.stamp();
+    } while (currentTime < abstime);
+
+    return true;
+
+#elif defined _WIN32
+
+    SGTimeStamp currentTime;
+    currentTime.stamp();
+    if (abstime <= currentTime)
+        return true;
+
+    SGTimeStamp abstimeForSleep = abstime - SGTimeStamp::fromSecMSec(0, 2);
+    for (;abstimeForSleep < currentTime;) {
+        SGTimeStamp timeDiff = abstimeForSleep - currentTime;
+        if (timeDiff < SGTimeStamp::fromSecMSec(0, 1))
+            break;
+        // Don't know, but may be win32 has something better today??
+        Sleep(static_cast<DWORD>(timeDiff.toMSecs()));
+
+        currentTime.stamp();
+    }
+
+    // Follow by a busy loop
+    while (currentTime < abstime) {
+        currentTime.stamp();
+    }
+
+    return true;
+
+#else
+
+    SGTimeStamp currentTime;
+    currentTime.stamp();
+    if (abstime <= currentTime)
+        return true;
+
+    SGTimeStamp abstimeForSleep = abstime - SGTimeStamp::fromSecMSec(0, 2);
+    for (;abstimeForSleep < currentTime;) {
+        SGTimeStamp timeDiff = abstimeForSleep - currentTime;
+        if (timeDiff < SGTimeStamp::fromSecUSec(0, 1))
+            break;
+        // Its documented that some systems bail out on usleep for longer than 1s
+        // since we recheck the current time anyway just wait for
+        // less than a second multiple times
+        bool truncated = false;
+        if (SGTimeStamp::fromSec(1) < timeDiff) {
+            timeDiff = SGTimeStamp::fromSec(1);
+            truncated = true;
+        }
+        int ret = usleep(static_cast<int>(timeDiff.toUSecs()));
+        if (-1 == ret && errno != EINTR)
+            return false;
+        if (ret == 0 && !truncated)
+            break;
+
+        currentTime.stamp();
+    }
+
+    // Follow by a busy loop
+    while (currentTime < abstime) {
+        currentTime.stamp();
+    }
+
+    return true;
+
+#endif
+}
+
+bool SGTimeStamp::sleepFor(const SGTimeStamp& reltime)
+{
+#if defined(_POSIX_TIMERS) && (0 < _POSIX_TIMERS)
+    struct timespec ts;
+    ts.tv_sec = reltime._sec;
+    ts.tv_nsec = reltime._nsec;
+    for (;;) {
+        struct timespec rem;
+        int ret = clock_nanosleep(getClockId(), 0, &ts, &rem);
+        if (-1 == ret && errno != EINTR)
+            return false;
+        if (ret == 0)
+            break;
+        // Use the remainder for the next cycle.
+        ts = rem;
+    }
+    return true;
+#elif defined _WIN32
+    if (reltime < SGTimeStamp::fromSecMSec(0, 1))
+        return true;
+    // Don't know, but may be win32 has something better today??
+    Sleep(static_cast<DWORD>(reltime.toMSecs()));
+    return true;
+#else
+    SGTimeStamp abstime;
+    abstime.stamp();
+    abstime += reltime;
+
+    SGTimeStamp currentTime;
+    currentTime.stamp();
+    for (;abstime < currentTime;) {
+        SGTimeStamp timeDiff = abstime - currentTime;
+        if (timeDiff < SGTimeStamp::fromSecUSec(0, 1))
+            break;
+        // Its documented that some systems bail out on usleep for longer than 1s
+        // since we recheck the current time anyway just wait for
+        // less than a second multiple times
+        bool truncated = false;
+        if (SGTimeStamp::fromSec(1) < timeDiff) {
+            timeDiff = SGTimeStamp::fromSec(1);
+            truncated = true;
+        }
+        int ret = usleep(static_cast<int>(timeDiff.toUSecs()));
+        if (-1 == ret && errno != EINTR)
+            return false;
+        if (ret == 0 && !truncated)
+            break;
+
+        currentTime.stamp();
+    }
+
+    return true;
+#endif
+}
+
 int SGTimeStamp::elapsedMSec() const
 {
     SGTimeStamp now;

diff --git a/simgear/timing/timestamp.hxx b/simgear/timing/timestamp.hxx
index f195dc3740fbdedd4593b2d6c9ff6f574a648494..ac9ca1423fe81b56041b4a49ce2e146b5d6cb662 100644 (file)
--- a/simgear/timing/timestamp.hxx
+++ b/simgear/timing/timestamp.hxx
@@ -184,6 +184,8 @@ public:
     { SGTimeStamp ts; ts.setTime(sec); return ts; }
     static SGTimeStamp fromSec(const double& sec)
     { SGTimeStamp ts; ts.setTime(sec); return ts; }
+    static SGTimeStamp fromMSec(nsec_type msec)
+    { return SGTimeStamp(0, 1000*1000*msec); }
     static SGTimeStamp fromUSec(nsec_type usec)
     { return SGTimeStamp(0, 1000*usec); }
     static SGTimeStamp fromNSec(nsec_type nsec)
@@ -195,6 +197,22 @@ public:
     static SGTimeStamp now()
     { SGTimeStamp ts; ts.stamp(); return ts; }
 
+    /**
+     *  Sleep until the time of abstime is passed.
+     */
+    static bool sleepUntil(const SGTimeStamp& abstime);
+
+    /**
+     *  Sleep for reltime.
+     */
+    static bool sleepFor(const SGTimeStamp& reltime);
+
+    /**
+     *  Alias for the most common use case with milliseconds.
+     */
+    static bool sleepForMSec(unsigned msec)
+    { return sleepFor(fromMSec(msec)); }
+
     /**
      * elapsed time since the stamp was taken, in msec
      */