23 #include "acquisition_thread.h"
25 #include "force_feedback.h"
27 #include <core/exceptions/system.h>
28 #include <core/threading/mutex.h>
29 #include <linux/joystick.h>
31 #include <sys/types.h>
32 #include <utils/time/time.h>
43 #define COMBO_IDX_UP 0
44 #define COMBO_IDX_DOWN 1
45 #define COMBO_IDX_LEFT 2
46 #define COMBO_IDX_RIGHT 3
47 #define COMBO_IDX_RELEASE 4
57 :
Thread(
"JoystickAcquisitionThread",
Thread::OPMODE_CONTINUOUS)
77 :
Thread(
"JoystickAcquisitionThread",
Thread::OPMODE_CONTINUOUS)
85 safety_lockout_ =
true;
94 cfg_retry_interval_ =
config->
get_float(
"/hardware/joystick/retry_interval");
96 e.
append(
"Could not read all required config values for %s",
name());
100 safety_lockout_ =
true;
102 safety_lockout_ =
config->
get_bool(
"/hardware/joystick/safety_lockout/enable");
105 if (safety_lockout_) {
106 cfg_safety_lockout_timeout_ =
config->
get_float(
"/hardware/joystick/safety_lockout/timeout");
107 cfg_safety_button_mask_ =
config->
get_uint(
"/hardware/joystick/safety_lockout/button-mask");
108 cfg_safety_bypass_button_mask_ = 0;
110 cfg_safety_bypass_button_mask_ =
111 config->
get_uint(
"/hardware/joystick/safety_lockout/bypass-button-mask");
115 for (
int i = 0; i < 5; ++i)
116 safety_combo_[i] =
false;
118 cfg_lazy_init_ =
false;
120 cfg_lazy_init_ =
config->
get_bool(
"/hardware/joystick/allow_deferred_initialization");
125 init(cfg_device_file_, cfg_lazy_init_);
127 if (!cfg_lazy_init_) {
128 e.
append(
"Deferred initialization of joystick device disabled");
133 if (!connected_ && cfg_lazy_init_) {
137 if (safety_lockout_) {
139 "To enable joystick, move primary cross all the way in all "
140 "directions while holding first button. Then let go of button.");
145 JoystickAcquisitionThread::open_joystick()
147 fd_ = open(cfg_device_file_.c_str(), O_RDONLY);
151 "Opening the joystick device file failed");
154 if (ioctl(fd_, JSIOCGNAME(
sizeof(joystick_name_)), joystick_name_) < 0) {
155 throw Exception(errno,
"Failed to get name of joystick");
157 if (ioctl(fd_, JSIOCGAXES, &num_axes_) < 0) {
158 throw Exception(errno,
"Failed to get number of axes for joystick");
160 if (ioctl(fd_, JSIOCGBUTTONS, &num_buttons_) < 0) {
161 throw Exception(errno,
"Failed to get number of buttons for joystick");
164 if (axis_values_ == NULL) {
167 axis_array_size_ = std::max((
int)num_axes_, 8);
168 axis_values_ = (
float *)malloc(
sizeof(
float) * axis_array_size_);
169 }
else if (num_axes_ > std::max((
int)axis_array_size_, 8)) {
171 num_axes_ = axis_array_size_;
180 memset(axis_values_, 0,
sizeof(
float) * axis_array_size_);
181 pressed_buttons_ = 0;
187 just_connected_ =
true;
191 JoystickAcquisitionThread::open_forcefeedback()
232 just_connected_ =
false;
235 cfg_device_file_ = device_file;
239 open_forcefeedback();
245 if (!allow_open_fail)
248 data_mutex_ =
new Mutex();
267 long int timeout_sec = (
long int)truncf(cfg_safety_lockout_timeout_);
268 long int timeout_usec = (cfg_safety_lockout_timeout_ - timeout_sec) * 10000000;
269 timeval timeout = {timeout_sec, timeout_usec};
273 FD_SET(fd_, &read_fds);
276 rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
279 if (!safety_lockout_) {
281 "No action for %.2f seconds, re-enabling safety lockout",
282 cfg_safety_lockout_timeout_);
283 safety_lockout_ =
true;
284 for (
int i = 0; i < 5; ++i)
285 safety_combo_[i] =
false;
291 if (rv == -1 || read(fd_, &e,
sizeof(
struct js_event)) < (
int)
sizeof(
struct js_event)) {
296 just_connected_ =
false;
297 safety_lockout_ =
true;
307 new_data_ = !safety_lockout_;
308 unsigned int last_pressed_buttons = pressed_buttons_;
310 if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON) {
312 if (e.number <= 32) {
314 pressed_buttons_ |= (1 << e.number);
316 pressed_buttons_ &= ~(1 << e.number);
321 }
else if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_AXIS) {
322 if (e.number >= axis_array_size_) {
324 "Got value for axis %u, but only %u axes registered. "
325 "Plugged in a different joystick? Ignoring.",
332 axis_values_[e.number] = (e.value == 0) ? 0. : (e.value / -32767.f);
343 && ((cfg_safety_bypass_button_mask_ & pressed_buttons_)
344 || ((cfg_safety_bypass_button_mask_ & last_pressed_buttons)
345 && pressed_buttons_ == 0))) {
351 if (safety_lockout_) {
354 if (num_axes_ < 2 || num_buttons_ == 0) {
355 safety_combo_[COMBO_IDX_UP] =
true;
356 safety_combo_[COMBO_IDX_DOWN] =
true;
357 safety_combo_[COMBO_IDX_RIGHT] =
true;
358 safety_combo_[COMBO_IDX_LEFT] =
true;
359 safety_combo_[COMBO_IDX_RELEASE] =
true;
361 if (pressed_buttons_ & cfg_safety_button_mask_) {
362 if (axis_values_[0] > 0.9)
363 safety_combo_[COMBO_IDX_UP] =
true;
364 if (axis_values_[0] < -0.9)
365 safety_combo_[COMBO_IDX_DOWN] =
true;
366 if (axis_values_[1] > 0.9)
367 safety_combo_[COMBO_IDX_RIGHT] =
true;
368 if (axis_values_[1] < -0.9)
369 safety_combo_[COMBO_IDX_LEFT] =
true;
371 if (safety_combo_[COMBO_IDX_UP] && safety_combo_[COMBO_IDX_DOWN]
372 && safety_combo_[COMBO_IDX_LEFT] && safety_combo_[COMBO_IDX_RIGHT]
373 && pressed_buttons_ == 0) {
374 safety_combo_[COMBO_IDX_RELEASE] =
true;
378 if (safety_combo_[COMBO_IDX_UP] && safety_combo_[COMBO_IDX_DOWN]
379 && safety_combo_[COMBO_IDX_LEFT] && safety_combo_[COMBO_IDX_RIGHT]
380 && safety_combo_[COMBO_IDX_RELEASE]) {
382 safety_lockout_ =
false;
395 open_forcefeedback();
400 Time duration(cfg_retry_interval_);
416 if (new_data_ || just_connected_) {
417 just_connected_ =
false;
457 return joystick_name_;
466 if (!safety_lockout_) {
467 return pressed_buttons_;
468 }
else if (pressed_buttons_ & cfg_safety_bypass_button_mask_) {
469 return pressed_buttons_ & cfg_safety_bypass_button_mask_;
481 if (safety_lockout_) {
482 memset(axis_values_, 0, axis_array_size_ *
sizeof(
float));