robofsm: Motion_control

Move global variables to struct.

Make constants from defines.

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 96efa0a43ab499c3f0f9509cd2224bdd7b4d3588..7e022b75ee5275a9938e74a36218fbc3e9c7a857 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -60,13 +60,13 @@ UL_LOG_CUST(ulogd_motion_control); /* Log domain name = ulogd + name of the file
  * requested position if above this value, the robot lost and we try
  * to reset localization.
  */
-#define MAX_POS_ERROR_M 0.25
+const float MAX_POS_ERROR_M = 0.25;
 
 /**
  * If trajectory end is reached and robot's estimated position is
  * closer than this distance, the movement is considered as "done".
  */
-#define CLOSE_TO_TARGET_M 0.1
+const float CLOSE_TO_TARGET_M = 0.1;
 
 //Controller gains
 const struct balet_params k = {
@@ -84,12 +84,16 @@ const struct balet_params k = {
 #define SIG_DO_CONTROL_NOW (SIGRTMIN+1)
 
 // Global varibles
-static pthread_t thr_trajectory_follower;
-static struct timeval tv_start; /**< Absolute time, when trajectory started. */
+struct MotionControlHandler {
+pthread_t thr_trajectory_follower;
+struct timeval tv_start; /**< Absolute time, when trajectory started. */
 
 /** Stores the actually followed trajectory object */
-static Trajectory *actual_trajectory;
-static pthread_mutex_t actual_trajectory_lock;
+Trajectory *actual_trajectory;
+pthread_mutex_t actual_trajectory_lock;
+};
+
+MotionControlHandler motion_handler;
 
 // Trajectory recalculation 
 sem_t recalculation_not_running;
@@ -109,12 +113,12 @@ static void delete_actual_trajectory()
 {
        Trajectory *old;
        {
-               Guard g(actual_trajectory_lock);
-               old = actual_trajectory;
-               actual_trajectory = NULL;
+               Guard g(motion_handler.actual_trajectory_lock);
+               old = motion_handler.actual_trajectory;
+               motion_handler.actual_trajectory = NULL;
        }
        robot.move_helper.send_speed(0,0);
-       if (old) delete(old);
+       if (old) delete old;
 }
 
 /** Sends events from follower thread to FSM. */
@@ -184,8 +188,8 @@ static void do_control()
        // Calculate reference position
        /***FIXME:should not rely on system clock, the period is fixed***/
        gettimeofday(&tv, NULL);
-       t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
-       t += (tv.tv_sec - tv_start.tv_sec);
+       t = (double)(tv.tv_usec - motion_handler.tv_start.tv_usec) / 1000000.0;
+       t += (tv.tv_sec - motion_handler.tv_start.tv_sec);
 /*
        // check for new trajectory to switch
        // only if the trajectory is already prepared
@@ -200,16 +204,16 @@ static void do_control()
                pthread_mutex_unlock(&switch_to_trajectory_lock);
         }
 */
-       Guard g(actual_trajectory_lock);
-       Trajectory *w = actual_trajectory;
+       Guard g(motion_handler.actual_trajectory_lock);
+       Trajectory *w = motion_handler.actual_trajectory;
        if (w) {
                Pos ref_pos, est_pos, balet_out;
                bool done;
 
                // Calculate reference position
                gettimeofday(&tv, NULL);
-               t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
-               t += (tv.tv_sec - tv_start.tv_sec);
+               t = (double)(tv.tv_usec - motion_handler.tv_start.tv_usec) / 1000000.0;
+               t += (tv.tv_sec - motion_handler.tv_start.tv_sec);
 
                // if switch_to_trajectory is being prepared, it can not stop calculation
                // and start to count again, it could evoke overloading
@@ -219,8 +223,8 @@ static void do_control()
 
                done = w->getRefPos(t, ref_pos);
 
-               if (ref_pos.omega > actual_trajectory->constr.maxomega)
-                       DBG("Omega constraint problem %lf, max %lf -------------------- \n", ref_pos.omega, actual_trajectory->constr.maxomega);
+               if (ref_pos.omega > motion_handler.actual_trajectory->constr.maxomega)
+                       DBG("Omega constraint problem %lf, max %lf -------------------- \n", ref_pos.omega, motion_handler.actual_trajectory->constr.maxomega);
                {
                        Guard g(robot.lock_ref_pos);
                        robot.ref_pos.x = ref_pos.x;
@@ -280,7 +284,7 @@ void *thread_trajectory_follower(void *arg)
        int ret;
        struct sched_param param;
 
-       param.sched_priority = THREAD_PRIO_TRAJ_FOLLOWER;
+       param.sched_priority = 99;
        if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &param) != 0)
                perror("Warning: Cannot set RT priority for thread_prio_traj_follower()");
 
@@ -317,19 +321,19 @@ void *thread_trajectory_follower(void *arg)
  */
 void go(Trajectory *t, double append_time)
 {
-       Guard g(actual_trajectory_lock);
+       Guard g(motion_handler.actual_trajectory_lock);
        Trajectory *old;
-        if (actual_trajectory && append_time != 0) {
+        if (motion_handler.actual_trajectory && append_time != 0) {
                 // trajectory only connects a new one in some specific time
-                if(!actual_trajectory->appendTrajectory(*t, append_time))
+                if(!motion_handler.actual_trajectory->appendTrajectory(*t, append_time))
                         DBG("Can not append trajectory\n");
         } else {
                 // trajectory starts from zero time
-               old = actual_trajectory;
-               gettimeofday(&tv_start, NULL);
-               actual_trajectory = t;
+               old = motion_handler.actual_trajectory;
+               gettimeofday(&(motion_handler.tv_start), NULL);
+               motion_handler.actual_trajectory = t;
 #ifdef MOTION_LOG              
-               t->logTraj(tv_start.tv_sec + 1e-6*tv_start.tv_usec);
+               t->logTraj(motion_handler.tv_start.tv_sec + 1e-6*motion_handler.tv_start.tv_usec);
 #endif
                if (old)
                        delete(old);
@@ -374,7 +378,7 @@ int motion_control_init()
        pthread_mutexattr_t mattr;
        int ret;
 
-       actual_trajectory = NULL;
+       motion_handler.actual_trajectory = NULL;
        //switch_to_trajectory = NULL;
 
 
@@ -382,13 +386,13 @@ int motion_control_init()
 #ifdef HAVE_PRIO_INHERIT
        ret = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
 #endif
-       pthread_mutex_init(&actual_trajectory_lock, &mattr);
+       pthread_mutex_init(&(motion_handler.actual_trajectory_lock), &mattr);
 
        sem_init(&recalculation_not_running, 0, 1);
 
        // Trajectory follower thread
        sem_init(&measurement_received, 0, 0);
-       ret = pthread_create(&thr_trajectory_follower, NULL, thread_trajectory_follower, NULL);
+       ret = pthread_create(&(motion_handler.thr_trajectory_follower), NULL, thread_trajectory_follower, NULL);
        if(ret) {
                perror("move_init: pthread_create");
                goto err;
@@ -401,8 +405,8 @@ int motion_control_init()
 
 void motion_control_done()
 {
-       pthread_cancel(thr_trajectory_follower);
-       pthread_join(thr_trajectory_follower, NULL);
+       pthread_cancel(motion_handler.thr_trajectory_follower);
+       pthread_join(motion_handler.thr_trajectory_follower, NULL);
 
        robot.orte.motion_speed.right = 0;
        robot.orte.motion_speed.left = 0;
@@ -415,17 +419,17 @@ void get_future_pos(double rel_time_sec, Pos &pos, double &switch_time)
        struct timeval tv;
 
        gettimeofday(&tv, NULL);
-       switch_time = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
-       switch_time += (tv.tv_sec - tv_start.tv_sec);
+       switch_time = (double)(tv.tv_usec - motion_handler.tv_start.tv_usec) / 1000000.0;
+       switch_time += (tv.tv_sec - motion_handler.tv_start.tv_sec);
        switch_time += rel_time_sec;
 
-       pthread_mutex_lock(&actual_trajectory_lock);
-       if (actual_trajectory) {
-               actual_trajectory->getRefPos(switch_time, pos);
-               pthread_mutex_unlock(&actual_trajectory_lock);
+       pthread_mutex_lock(&(motion_handler.actual_trajectory_lock));
+       if (motion_handler.actual_trajectory) {
+               motion_handler.actual_trajectory->getRefPos(switch_time, pos);
+               pthread_mutex_unlock(&(motion_handler.actual_trajectory_lock));
        } else {
                // Robot doesn't move, so return current position
-               pthread_mutex_unlock(&actual_trajectory_lock);
+               pthread_mutex_unlock(&(motion_handler.actual_trajectory_lock));
 
                robot.get_est_pos(pos.x, pos.y, pos.phi);
                pos.v = 0;