- /* locking should not be needed, but... */
- ROBOT_LOCK(motion_irc);
- robot.motion_irc = *instance;
- robot.motion_irc_received = 1;
- ROBOT_UNLOCK(motion_irc);
+ if (first_run) {
+ ROBOT_LOCK(odo_data);
+ robot.odo_data = *instance;
+ ROBOT_UNLOCK(odo_data);
+ first_run = false;
+ break;
+ }
+
+ dleft = ((robot.odo_data.left - instance->left) >> 8) * c * robot.odo_cal_a; // TODO >> 8 ?
+ dright = ((instance->right - robot.odo_data.right) >> 8) * c * robot.odo_cal_b;
+
+ dtang = (dleft + dright) / 2.0;
+ dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
+
+ ROBOT_LOCK(est_pos_indep_odo);
+ robot.odo_distance_a +=dleft;
+ robot.odo_distance_b +=dright;
+ double a = robot.est_pos_indep_odo.phi;
+ robot.est_pos_indep_odo.x += dtang*cos(a);
+ robot.est_pos_indep_odo.y += dtang*sin(a);
+ robot.est_pos_indep_odo.phi += dphi;
+ ROBOT_UNLOCK(est_pos_indep_odo);
+
+ ROBOT_LOCK(odo_data);
+ robot.odo_data = *instance;
+ ROBOT_UNLOCK(odo_data);
+
+ robot.indep_odometry_works = true;
+
+ /* wake up motion-control/thread_trajectory_follower */
+ sem_post(&measurement_received);