Improve motor interface

packet-exchange/src/client.c

@@ -87,14 +87,14 @@ static rtt_stat_t rtt_stats = {.min_rtt = INT_MAX};
 static egress_param_t egress_params;
 static egress_param_t egress_params2;
 static egress_stat_t egress_stats = {.packets_sent = 0,
-                                     .min_kernel_latency = INT_MAX,
-                                     .min_interval = INT_MAX};
+  .min_kernel_latency = INT_MAX,
+  .min_interval = INT_MAX};
 static egress_stat_t egress_stats2 = {.packets_sent = 0,
-                                     .min_kernel_latency = INT_MAX,
-                                     .min_interval = INT_MAX};
+  .min_kernel_latency = INT_MAX,
+  .min_interval = INT_MAX};
 
 static ingress_stat_t ingress_stats = {.min_kernel_latency = INT_MAX,
-                                       .min_interval = INT_MAX};
+  .min_interval = INT_MAX};
 static egress_info_t egress_info;
 static egress_info_t egress_info2;
 
@@ -107,6 +107,8 @@ static struct timespec measures_start;
 static struct timespec measures_end;
 static char send_data[MAX_BUFFER_SIZE];
 
+static int reverse_motors = 0;
+
 static void help(char *argv[]) {
   printf(
       "Usage: %s [-a CPU -p PRIO -i USEC -r USEC -l N] [-d BUF_LEN | -c "
@@ -131,7 +133,14 @@ static void help(char *argv[]) {
       "       -S                 Enable tracing until threshold is reached\n"
       "       -U                 Interactive interval change\n"
       "\n",
-      argv[0]);
+    argv[0]);
+}
+
+static double i_to_rps(int i) {
+  return ((double)USEC_PER_SEC) / (MOTOR_STEPS * i);
+}
+static int rps_to_i(double rps) {
+  return USEC_PER_SEC / (MOTOR_STEPS * rps);
 }
 
 static void *print_thread(void *p) {
@@ -142,7 +151,9 @@ static void *print_thread(void *p) {
         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("RPS: %5F RPS Target: %5F s/100 RPS %5d\n", i_to_rps(thread_params.interval / 1000),
+                                                       i_to_rps(main_params.target_interval),
+                                                       main_params.transition_time);
     printf("\033[%dA", 1);
 
     usleep(100000);
@@ -175,7 +186,7 @@ static void *packet_sending_thread(void *p) {
     CPU_SET(thread_params.affinity_cpu, &mask);
     if (sched_setaffinity(0, sizeof(mask), &mask))
       error(EXIT_FAILURE, errno, "Could not set CPU affinity to CPU %d\n",
-            thread_params.affinity_cpu);
+          thread_params.affinity_cpu);
   }
 
   clock_gettime(CLOCK_MONOTONIC, &measures_start);
@@ -244,6 +255,12 @@ static void *packet_sending_thread(void *p) {
     if (thread_params.enable_send_ts) {
       tx_data = next.tv_sec * NSEC_PER_SEC + next.tv_nsec;
       tx_data += thread_params.send_ts_delay;
+
+      if(reverse_motors) {
+        reverse_motors = 0;
+        tx_data = 17;
+      }
+
     }
 
     // Update statistics
@@ -253,8 +270,8 @@ static void *packet_sending_thread(void *p) {
       egress_stats.min_interval = _min_(interval_us, egress_stats.min_interval);
       egress_stats.max_interval = _max_(interval_us, egress_stats.max_interval);
       egress_stats.avg_interval =
-          (egress_stats.avg_interval * nb_cycles + interval_us) /
-          (nb_cycles + 1);
+        (egress_stats.avg_interval * nb_cycles + interval_us) /
+        (nb_cycles + 1);
 
       if (thread_params.enable_threshold_tracing) {
         int jitter = (egress_stats.max_interval - egress_stats.min_interval);
@@ -281,9 +298,9 @@ static void *packet_sending_thread(void *p) {
         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);
+          end_t = (main_params.transition_time * USEC_PER_SEC * USEC_PER_SEC * (i_c - i_t)) / (100 * 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);
+          end_t = (main_params.transition_time * USEC_PER_SEC * USEC_PER_SEC * (i_t - i_c)) / (100 * MOTOR_STEPS * i_t * i_c);
 
       }
 
@@ -339,6 +356,28 @@ invalid_ts:
   exit(EXIT_FAILURE);
 }
 
+static void motor_input(void) {
+
+  char user_input[255];
+  int v;
+  scanf("%s", user_input);
+  switch(user_input[0]) {
+    case 'a':
+      v = atoi(user_input + 1);
+      if(v)
+        main_params.transition_time = v;
+      break;
+    case 'r':
+      reverse_motors = 1;
+      break;
+    default:
+      v = rps_to_i(strtod(user_input, NULL));
+      if(v)
+        main_params.target_interval = v;
+      break;
+  }
+}
+
 /*
  * Main thread:
  *  - Has non-real time priority
@@ -420,7 +459,7 @@ int main(int argc, char *argv[]) {
   // Initialize the UDP packet receiving socket if RTT is measured
   if (tsn_task == RTT_TASK)
     init_udp_recv(&ingress_params, &ingress_stats, enable_histograms,
-                  kernel_latency_hist);
+        kernel_latency_hist);
 
   // Initialize pthread attributes (default values)
   if (pthread_attr_init(&attr)) {
@@ -463,34 +502,33 @@ int main(int argc, char *argv[]) {
     usleep(main_params.refresh_rate);
 
     if (main_params.interval_input) {
-      uint64_t user_input;
-      scanf("%" PRIu64, &user_input);
-      if(user_input)
-        main_params.target_interval = user_input;
+
+      motor_input();
+
     }
     else if (main_params.verbose) {
       // RTT stats printing
       if (tsn_task == RTT_TASK) {
         // N_CYCLES, RTT min / avg / max
         printf("%9" PRIu64 ": RTT: %4d %4d %4d\n", nb_cycles, rtt_stats.min_rtt,
-               rtt_stats.avg_rtt, rtt_stats.max_rtt);
+            rtt_stats.avg_rtt, rtt_stats.max_rtt);
         printf("\033[%dA", 1);
 
         // Packet send stats printing
       } else {
         // N_CYCLES, error queue, measured send interval min / avg / max
         printf("%9" PRIu64 ": [%4d, %4d], I (10us): %3d %3d %3d", nb_cycles,
-               (int)egress_stats.invalid_parameter,
-               (int)egress_stats.missed_deadline,
-               egress_stats.min_interval / 10, egress_stats.avg_interval / 10,
-               egress_stats.max_interval / 10);
+            (int)egress_stats.invalid_parameter,
+            (int)egress_stats.missed_deadline,
+            egress_stats.min_interval / 10, egress_stats.avg_interval / 10,
+            egress_stats.max_interval / 10);
 
         // SOF_TX timestamps
         if (enable_timestamps)
           printf(", K: %4d %4d %4d [%4d]\n", egress_stats.min_kernel_latency,
-                 egress_stats.avg_kernel_latency,
-                 egress_stats.max_kernel_latency,
-                 (int)egress_stats.high_kernel_latency);
+              egress_stats.avg_kernel_latency,
+              egress_stats.max_kernel_latency,
+              (int)egress_stats.high_kernel_latency);
         else
           printf("\n");
 
@@ -508,7 +546,7 @@ int main(int argc, char *argv[]) {
 }
 
 /* Critical TSN task
- */
+*/
 static void do_tsn_task(char *data, uint64_t next_txtime) {
   struct timespec t1, t2;
   int rtt_us;
@@ -531,18 +569,18 @@ static void do_tsn_task(char *data, uint64_t next_txtime) {
     rtt_stats.min_rtt = _min_(rtt_us, rtt_stats.min_rtt);
     rtt_stats.max_rtt = _max_(rtt_us, rtt_stats.max_rtt);
     rtt_stats.avg_rtt =
-        (((uint64_t)rtt_stats.avg_rtt) * (nb_cycles - 1) + rtt_us) / nb_cycles;
+      (((uint64_t)rtt_stats.avg_rtt) * (nb_cycles - 1) + rtt_us) / nb_cycles;
 
     if (rtt_us > MAX_RTT)
       fprintf(stderr, "RTT value higher than MAX_RTT : %d ( > %d)\n", rtt_us,
-              MAX_RTT);
+          MAX_RTT);
     else
       rtt_hist[rtt_us]++;
   }
 }
 
 /* Print histogram
- */
+*/
 static void print_histograms() {
   uint64_t duration;
   int duration_hour, duration_minutes;
@@ -586,7 +624,7 @@ static void sighand(int sig_num) {
 }
 
 /* Process bash options
- */
+*/
 static void process_options(int argc, char *argv[]) {
   for (;;) {
     int c = getopt(argc, argv, "a:bc:d:e:ghi:l:p:q:r:s:tvTS:U:");

packet-exchange/src/common.h

@@ -19,7 +19,7 @@
 #include <linux/udp.h>
 #endif
 
-#define MOTOR_STEPS 20000
+#define MOTOR_STEPS 800
 
 #define NSEC_PER_SEC UINT64_C(1000000000)
 #define USEC_PER_SEC UINT64_C(1000000)

packet-exchange/src/gpio.c

@@ -5,28 +5,44 @@
 #include <sys/stat.h>
 #include <fcntl.h>
 
-static int gpio_state;
+static char path[64];
 static char cmd[128];
 static char * one = "1";
 static char * zero = "0";
-static int fd;
 
-void enable_gpio(int gpio_index) {
-  sprintf(cmd, "echo %d > /sys/class/gpio/export", gpio_index);
-  system(cmd);
-  sprintf(cmd, "echo out > /sys/class/gpio/gpio%d/direction", gpio_index);
-  system(cmd);
+int enable_gpio(int * fd, int gpio_index) {
+  FILE *fp;
+  char gpio_state_str[16];
+  int gpio_state = 0;
 
-  sprintf(cmd, "/sys/class/gpio/gpio%d/value", gpio_index);
-  fd = open(cmd, O_WRONLY);
+  sprintf(path, "/sys/class/gpio/gpio%d/value", gpio_index);
+  sprintf(cmd, "cat %s", path);
+
+  fp = popen(cmd, "r");
+  if (fp == NULL) {
+    fprintf(stderr, "Error when reading gpio\n");
+    exit(EXIT_FAILURE);
+  }
+
+  while (fgets(gpio_state_str, sizeof(gpio_state_str), fp) != NULL) {
+    gpio_state = atoi(gpio_state_str);
+  }
+
+  pclose(fp);
+
+  *fd = open(path, O_WRONLY);
+
+  return gpio_state;
 }
 
-void toggle_gpio() {
+int toggle_gpio(int fd, int gpio_state) {
 
   if(gpio_state)
-        write(fd, one, 1);
-  else
         write(fd, zero, 1);
+  else
+        write(fd, one, 1);
 
   gpio_state = !gpio_state;
+
+  return gpio_state;
 }

packet-exchange/src/gpio.h

 #ifndef GPIO_H
 #define GPIO_H
 
-void enable_gpio(int gpio_index);
-void toggle_gpio();
+int enable_gpio(int * fd, int gpio_index);
+int toggle_gpio(int fd, int gpio_state);
 
 #endif

packet-exchange/src/server.c

@@ -88,8 +88,15 @@ static int64_t max_diff_ts = 0;
 static int64_t avg_diff_ts = 0;
 static uint64_t high_diff_ts = 0;
 
+static int debug_latencies = 0;
 static int interval_us;
 
+static int gpio86_fd;
+static int gpio84_fd;
+static int gpio86_state;
+static int gpio84_state;
+static int received_reverse_motor = 0;
+
 static char ts_tracemark_buf[64];
 
 static void help(char *argv[]) {
@@ -110,6 +117,7 @@ static void help(char *argv[]) {
       "       -g                 Print histograms to sdtout on exit\n"
       "       -t                 Enable timestamps\n"
       "       -x                 Use AF_XDP sockets\n"
+      "       -D                 Debug latencies\n"
       "       -X                 Trace during XDP packet reception\n"
       "       -T THRESHOLD       Enable tracing until THRESHOLD is reached\n"
       "       -M                 Send tracemark when packet is received\n"
@@ -127,6 +135,7 @@ static void *emit_signal_thread(void *p) {
   int64_t emit_diff, ts_diff;
   int latency_spike = 0;
   int ret;
+  int reverse_motor = 0;
 
   // Set thread CPU affinity
   if (thread_params.affinity_cpu) {
@@ -142,34 +151,51 @@ static void *emit_signal_thread(void *p) {
   for (int i = 0;;i++) {
     pthread_cond_wait(&emit_signal_ts_received, &emit_signal_mutex);
 
+    if(received_reverse_motor) {
+      received_reverse_motor = 0;
+      reverse_motor = 1;
+      continue;
+    }
+
     ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &emit_signal_next, NULL);
     if (ret) {
       fprintf(stderr, "clock_nanosleep returned error: %d, aborting...\n", ret);
       exit(EXIT_FAILURE);
     }
 
-    toggle_gpio();
+    if(reverse_motor) {
+      gpio84_state = toggle_gpio(gpio84_fd, gpio84_state);
+      reverse_motor = 0;
+    }
+    else {
+      gpio86_state = toggle_gpio(gpio86_fd, gpio86_state);
+      usleep(3);
+      gpio86_state = toggle_gpio(gpio86_fd, gpio86_state);
+    }
+
     clock_gettime(CLOCK_REALTIME, &current);
     
-    // Check if something went wrong
-    //if(i > 0) {
-    //  emit_diff = calcdiff_ns_signed(current, previous_emit);
-    //  ts_diff   = calcdiff_ns_signed(emit_signal_next, previous_ts);
-    //  if((emit_diff < ((int64_t)thread_params.interval) - ERROR_MARGIN_NS) ||
-    //     (emit_diff > ((int64_t)thread_params.interval) + ERROR_MARGIN_NS)) {
-    //    fprintf(stderr, "Signal emission interval reached error threshold: %" PRIi64 "\n", emit_diff);
-    //    latency_spike = 1;
-    //  }
-    //  if((ts_diff < ((int64_t)thread_params.interval) - ERROR_MARGIN_NS) ||
-    //     (ts_diff > ((int64_t)thread_params.interval) + ERROR_MARGIN_NS)) {
-    //    fprintf(stderr, "Timestamp interval reached error threshold: %" PRIi64 "\n", ts_diff);
-    //    latency_spike = 1;
-    //  }
-    //  if(latency_spike) {
-    //    fprintf(stderr, "Exiting... Current interval: %d\n", interval_us);
-    //    exit(EXIT_FAILURE);
-    //  }
-    //}
+    if(debug_latencies) {
+      //Check if something went wrong
+      if(i > 0) {
+        emit_diff = calcdiff_ns_signed(current, previous_emit);
+        ts_diff   = calcdiff_ns_signed(emit_signal_next, previous_ts);
+        if((emit_diff < ((int64_t)thread_params.interval) - ERROR_MARGIN_NS) ||
+           (emit_diff > ((int64_t)thread_params.interval) + ERROR_MARGIN_NS)) {
+          fprintf(stderr, "Signal emission interval reached error threshold: %" PRIi64 "\n", emit_diff);
+          latency_spike = 1;
+        }
+        if((ts_diff < ((int64_t)thread_params.interval) - ERROR_MARGIN_NS) ||
+           (ts_diff > ((int64_t)thread_params.interval) + ERROR_MARGIN_NS)) {
+          fprintf(stderr, "Timestamp interval reached error threshold: %" PRIi64 "\n", ts_diff);
+          latency_spike = 1;
+        }
+        if(latency_spike) {
+          fprintf(stderr, "Exiting... Current interval: %d\n", interval_us);
+          exit(EXIT_FAILURE);
+        }
+      }
+    }
     previous_emit = current;
     previous_ts = emit_signal_next;
   }
@@ -255,8 +281,14 @@ static void *tsn_thread(void *p) {
         else
           emit_signal_t = decode(ingress_stats.data);
 
+        if(emit_signal_t < UINT64_C(1576800000000000000) && emit_signal_t != 17)
+          continue;
+
         pthread_mutex_lock(&emit_signal_mutex);
-        emit_signal_next = uint_to_ts(emit_signal_t);
+        if(emit_signal_t == 17)
+          received_reverse_motor = 1;
+        else
+          emit_signal_next = uint_to_ts(emit_signal_t);
         pthread_cond_signal(&emit_signal_ts_received);
         pthread_mutex_unlock(&emit_signal_mutex);
       }
@@ -445,7 +477,8 @@ int main(int argc, char *argv[]) {
     pthread_mutex_init(&emit_signal_mutex, NULL);
     pthread_cond_init(&emit_signal_ts_received, NULL);
 
-    enable_gpio(86);
+    gpio86_state = enable_gpio(&gpio86_fd, 86);
+    gpio84_state = enable_gpio(&gpio84_fd, 84);
   }
 
   create_thread(tsn_thread);
@@ -577,7 +610,7 @@ static void process_options(int argc, char *argv[]) {
   int network_if_specified = 0;
 
   for (;;) {
-    int c = getopt(argc, argv, "a:b:cCs:d:f:ghi:p:r:tvxXT:GMS");
+    int c = getopt(argc, argv, "a:b:cCs:d:f:ghi:p:r:tvxDXT:GMS");
 
     if (c == -1) break;
 
@@ -634,6 +667,9 @@ static void process_options(int argc, char *argv[]) {
       case 'x':
         tsn_task = XDP_TASK;
         break;
+      case 'D':
+        debug_latencies = 1;
+        break;
       case 'X':
         main_params.enable_xdp_tracing = 1;
         break;

scripts/start-motor

+#!/bin/bash
+
+script_dir=$(dirname $(realpath $0))
+
+usage() {
+  cat << ENDUSAGE
+Usage: $0 [-h] [-is -c USEC] [-X]
+   -h             Show help
+   -i             Init motors
+   -s             Start servers on olimex boards
+   -c USEC        Specify which offset to use for the timestamp in the packet
+   -X             Use XDP
+   BOARD_HOSTNAME Uses /etc/hosts to find the IP address associated to the hostname
+ENDUSAGE
+1>&2;
+
+  exit 1;
+}
+
+interval=1000
+server_opts=""
+delay=250
+
+while getopts "hisc:X" opt; do
+  case "${opt}" in
+    h )
+        usage
+        ;;
+    i )
+        init_motors=1
+        ;;
+    s )
+        start_servers=1
+        ;;
+    c )
+        delay=${OPTARG}
+        ;;
+    X )
+        server_opts+=" -X "
+      ;;
+    * )
+        usage
+        ;;
+  esac
+done
+
+shift $((OPTIND-1))
+
+board1="onyx"
+board2="slate"
+
+killall client;
+killall run-client;
+
+if [ -n "$init_motors" ]; then
+  $script_dir/sudossh $board1 "motor-scripts/motor-init";
+  $script_dir/sudossh $board2 "motor-scripts/motor-init";
+  $script_dir/sudossh $board1 "motor-scripts/motor-cw";
+  $script_dir/sudossh $board2 "motor-scripts/motor-ccw";
+fi
+
+if [ -n "$start_servers" ]; then
+  $script_dir/sudossh $board1 "killall server";
+  $script_dir/sudossh $board2 "killall server";
+  $script_dir/exec-ssh-nohup $board1 "run-server -c $interval $server_opts" server_log;
+  $script_dir/exec-ssh-nohup $board2 "run-server -c $interval $server_opts" server_log;
+  sleep 5;
+fi
+
+echo "$script_dir/run-client -i 40000 -c $delay -U 100 enp1s0 $board1 enp2s0 $board2";
+$script_dir/run-client -i 40000 -c $delay -U 100 enp1s0 $board1 enp2s0 $board2