Skip to content

T
tsn-measures
Commit e51b5052 authored by Joanne Hugé's avatar Joanne Hugé
Browse files
Options

Improve motor interface

parent 6c2dd397
Hide whitespace changes
Inline Side-by-side
Showing with 229 additions and 68 deletions
packet-exchange/src/client.c
View file @ e51b5052
......@@ -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
View file @ e51b5052
......@@ -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
View file @ e51b5052
......@@ -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
View file @ e51b5052
#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
View file @ e51b5052
......@@ -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 0 → 100755
View file @ e51b5052
#!/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
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment
GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7