Motion control timing is now driven by clock_nanosleep()

This approach is more straightforward than playing with signals and POSIX
timers.

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index fdea81a938777e0598aa4050907e64bd1058ac02..097e71b46762164bfc30b36be87b5440ad32f6a8 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -8,6 +8,8 @@
  * 
  */
 
+#define DEBUG
+
 #include <sys/time.h>
 #include <time.h>
 #include "trgen.h"
@@ -41,10 +43,8 @@ const struct balet_params k = {
   p_perpen: 10                 // dy gain
 };
 
-//#define MOTION_PERIOD_NS (500/*ms*/*1000*1000)
 #define MOTION_PERIOD_NS (50/*ms*/*1000*1000)
-#define SIG_TIMER (SIGRTMIN+0)
-#define SIG_NEWDATA (SIGRTMIN+1)
+#define SIG_DO_CONTROL_NOW (SIGRTMIN+1)
 
 // Global varibles
 pthread_t thr_trajectory_follower;
@@ -146,9 +146,8 @@ static void do_control()
        // only if the trajectory is already prepared
        if (switch_to_trajectory != NULL && t >= switch_time) {
                pthread_mutex_lock(&switch_to_trajectory_lock);
-#ifdef NEVER // DEBUG
+#ifdef DEBUG
                printf("SWITCHING to new trajectory\n");
-               fflush(stdout);
 #endif
                go(switch_to_trajectory);
                // nothing prepared now
@@ -200,7 +199,7 @@ static void do_control()
                est_pos.phi = ref_pos.phi;
 #endif
 
-#ifdef NEVER //DEBUG
+#ifdef DEBUG
                printf("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f  omega=%-4.02f, time=%lf \r", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega, t);
                fflush(stdout);
 #endif
@@ -221,6 +220,10 @@ static void do_control()
        pthread_mutex_unlock(&actual_trajectory_lock);
 }
 
+void dummy_handler(int)
+{
+}
+
 /**
  * A thread running the controller.
  *
@@ -235,42 +238,24 @@ static void do_control()
  */
 void *thread_trajectory_follower(void *arg)
 {
-       struct itimerspec ts;
-       sigset_t sigset;
-       struct sigevent sigevent;
-       timer_t timer;
-
-       ts.it_value.tv_sec = 0;
-       ts.it_value.tv_nsec = MOTION_PERIOD_NS;
-       ts.it_interval.tv_sec = 0;
-       ts.it_interval.tv_nsec = MOTION_PERIOD_NS;
-
-       sigemptyset(&sigset);
-       sigaddset(&sigset, SIG_TIMER);
-       sigaddset(&sigset, SIG_NEWDATA);
-
-       pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*) NULL);
-
-       sigevent.sigev_notify = SIGEV_SIGNAL;
-       sigevent.sigev_signo = SIG_TIMER;
-       sigevent.sigev_value.sival_int = 0;
-
-       if (timer_create(CLOCK_REALTIME, &sigevent, &timer) < 0) {
-               perror("move: timer_create");
-               return NULL;
-       }
+       struct timespec next;
+       int ret;
 
-       if (timer_settime(timer, 0, &ts, NULL) < 0) {
-               perror("move: timer_settimer");
-               return NULL;
-       }
+       // Register an empty handler, so that clock_nanosleep bellow
+       // is interrupted by this signal.
+       signal(SIG_DO_CONTROL_NOW, dummy_handler);
+       clock_gettime(CLOCK_MONOTONIC, &next);
 
        while (1) {
-               int sig;
-               // Wait for start of new period or new data
-               sigwait(&sigset, &sig);
-               //DBG("signal %d ", sig - SIGRTMIN);
+               ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);
+
                do_control();
+
+               next.tv_nsec += MOTION_PERIOD_NS;
+               if (next.tv_nsec >= 1000000000) {
+                   next.tv_sec++;
+                   next.tv_nsec -= 1000000000;
+               }
        }
 }
 
@@ -313,7 +298,10 @@ void switch_trajectory_at(Trajectory *t, double time)
 void stop()
 {
        delete_actual_trajectory();
-       pthread_kill(thr_trajectory_follower, SIG_NEWDATA);
+
+       // Interrupt clock_nanosleep in thread_trajectory_follower(),
+       // so we stop immediately.
+       pthread_kill(thr_trajectory_follower, SIG_DO_CONTROL_NOW);
 }
 
 /** 

diff --git a/src/robofsm/motion-control.h b/src/robofsm/motion-control.h
index 2701120432ce263011ff052f334205915a327962..ec0b0f651fc34560c661b18bf01e74c7c3e118bc 100644 (file)
--- a/src/robofsm/motion-control.h
+++ b/src/robofsm/motion-control.h
@@ -29,6 +29,5 @@ int motion_control_init();
 void go(Trajectory *t);
 void stop();
 void switch_trajectory_at(Trajectory *t, double time);
-void *thread_trajectory_follower(void *arg);
 
 #endif

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index b88fd2d7271504a7634e65ef6c3bda68bb9804ef..9ed519df49f1dad5d2326b7381105610fd0d4588 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -30,18 +30,6 @@ struct robot robot;
 struct lock_log robot_lock_log;
 #endif
 
-static void block_signals()
-{
-       sigset_t sigset;
-       int i;
-
-       sigemptyset(&sigset);
-       for (i=0; i<7; i++)
-               sigaddset(&sigset, SIGRTMIN+i);
-
-       pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*)NULL);
-}
-
 static void int_handler(int sig)
 {
        robot_exit();
@@ -119,7 +107,6 @@ int robot_init()
 
        signal(SIGINT, int_handler);
        signal(SIGTERM, int_handler);
-       block_signals();
 
        /* initial values */
        robot.orte.motion_speed.left = 0;