robofsm: Use a correct way to setup real-time priority

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index b65dc95c45188597993203ea32397d7da52bebd5..de83e95bd54024b974c6252b8a5d20c832037f20 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -277,6 +277,11 @@ void *thread_trajectory_follower(void *arg)
 {
        struct timespec next;
        int ret;
+       struct sched_param param;
+
+       param.sched_priority = THREAD_PRIO_TRAJ_FOLLOWER;
+       if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &param) != 0)
+               perror("Warning: Cannot set RT priority for thread_prio_traj_follower()");
 
        clock_gettime(CLOCK_REALTIME, &next);
        
@@ -367,7 +372,6 @@ void stop()
 int motion_control_init()
 {
        pthread_attr_t tattr;
-       sched_param param;
        pthread_mutexattr_t mattr;
        int ret;
 
@@ -385,15 +389,6 @@ int motion_control_init()
 
        // Trajectory follower thread
        sem_init(&measurement_received, 0, 0);
-       pthread_attr_init (&tattr);
-       pthread_attr_getschedparam (&tattr, &param);
-       pthread_attr_setschedpolicy(&tattr, SCHED_FIFO);
-       param.sched_priority = THREAD_PRIO_TRAJ_FOLLOWER;
-       ret = pthread_attr_setschedparam (&tattr, &param);
-       if(ret) {
-               perror("move_init: pthread_attr_setschedparam(follower)");
-               goto err;
-       }
        ret = pthread_create(&thr_trajectory_follower, &tattr, thread_trajectory_follower, NULL);
        if(ret) {
                perror("move_init: pthread_create");

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 2372fe2cdbef042207f90fbe017141cd5a7258f6..80a4984bda5aeebbc0d2e4eb0bc7203c3026ed1d 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -246,7 +246,7 @@ FSM_STATE_FULL_DECL(act, wait_for_command);
 #define THREAD_PRIO_TRAJ_FOLLOWER 90   /* As high as possible */
 #define THREAD_PRIO_TRAJ_RECLAC 18
 #define OBST_FORGETING_PRIO 17         /* Priority of the thread for forgeting detected obstacles. */
-#define THREAD_PRIO_DISP       15
-#define THREAD_PRIO_JOY                10
+
+
 
 #endif /* ROBOT_H */