Update software-pulse

packet-exchange/src/server.c

@@ -186,6 +186,9 @@ static void *tsn_thread(void *p) {
   cpu_set_t mask;
   char tracemark_message[128];
   
+  uint64_t time_elapsed;
+  uint64_t next_interval;
+
   // Set thread CPU affinity
   if (thread_params.affinity_cpu) {
     CPU_ZERO(&mask);

software-pulse/src/common.h

@@ -19,7 +19,10 @@
 #include <linux/udp.h>
 #endif
 
+#define MOTOR_STEPS 200
+
 #define NSEC_PER_SEC UINT64_C(1000000000)
+#define USEC_PER_SEC UINT64_C(1000000)
 #define SERVER_PORT "50000"
 #define SERVER_PORT_INT 50000
 

software-pulse/src/software-pulse.c

@@ -40,6 +40,11 @@ typedef struct thread_param {
 typedef struct main_param {
   int refresh_rate;
   int verbose;
+
+  int interval_input;
+  int transition_time;
+  uint64_t target_interval;
+
 } main_param_t;
 
 // Static functions
@@ -88,6 +93,23 @@ static void poll_wakeup(struct timespec ts, int margin) {
   } while(calcdiff_ns_signed(ts, current) > 1000);
 }
 
+static void *print_thread(void *p) {
+  (void)p;
+
+  for (;;) {
+    if (thread_params.max_cycles &&
+        nb_cycles >= ((unsigned int)thread_params.max_cycles))
+      break;
+
+    printf("Interval: %10" PRIu64 " Target: %10" PRIu64 "\n", thread_params.interval / 1000, main_params.target_interval);
+    printf("\033[%dA", 1);
+
+    usleep(100000);
+  }
+
+  pthread_exit(NULL);
+}
+
 /*
  * Real-time thread:
  */
@@ -97,6 +119,13 @@ static void *pulse_thread(void *p) {
   int ret;
   cpu_set_t mask;
 
+  uint64_t next_increment = 0;
+  uint64_t end_t = 0;
+  uint64_t cur_t = 0;
+  uint64_t i_t = 0;
+  uint64_t i_s = thread_params.interval / 1000;
+  uint64_t i_c = thread_params.interval / 1000;
+
   // Set thread CPU affinity
   if (thread_params.affinity_cpu != -1) {
     CPU_ZERO(&mask);
@@ -120,7 +149,7 @@ static void *pulse_thread(void *p) {
     next = uint_to_ts(thread_params.start_ts);
   }
 
-  // Packet sending loop
+  // Pulse loop
   for (nb_cycles = 0;; nb_cycles++) {
     if (thread_params.max_cycles &&
         nb_cycles >= ((unsigned int)thread_params.max_cycles))
@@ -133,6 +162,65 @@ static void *pulse_thread(void *p) {
 
     add_ns(&next, thread_params.interval);
 
+    if(main_params.interval_input && (thread_params.interval != (main_params.target_interval * 1000))) {
+
+      i_c = thread_params.interval / 1000;
+
+      // If target interval changed
+      if(i_t != main_params.target_interval) {
+
+        i_t = main_params.target_interval;
+        i_s = i_c;
+        cur_t = 0;
+        next_increment = 0;
+
+        if(i_t < i_s)
+          end_t = (main_params.transition_time * USEC_PER_SEC * USEC_PER_SEC * (i_c - i_t)) / (MOTOR_STEPS * i_t * i_c);
+        else
+          end_t = (main_params.transition_time * USEC_PER_SEC * USEC_PER_SEC * (i_t - i_c)) / (MOTOR_STEPS * i_t * i_c);
+
+      }
+
+      if(next_increment) {
+        if(cur_t > next_increment) {
+          i_c += i_t < i_s ? -1 : 1;
+          next_increment = 0;
+        }
+      } else {
+        uint64_t next_i = i_t < i_s ? i_c - 1 : i_c + 1;
+        // Compute time at which we will need to increment / decrement the interval
+
+        if(i_t < i_s)
+          next_increment = (end_t * i_t * (i_s - next_i)) / ((i_s - i_t) * next_i);
+        else
+          next_increment = (end_t * i_t * (next_i - i_s)) / ((i_t - i_s) * next_i);
+
+        // If next increment time is before next interval
+        if(next_increment < cur_t + i_c) {
+          if(i_t < i_s) {
+            i_c = (i_t * i_s * end_t) / ((i_s - i_t) * (cur_t + i_c) + end_t * i_t);
+            i_c = i_c < i_t ? i_t : i_c;
+          }
+          else {
+            i_c = (i_t * i_s * end_t) / (end_t * i_t - (i_t - i_s) * (cur_t + i_c));
+            i_c = i_c > i_t ? i_t : i_c;
+          }
+          next_increment = 0;
+        }
+      }
+
+      cur_t += i_c;
+      if(i_c < 50) {
+        fprintf(stderr, "Interval too small, exiting..\n");
+        exit(EXIT_FAILURE);
+      }
+      if(i_c > USEC_PER_SEC) {
+        fprintf(stderr, "Interval too big, exiting..\n");
+        exit(EXIT_FAILURE);
+      }
+      thread_params.interval = i_c * 1000;
+    }
+
     if(thread_params.poll) {
       poll_wakeup(next, thread_params.poll_margin);
     }
@@ -143,6 +231,7 @@ static void *pulse_thread(void *p) {
         exit(EXIT_FAILURE);
       }
     }
+
   }
 
   pthread_exit(NULL);
@@ -154,7 +243,7 @@ static void *pulse_thread(void *p) {
  *  - Handles the IO and creates the real time thread
  */
 int main(int argc, char *argv[]) {
-  pthread_t thread;
+  pthread_t thread, print_pthread;
   struct sched_param param;
   pthread_attr_t attr;
 
@@ -168,6 +257,7 @@ int main(int argc, char *argv[]) {
   thread_params.poll_margin = 75;
   main_params.refresh_rate = 50000;
   main_params.verbose = 0;
+  main_params.interval_input = 0;
   use_gpio = 0;
 
   // Lock all current and future pages from preventing of being paged to
@@ -180,6 +270,8 @@ int main(int argc, char *argv[]) {
   // Process bash options
   process_options(argc, argv);
 
+  main_params.target_interval = thread_params.interval / 1000;
+
   if (use_gpio)
     enable_gpio(86);
   else
@@ -215,11 +307,21 @@ int main(int argc, char *argv[]) {
   if (pthread_create(&thread, &attr, pulse_thread, NULL))
     error(EXIT_FAILURE, errno, "Couldn't create pulse thread");
 
+  // Create the print thread
+  if (pthread_create(&print_pthread, NULL, print_thread, NULL))
+    error(EXIT_FAILURE, errno, "Couldn't create print thread");
+
   // Verbose loop
   for (;;) {
-    usleep(main_params.refresh_rate);
 
-    if (main_params.verbose) {
+    if (main_params.interval_input) {
+      uint64_t user_input;
+      scanf("%" PRIu64, &user_input);
+      if(user_input)
+        main_params.target_interval = user_input;
+    }
+    else if (main_params.verbose) {
+      usleep(main_params.refresh_rate);
       printf("Toggles: %9" PRIu64 "\n", nb_cycles);
       printf("\033[%dA", 1);
     }
@@ -235,7 +337,7 @@ int main(int argc, char *argv[]) {
  */
 static void process_options(int argc, char *argv[]) {
   for (;;) {
-    int c = getopt(argc, argv, "a:hgi:l:p:r:s:vP:");
+    int c = getopt(argc, argv, "a:hgi:l:p:r:s:vP:U:");
 
     if (c == -1) break;
 
@@ -272,6 +374,10 @@ static void process_options(int argc, char *argv[]) {
         thread_params.poll = 1;
         thread_params.poll_margin = atoi(optarg);
         break;
+      case 'U':
+        main_params.interval_input = 1;
+        main_params.transition_time = atoi(optarg);
+        break;
     }
   }
 }