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));
unsigned int pressed_buttons() const
Pressed buttons.
File could not be opened.
JoystickAcquisitionThread()
Constructor.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void loop()
Code to execute in the thread.
bool can_ramp()
Check if ramp effect is supported.
float * axis_values()
Get values for the axes.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void unlock()
Unlock the mutex.
bool can_friction()
Check if friction effect is supported.
bool can_triangle()
Check if triangle effect is supported.
Handler class for joystick data.
A class for handling time.
bool can_square()
Check if square effect is supported.
Thread class encapsulation of pthreads.
bool lock_if_new_data()
Lock data if fresh.
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
Logger * logger
This is the Logger member used to access the logger.
const char * joystick_name() const
Get joystick name.
bool can_constant()
Check if constant effect is supported.
bool can_periodic()
Check if periodic effect is supported.
bool can_custom()
Check if custom effect is supported.
virtual void joystick_changed(unsigned int pressed_buttons, float *axis_values)=0
Joystick data changed.
void wait_systime()
Wait (sleep) for this system time.
virtual void joystick_plugged(char num_axes, char num_buttons)=0
A (new) joystick has been plugged in.
Cause force feedback on a joystick.
char num_buttons() const
Get number of buttons.
Base class for exceptions in Fawkes.
virtual void finalize()
Finalize the thread.
bool can_sine()
Check if sine effect is supported.
const char * name() const
Get name of thread.
bool can_damper()
Check if damper effect is supported.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
bool can_saw_down()
Check if downward saw effect is supported.
bool can_spring()
Check if spring effect is supported.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void unlock()
Unlock data.
bool can_rumble()
Check if rumbling effect is supported.
virtual void joystick_unplugged()=0
The joystick has been unplugged and is no longer available.
void lock()
Lock this mutex.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
Mutex mutual exclusion lock.
virtual void init()
Initialize the thread.
char num_axes() const
Get number of axes.
Configuration * config
This is the Configuration member used to access the configuration.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
bool can_inertia()
Check if inertia effect is supported.
void append(const char *format,...)
Append messages to the message list.
bool can_saw_up()
Check if upward saw effect is supported.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.