robofsm: added instrumentation for do_estimation()

diff --git a/build/ppc/config.target b/build/ppc/config.target
index df42373207da7a4edd2683279fb29a7b95337ca9..22acc43366652b06a2d7fcaaf4acaab4f3588132 100644 (file)
--- a/build/ppc/config.target
+++ b/build/ppc/config.target
@@ -29,6 +29,7 @@ CONFIG_CLOCK_PROCESS=n
 CONFIG_CLOCK_THREAD=n
 # turn on timestamping for individual parts of SW
 CONFIG_TIMING_MOTION_CONTROL=y
+CONFIG_TIMING_MOTION_ESTIMATION=y
 # timestamping method - choose only one!
 CONFIG_TIMING_IPOINT_ASM=y
 CONFIG_TIMING_IPOINT_CLOCK_GETTIME=n

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 6e4b29b6d1ff8bf025d505cf19f0107077aefa2e..959bb153e6f2434daf2159cbbca64a9f3d4c17fa 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -33,6 +33,7 @@
 #include <map.h>
 #include "robot_config.h"
 #include <robomath.h>
+#include <sys/syscall.h>
 
 #define MOTION_CONTROL
 #include "motion-control.h"
@@ -42,7 +43,7 @@
 #include <geom.h>
 
 /* Timing analysis */
-#ifdef CONFIG_TIMING_MOTION_CONTROL
+#ifdef CONFIG_TIMING_ANALYSIS
 #include <timing.h>
 #endif
 
@@ -208,6 +209,12 @@ static void do_estimation()
        real_t x[8], P[8*8];
        int i, odo_received, err[5];
 
+#ifdef CONFIG_TIMING_ANALYSIS
+#ifdef CONFIG_TIMING_MOTION_ESTIMATION
+       timing_ipoint(IPOINT_MOTION_DO_ESTIMATION_IN);
+#endif
+#endif
+
 #ifdef WE_ARE_RED
        robot.team_color = RED;
 #else
@@ -265,6 +272,11 @@ static void do_estimation()
        robot.est_pos_uzv.y = x[1] - ODO_D*sin(x[6]);
        robot.est_pos_uzv.phi = x[6];
        ROBOT_UNLOCK(est_pos_uzv);
+#ifdef CONFIG_TIMING_ANALYSIS
+#ifdef CONFIG_TIMING_MOTION_ESTIMATION
+       timing_ipoint(IPOINT_MOTION_DO_ESTIMATION_OUT);
+#endif
+#endif
 }
 
 static void do_control()
@@ -276,8 +288,10 @@ static void do_control()
 
        // Calculate reference position
        /***FIXME:should not rely on system clock, the period is fixed***/
+#ifdef CONFIG_TIMING_ANALYSIS
 #ifdef CONFIG_TIMING_MOTION_CONTROL
        timing_ipoint(IPOINT_MOTION_DO_CONTROL_IN);
+#endif
 #endif
        gettimeofday(&tv, NULL);
        t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
@@ -354,9 +368,11 @@ static void do_control()
        robot_send_speed(speedl, speedr);
        pthread_mutex_unlock(&actual_trajectory_lock);
 
+#ifdef CONFIG_TIMING_ANALYSIS
 #ifdef CONFIG_TIMING_MOTION_CONTROL
        timing_ipoint(IPOINT_MOTION_DO_CONTROL_OUT);
 #endif
+#endif
 }
 
 void dummy_handler(int)
@@ -389,6 +405,9 @@ void *thread_trajectory_follower(void *arg)
        struct timespec next;
        int ret;
 
+#ifdef CONFIG_TIMING_ANALYSIS
+       timing_set_pid(syscall(SYS_gettid));
+#endif
        clock_gettime(CLOCK_REALTIME, &next);
        
        while (1) {