Merge pull request #1128 from paparazzi/linux_sys_time_clock_monotonic

[arch/linux] sys_time: get time from CLOCK_MONOTONIC

Instead of simply adding up the sys_time ticks, seconds, get current time from CLOCK_MONOTONIC and directly set sys_time from that (difference to clock monotonic time at startup).

conf/firmwares/rotorcraft.makefile

@@ -69,6 +69,10 @@ endif
 # Systime
 #
 $(TARGET).srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c
+ifeq ($(ARCH), linux)
+# seems that we need to link against librt for glibc < 2.17
+$(TARGET).LDFLAGS += -lrt
+endif
 
 
 #

conf/firmwares/setup.makefile

@@ -30,6 +30,10 @@ ifneq ($(SYS_TIME_LED),none)
 endif
 COMMON_SETUP_CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)
 COMMON_SETUP_SRCS   += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c
+ifeq ($(ARCH), linux)
+# seems that we need to link against librt for glibc < 2.17
+$(TARGET).LDFLAGS += -lrt
+endif
 
 COMMON_SETUP_CFLAGS += -DUSE_LED
 

conf/firmwares/subsystems/fixedwing/autopilot.makefile

@@ -71,6 +71,10 @@ endif
 # Sys-time
 #
 $(TARGET).srcs   += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c
+ifeq ($(ARCH), linux)
+# seems that we need to link against librt for glibc < 2.17
+$(TARGET).LDFLAGS += -lrt
+endif
 
 #
 # InterMCU & Commands

conf/firmwares/test_progs.makefile

@@ -54,6 +54,10 @@ ifneq ($(SYS_TIME_LED),none)
 endif
 COMMON_TEST_CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)
 COMMON_TEST_SRCS   += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c
+ifeq ($(ARCH), linux)
+# seems that we need to link agains librt for glibc < 2.17
+$(TARGET).LDFLAGS += -lrt
+endif
 
 COMMON_TEST_CFLAGS += -DUSE_LED
 

sw/airborne/arch/linux/mcu_periph/sys_time_arch.c

@@ -28,6 +28,7 @@
 #include <stdio.h>
 #include <pthread.h>
 #include <sys/timerfd.h>
+#include <time.h>
 #include "rt_priority.h"
 
 #ifdef SYS_TIME_LED
@@ -39,6 +40,8 @@
 #endif
 
 pthread_t sys_time_thread;
+static struct timespec startup_time;
+
 static void sys_tick_handler(void);
 void *sys_time_thread_main(void *data);
 
@@ -82,11 +85,8 @@ void *sys_time_thread_main(void *data)
     if (missed > 1) {
       fprintf(stderr, "Missed %lld timer events!\n", missed);
     }
-    /* advance sys_time, in case we missed some events: call it more than once */
-    unsigned int i;
-    for (i = 0; i < missed; i++) {
-      sys_tick_handler();
-    }
+    /* set current sys_time */
+    sys_tick_handler();
   }
   return NULL;
 }
@@ -96,6 +96,8 @@ void sys_time_arch_init(void)
   sys_time.cpu_ticks_per_sec = 1e6;
   sys_time.resolution_cpu_ticks = (uint32_t)(sys_time.resolution * sys_time.cpu_ticks_per_sec + 0.5);
 
+  clock_gettime(CLOCK_MONOTONIC, &startup_time);
+
   int ret = pthread_create(&sys_time_thread, NULL, sys_time_thread_main, NULL);
   if (ret) {
     perror("Could not setup sys_time_thread");
@@ -105,21 +107,37 @@ void sys_time_arch_init(void)
 
 static void sys_tick_handler(void)
 {
-  sys_time.nb_tick++;
-  sys_time.nb_sec_rem += sys_time.resolution_cpu_ticks;;
-  if (sys_time.nb_sec_rem >= sys_time.cpu_ticks_per_sec) {
-    sys_time.nb_sec_rem -= sys_time.cpu_ticks_per_sec;
-    sys_time.nb_sec++;
+  /* get current time */
+  struct timespec now;
+  clock_gettime(CLOCK_MONOTONIC, &now);
+
+  /* time difference to startup */
+  time_t d_sec = now.tv_sec - startup_time.tv_sec;
+  long d_nsec = now.tv_nsec - startup_time.tv_nsec;
+
+  /* wrap if negative nanoseconds */
+  if (d_nsec < 0) {
+    d_sec -= 1;
+    d_nsec += 1000000000L;
+  }
+
 #ifdef SYS_TIME_LED
+  if (d_sec > sys_time.nb_sec) {
     LED_TOGGLE(SYS_TIME_LED);
+  }
 #endif
 
-  }
+  sys_time.nb_sec = d_sec;
+  sys_time.nb_sec_rem = cpu_ticks_of_nsec(d_nsec);
+  sys_time.nb_tick = sys_time_ticks_of_sec(d_sec) + sys_time_ticks_of_usec(d_nsec / 1000);
+
+  /* advance virtual timers */
   for (unsigned int i = 0; i < SYS_TIME_NB_TIMER; i++) {
     if (sys_time.timer[i].in_use &&
         sys_time.nb_tick >= sys_time.timer[i].end_time) {
       sys_time.timer[i].end_time += sys_time.timer[i].duration;
       sys_time.timer[i].elapsed = TRUE;
+      /* call registered callbacks, WARNING: they will be executed in the sys_time thread! */
       if (sys_time.timer[i].cb) {
         sys_time.timer[i].cb(i);
       }