-#define __UNLOCK_CHECK(mutex) \
- if (robot_lock_log.locked[--robot_lock_log.idx] != mutex || \
- robot_lock_log.idx < 0) \
- ul_logerr("!!! Locking error %s:%d\n", __FILE__, __LINE__);
+#define __UNLOCK_CHECK(mutex) \
+ if (robot_lock_log.locked[--robot_lock_log.idx] != mutex || \
+ robot_lock_log.idx < 0) \
+ ul_logerr("!!! Locking error %s:%d\n", __FILE__, __LINE__);
-//#define __robot_lock_ lock /* ROBOT_LOCK() */
-#define __robot_lock_ref_pos lock_ref_pos
-#define __robot_lock_est_pos_odo lock_est_pos_odo
-#define __robot_lock_est_pos_indep_odo lock_est_pos_indep_odo
-#define __robot_lock_joy_data lock_joy_data
-#define __robot_lock_meas_angles lock_meas_angles
-#define __robot_lock_drives lock
-#define __robot_lock_sharps lock
-#define __robot_lock_hokuyo lock
+//#define __robot_lock_ lock /* ROBOT_LOCK() */
+#define __robot_lock_ref_pos lock_ref_pos
+#define __robot_lock_est_pos_odo lock_est_pos_odo
+#define __robot_lock_est_pos_indep_odo lock_est_pos_indep_odo
+#define __robot_lock_joy_data lock_joy_data
+#define __robot_lock_meas_angles lock_meas_angles
+#define __robot_lock_drives lock
+#define __robot_lock_sharps lock
+#define __robot_lock_hokuyo lock
- pthread_mutex_t lock;
- pthread_mutex_t lock_ref_pos;
- pthread_mutex_t lock_est_pos_odo;
- pthread_mutex_t lock_est_pos_indep_odo;
- pthread_mutex_t lock_meas_angles;
- pthread_mutex_t lock_joy_data;
- pthread_mutex_t lock_disp;
- pthread_mutex_t lock_camera;
-
- /* competition parameters */
+ pthread_mutex_t lock;
+ pthread_mutex_t lock_ref_pos;
+ pthread_mutex_t lock_est_pos_odo;
+ pthread_mutex_t lock_est_pos_indep_odo;
+ pthread_mutex_t lock_meas_angles;
+ pthread_mutex_t lock_joy_data;
+ pthread_mutex_t lock_disp;
+ pthread_mutex_t lock_camera;
+
+ /* competition parameters */
- /** State variable used for controlling the robot by pluggin
- * in and out start connector. */
- enum robot_start_state start_state;
-
- /** Temporary storage for new trajectory. After the trajectory creation is
- * finished, this trajectory is submitted to fsmmove. */
- void *new_trajectory;
-
- unsigned char isTrajectory;
- sem_t start;
-
- struct fsm_main_loop main_loop;
-
- /* partial FSMs */
- struct {
- struct robo_fsm main;
- struct robo_fsm motion;
- struct robo_fsm act;
- } fsm;
-
- /* actual position */
- struct robot_pos_type ref_pos;
- /* estimated position */
- struct robot_pos_type est_pos_odo;
- struct robot_pos_type est_pos_indep_odo;
-
- /** True if est_pos_odo is updated according to reception of motion_irc */
- bool odometry_works;
- /** True if est_pos_indep_odo is updated according to reception of motion_indep_odo */
- bool indep_odometry_works;
-
- bool use_back_bumpers;
- bool use_left_bumper;
- bool use_right_bumper;
-
- /** True iff at least one wheel rotates backward */
- bool moves_backward;
-
- /*
- * sometimes H8S does not send queried odometry
- * following flag has been added for EKF estimator,
- * since is has been hardly disturbed by missing measurement
- * (taken as sudden zero velocities)
- */
- bool motion_irc_received;
-
- /* orte */
- struct robottype_orte_data orte;
-
- /* sensors */
- struct motion_irc_type motion_irc; /* motor odometry */
- struct odo_data_type odo_data; /* independent odometry */
- struct corr_distances_type corr_distances; /* ultrasound */
-
- struct lidar_scan_type hokuyo;
- bool ignore_hokuyo;
+ /** State variable used for controlling the robot by pluggin
+ * in and out start connector. */
+ enum robot_start_state start_state;
+
+ /** Temporary storage for new trajectory. After the trajectory creation is
+ * finished, this trajectory is submitted to fsmmove. */
+ void *new_trajectory;
+
+ unsigned char isTrajectory;
+ sem_t start;
+
+ struct fsm_main_loop main_loop;
+
+ /* partial FSMs */
+ struct {
+ struct robo_fsm main;
+ struct robo_fsm motion;
+ struct robo_fsm act;
+ } fsm;
+
+ /* actual position */
+ struct robot_pos_type ref_pos;
+ /* estimated position */
+ struct robot_pos_type est_pos_odo;
+ struct robot_pos_type est_pos_indep_odo;
+
+ /** True if est_pos_odo is updated according to reception of motion_irc */
+ bool odometry_works;
+ /** True if est_pos_indep_odo is updated according to reception of motion_indep_odo */
+ bool indep_odometry_works;
+
+ bool use_back_bumpers;
+ bool use_left_bumper;
+ bool use_right_bumper;
+
+ /** True iff at least one wheel rotates backward */
+ bool moves_backward;
+
+ /*
+ * sometimes H8S does not send queried odometry
+ * following flag has been added for EKF estimator,
+ * since is has been hardly disturbed by missing measurement
+ * (taken as sudden zero velocities)
+ */
+ bool motion_irc_received;
+
+ /* orte */
+ struct robottype_orte_data orte;
+
+ /* sensors */
+ struct motion_irc_type motion_irc; /* motor odometry */
+ struct odo_data_type odo_data; /* independent odometry */
+ struct corr_distances_type corr_distances; /* ultrasound */
+
+ struct lidar_scan_type hokuyo;
+ bool ignore_hokuyo;