[openrtm-commit:02796] r3036 - in trunk/OpenRTM-aist/src/lib/coil: common vxworks/coil

[openrtm ＠ openrtm.org]

Author: miyamoto
Date: 2017-09-01 20:08:59 +0900 (Fri, 01 Sep 2017)
New Revision: 3036

Modified:
   trunk/OpenRTM-aist/src/lib/coil/common/ClockManager.cpp
   trunk/OpenRTM-aist/src/lib/coil/vxworks/coil/Time.cpp
   trunk/OpenRTM-aist/src/lib/coil/vxworks/coil/Time.h
Log:
[incompat, VxWorks] refs #4174

Modified: trunk/OpenRTM-aist/src/lib/coil/common/ClockManager.cpp
===================================================================
--- trunk/OpenRTM-aist/src/lib/coil/common/ClockManager.cpp	2017-09-01 11:01:17 UTC (rev 3035)
+++ trunk/OpenRTM-aist/src/lib/coil/common/ClockManager.cpp	2017-09-01 11:08:59 UTC (rev 3036)
@@ -34,7 +34,13 @@
   }
   bool SystemClock::settime(coil::TimeValue clocktime)
   {
-#if defined(VXWORKS_66) && !defined(__RTP__)
+#if defined(__powerpc__) && !defined(__RTP__)
+    if (coil::settimeofday(clocktime.sec(), clocktime.usec()*1000) == 0)
+      {
+        return true;
+      }
+    return false;
+#elif defined(VXWORKS_66) && !defined(__RTP__)
     timespec tv;
     tv.tv_sec = clocktime.sec();
     tv.tv_nsec = clocktime.usec()*1000;

Modified: trunk/OpenRTM-aist/src/lib/coil/vxworks/coil/Time.cpp
===================================================================
--- trunk/OpenRTM-aist/src/lib/coil/vxworks/coil/Time.cpp	2017-09-01 11:01:17 UTC (rev 3035)
+++ trunk/OpenRTM-aist/src/lib/coil/vxworks/coil/Time.cpp	2017-09-01 11:08:59 UTC (rev 3036)
@@ -18,7 +18,41 @@
 
 #include <coil/Time.h>
 
+#if defined(__powerpc__) && !defined(__RTP__)
+#define CLOCK_RATE_PPC 75000000
+//extern void vxTimeBaseSet(UINT32, UINT32);
+//extern void vxTimeBaseGet(UINT32*, UINT32*);
+#include <vxLib.h>
+#endif
+
 namespace coil
 {
+#if defined(__powerpc__) && !defined(__RTP__)
+  int gettimeofday(UINT32 &s, UINT32 &ns)
+  {
+    UINT32 tbu, tbl;
+    vxTimeBaseGet(&tbu, &tbl);
+    UINT64 t = ((UINT64)tbu << 32) | (UINT64)tbl;
+    s = t/CLOCK_RATE_PPC;
+    ns = ((t-s*CLOCK_RATE_PPC)*1000000000)/CLOCK_RATE_PPC;
+    return 0;
+  }
+  TimeValue gettimeofday()
+  {
+    UINT32 tbu, tbl;
+    vxTimeBaseGet(&tbu, &tbl);
+    UINT64 t = ((UINT64)tbu << 32) | (UINT64)tbl;
+    UINT32 s = t/CLOCK_RATE_PPC;
+    UINT32 us = ((t-s*CLOCK_RATE_PPC)*1000000)/CLOCK_RATE_PPC;
+    return TimeValue(s, us);
+  }
+  int settimeofday(UINT32 s, UINT32 ns)
+  {
+    UINT64 t = s*CLOCK_RATE_PPC + (ns*CLOCK_RATE_PPC)/1000000000;
+    UINT32 tbu = t >> 32;
+    UINT32 tbl = (UINT32)t;
+    vxTimeBaseSet(tbu,tbl);
+  }
+#endif
   // no implementation
 };

Modified: trunk/OpenRTM-aist/src/lib/coil/vxworks/coil/Time.h
===================================================================
--- trunk/OpenRTM-aist/src/lib/coil/vxworks/coil/Time.h	2017-09-01 11:01:17 UTC (rev 3035)
+++ trunk/OpenRTM-aist/src/lib/coil/vxworks/coil/Time.h	2017-09-01 11:08:59 UTC (rev 3036)
@@ -33,6 +33,8 @@
 
 #include <taskLib.h>
 #include <sysLib.h>
+
+
 /*
 #ifdef __RTP__
 #include <taskLib.h>
@@ -69,7 +71,7 @@
   inline unsigned int sleep(unsigned int seconds)
   {
     int tps = sysClkRateGet();
-    if(taskDelay(seconds*tps) == OK)
+    if(taskDelay(seconds*tps+1) == OK)
     {
         return 0;
     }
@@ -119,7 +121,7 @@
   {
     int tps = sysClkRateGet();
 
-    if(taskDelay(interval.sec()*tps + (interval.usec()*tps)/1000000l) == OK)
+    if(taskDelay(interval.sec()*tps + (interval.usec()*tps)/1000000l + 1) == OK)
     {
         return 0;
     }
@@ -171,7 +173,7 @@
   inline int usleep(unsigned int usec)
   {
     int tps = sysClkRateGet();
-    if(taskDelay(usec*tps/1000000l) == OK)
+    if(taskDelay(usec*tps/1000000l + 1) == OK)
     {
         return 0;
     }
@@ -220,7 +222,9 @@
    *
    * @endif
    */
-#if defined(VXWORKS_66) && !defined(__RTP__)
+#if defined(__powerpc__) && !defined(__RTP__)
+  int gettimeofday(UINT32 &s, UINT32 &ns);
+#elif defined(VXWORKS_66) && !defined(__RTP__)
   inline int gettimeofday(struct timespec *tv)
   {
     return ::clock_gettime(CLOCK_REALTIME, tv);
@@ -249,19 +253,23 @@
    *
    * @endif
    */
+#if defined(__powerpc__) && !defined(__RTP__)
+  TimeValue gettimeofday();
+#elif defined(VXWORKS_66) && !defined(__RTP__)
   inline TimeValue gettimeofday()
   {
-#if defined(VXWORKS_66) && !defined(__RTP__)
     timespec tv;
     ::clock_gettime(CLOCK_REALTIME, &tv);
     return TimeValue(tv.tv_sec, tv.tv_nsec/1000);
+  }
 #else
+  inline TimeValue gettimeofday()
+  {
     timeval tv;
     ::gettimeofday(&tv, 0);
     return TimeValue(tv.tv_sec, tv.tv_usec);
+  }
 #endif
-    
-  }
 
 
 
@@ -288,7 +296,9 @@
    *
    * @endif
    */
-#if defined(VXWORKS_66) && !defined(__RTP__)
+#if defined(__powerpc__) && !defined(__RTP__)
+  int settimeofday(UINT32 s, UINT32 ns);
+#elif defined(VXWORKS_66) && !defined(__RTP__)
   inline int settimeofday(struct timespec *tv)
   {
     return ::clock_settime(CLOCK_REALTIME, tv);