motion-control, do_estimation(): beacon coordinates set according to team color

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 5994684db17a658e820aab198d075e704fcb38f7..000a021c805eece9389e9468d5ce232672ee58f6 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -174,27 +174,8 @@ static void check_for_collision_in_future(Trajectory *traj, double current_time)
 static void do_estimation()
 {
        static bool virgo = true;
-
-//#define WE_ARE_RED
-#define WE_ARE_GREEN
-
-#ifdef WE_ARE_GREEN
-#ifndef WE_ARE_RED
-       static real_t beacon_xy[3][2] = {
-               { 3.062, -0.05},
-               {-0.062,  1.05},
-               { 3.062,  2.162},
-       };
-#endif
-#else
-#ifdef WE_ARE_RED
-       static real_t beacon_xy[3][2] = {
-               {-0.062, -0.05},
-               { 3.062,  1.05},
-               {-0.062,  2.162},
-       };
-#endif
-#endif
+       static int color = 0;  /*FIXME:maybe should be enum, but identifier is re#defined in robot.h */
+       static real_t beacon_xy[3][2];
        static ekf8_t ekf8;
        static uint32_t odo0[2];
        static real_t y[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
@@ -203,6 +184,7 @@ static void do_estimation()
        real_t x[8], P[8*8];
        int i, odo_received, err[5];
 
+       /**FIXME: why is this team_color forcing here??**/
 #ifdef WE_ARE_RED
        robot.team_color = RED;
 #else
@@ -239,16 +221,26 @@ static void do_estimation()
        DBG("UZV+ODO:  %f  %f  %f  %f  %f\n", y[0], y[1], y[2], y[3], y[4]);
        //DBG("ODO:  %f  %f    %u  %u\n", y[3], y[4], odo[0], odo[1]);
 
-       if (virgo || init_ekf_flag) {
-               /*FIXME:reflect init pos & beacon coords accord.to our color */
+       if (virgo || init_ekf_flag || (robot.team_color != color)) {
+               /* set beacon coords according to our color */
+               const struct beacon_pos *bp =
+                       (robot.team_color == RED) ? beacon_red : beacon_green;
+               for (i = 0; i < 3; i++) {
+                       beacon_xy[i][0] = bp[i].x;
+                       beacon_xy[i][1] = bp[i].y;
+               }
+
+               /*FIXME: is initial pos set correctly??*/
                ROBOT_LOCK(est_pos_uzv);
                real_t xy0[] = {robot.est_pos_uzv.x, robot.est_pos_uzv.y};
                y[3] = y[4] = 0.0;
                ekf8_init(&ekf8, (real_t*)beacon_xy, D_MAX, 30.0, xy0, y);
                ekf8.ekf.x[6] = robot.est_pos_uzv.phi;
+               ROBOT_UNLOCK(est_pos_uzv);
+
+               color = robot.team_color;
                init_ekf_flag = false;
                virgo = false;
-               ROBOT_UNLOCK(est_pos_uzv);
        }
 
        ekf8_step(&ekf8, x, P, err, y);