Clean the code, add a trace debug define

latency-measure/src/main.c

@@ -13,6 +13,12 @@
 #define CLOCK_ID CLOCK_MONOTONIC
 #define NSECS_PER_SECOND INT64_C(1000000000)
 
+#ifdef DEBUG_TRACE
+struct timespec * debug_current_times;
+struct timespec * debug_next_times;
+int64_t * debug_diffs;
+#endif
+
 typedef struct thread_stat {
   int nb_cycles;
   int64_t max_diff;
@@ -62,55 +68,18 @@ static inline struct timespec add_ns(struct timespec t, int64_t ns) {
   return ret;
 }
 
-// Real-time thread
-// Sends packets at a regular intervall
-static void *timerthread_poll(void *p) {
-
-  struct timespec next;
-  struct timespec current;
-  struct sched_param priority;
-
-  thread_param_t * param = (thread_param_t *)p;
-  thread_stat_t * stat = &param->stat;
-
-  priority.sched_priority = param->priority;
-
-  int err = sched_setscheduler(0, SCHED_FIFO, &priority);
-
-  if (err)
-    error(EXIT_FAILURE, errno, "Couldn't set priority");
-
-  stat->max_diff = 0;
-  stat->min_diff = 1000000;
-
-  clock_gettime(CLOCK_ID, &next);
-  next = add_ns(next, param->interval);
-
-  for(stat->nb_cycles = 0;; stat->nb_cycles++) {
-
-    if(param->max_cycles)
-      if(stat->nb_cycles >= param->max_cycles)
-        break;
-
+static void sleep_poll(struct timespec * next, struct timespec * current) {
     for(;;) {
-      clock_gettime(CLOCK_ID, &current);
-      if( current.tv_sec >= next.tv_sec || (current.tv_sec == next.tv_sec && current.tv_nsec >= next.tv_nsec) )
+      clock_gettime(CLOCK_ID, current);
+      if( current->tv_sec > next->tv_sec || (current->tv_sec == next->tv_sec && current->tv_nsec >= next->tv_nsec) )
         break;
     }
-
-    int64_t diff = diff_ns(current, next);
-
-    stat->max_diff = max(stat->max_diff, diff);
-    stat->min_diff = min(stat->min_diff, diff);
-    
-    next = add_ns(current, param->interval);
-  }
-
-  printf("Done\n");
-
-  return NULL;
 }
 
+static void sleep_nanosleep(struct timespec * next, struct timespec * current) {
+    clock_nanosleep(CLOCK_ID, TIMER_ABSTIME, next, NULL);
+    clock_gettime(CLOCK_ID, current);
+}
 
 // Real-time thread
 // Sends packets at a regular intervall
@@ -142,15 +111,27 @@ static void *timerthread(void *p) {
       if(stat->nb_cycles >= param->max_cycles)
         break;
 
-    clock_nanosleep(CLOCK_ID, TIMER_ABSTIME, &next, NULL);
-    clock_gettime(CLOCK_ID, &current);
+    #ifdef DEBUG_TRACE
+    debug_next_times[stat->nb_cycles] = next;
+    #endif
+
+    sleep_poll(&next, &current);
+
+    #ifdef DEBUG_TRACE
+    debug_current_times[stat->nb_cycles] = current;
+    #endif
 
     int64_t diff = diff_ns(current, next);
 
+    #ifdef DEBUG_TRACE
+    debug_diffs[stat->nb_cycles] = diff;
+    #endif
+
     stat->max_diff = max(stat->max_diff, diff);
     stat->min_diff = min(stat->min_diff, diff);
     
     next = add_ns(current, param->interval);
+
   }
 
   printf("Done\n");
@@ -194,6 +175,7 @@ int main(int argc, char *argv[]) {
   pthread_t thread;
   thread_param_t param;
   main_param_t main_param;
+  int err;
 
   // Default values
   param.interval = 1000000;
@@ -204,27 +186,40 @@ int main(int argc, char *argv[]) {
 
   process_options(argc, argv, &param, &main_param);
 
-  sleep(1);
+  usleep(10000);
+
+  #ifdef DEBUG_TRACE
+  debug_current_times = malloc(sizeof(struct timespec) * param.max_cycles);
+  debug_next_times = malloc(sizeof(struct timespec) * param.max_cycles);
+  debug_diffs = malloc(sizeof(int64_t) * param.max_cycles);
 
-  pthread_create(&thread, NULL, timerthread_poll, (void *)&param);
+  FILE * debug_log = fopen("latency-measure-log", "w+");
+  #endif
 
-  sleep(10);
+  err = pthread_create(&thread, NULL, timerthread, (void *)&param);
 
-  printf("Maximum latency: %" PRIi64 "us (%d)\n", (param.stat.max_diff / 1000), param.stat.nb_cycles);
-  printf("Minimum latency: %" PRIi64 "us (%d)\n", (param.stat.min_diff / 1000), param.stat.nb_cycles);
+  if(err)
+    error(EXIT_FAILURE, errno, "Couldn't create thread");
 
-  //if(err)
-  //  error(EXIT_FAILURE, errno, "Couldn't create thread");
+  for (;;) {
 
-  //for (;;) {
+    usleep(main_param.refresh_rate);
 
-  //  usleep(main_param.refresh_rate);
+    printf("Maximum latency: %" PRIi64 "ns (%d)", (param.stat.max_diff), param.stat.nb_cycles);
+    printf(", minimum latency: %" PRIi64 "ns (%d)\n", (param.stat.min_diff), param.stat.nb_cycles);
 
-  //  printf("Maximum latency: %" PRIi64 "us (%d)\n", (param.stat.max_diff / 1000), param.stat.nb_cycles);
+    if( param.max_cycles == param.stat.nb_cycles )
+      break;
+  }
 
-  //  if( param.max_cycles == param.stat.nb_cycles )
-  //    break;
-  //}
+  #ifdef DEBUG_TRACE
+  for(int i = 0; i < param.max_cycles; i++) {
+    fprintf(debug_log, "%ld      %" PRIi64 "  \n\n", debug_next_times[i].tv_sec, debug_next_times[i].tv_nsec);
+    fprintf(debug_log, "%ld      %" PRIi64 "  \n", debug_current_times[i].tv_sec, debug_current_times[i].tv_nsec);
+    fprintf(debug_log, "                                            %" PRIi64 "  \n", debug_diffs[i]);
+  }
+  fclose(debug_log);
+  #endif
 
   exit(EXIT_SUCCESS);
 }