23 #include "acquisition_thread.h" 24 #include "force_feedback.h" 26 #include <core/threading/mutex.h> 27 #include <core/exceptions/system.h> 29 #include <utils/time/time.h> 32 #include <linux/joystick.h> 34 #include <sys/types.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 58 :
Thread(
"JoystickAcquisitionThread",
Thread::OPMODE_CONTINUOUS)
97 e.
append(
"Could not read all required config values for %s",
name());
101 safety_lockout_ =
true;
103 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_ =
config->
get_uint(
"/hardware/joystick/safety_lockout/bypass-button-mask");
113 for (
int i = 0; i < 5; ++i) safety_combo_[i] =
false;
115 init(cfg_device_file_);
117 if (safety_lockout_) {
119 "directions while holding first button. Then let go of button.");
125 JoystickAcquisitionThread::open_joystick()
127 fd_ = open(cfg_device_file_.c_str(), O_RDONLY);
130 "Opening the joystick device file failed");
133 if ( ioctl(fd_, JSIOCGNAME(
sizeof(joystick_name_)), joystick_name_) < 0) {
134 throw Exception(errno,
"Failed to get name of joystick");
136 if ( ioctl(fd_, JSIOCGAXES, &num_axes_) < 0 ) {
137 throw Exception(errno,
"Failed to get number of axes for joystick");
139 if ( ioctl(fd_, JSIOCGBUTTONS, &num_buttons_) < 0 ) {
140 throw Exception(errno,
"Failed to get number of buttons for joystick");
143 if (axis_values_ == NULL) {
146 axis_array_size_ = std::max((
int)num_axes_, 8);
147 axis_values_ = (
float *)malloc(
sizeof(
float) * axis_array_size_);
148 }
else if ( num_axes_ > std::max((
int)axis_array_size_, 8) ) {
150 num_axes_ = axis_array_size_;
159 memset(axis_values_, 0,
sizeof(
float) * axis_array_size_);
160 pressed_buttons_ = 0;
166 just_connected_ =
true;
170 JoystickAcquisitionThread::open_forcefeedback()
196 cfg_device_file_ = device_file;
199 open_forcefeedback();
204 data_mutex_ =
new Mutex();
212 if ( fd_ >= 0 ) close(fd_);
213 if (axis_values_) free(axis_values_);
224 long int timeout_sec = (
long int)truncf(cfg_safety_lockout_timeout_);
225 long int timeout_usec = (cfg_safety_lockout_timeout_ - timeout_sec) * 10000000;
226 timeval timeout = {timeout_sec, timeout_usec};
230 FD_SET(fd_, &read_fds);
233 rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
236 if (! safety_lockout_) {
238 cfg_safety_lockout_timeout_);
239 safety_lockout_ =
true;
240 for (
int i = 0; i < 5; ++i) safety_combo_[i] =
false;
246 if (rv == -1 || read(fd_, &e,
sizeof(
struct js_event)) < (
int)
sizeof(
struct js_event)) {
251 just_connected_ =
false;
252 safety_lockout_ =
true;
262 new_data_ = ! safety_lockout_;
263 unsigned int last_pressed_buttons = pressed_buttons_;
265 if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON) {
267 if (e.number <= 32) {
269 pressed_buttons_ |= (1 << e.number);
271 pressed_buttons_ &= ~(1 << e.number);
276 }
else if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_AXIS) {
277 if ( e.number >= axis_array_size_ ) {
279 "Got value for axis %u, but only %u axes registered. " 280 "Plugged in a different joystick? Ignoring.",
281 e.number + 1 , axis_array_size_);
286 axis_values_[e.number] = (e.value == 0) ? 0. : (e.value / -32767.f);
296 if (safety_lockout_ &&
297 ((cfg_safety_bypass_button_mask_ & pressed_buttons_) ||
298 ((cfg_safety_bypass_button_mask_ & last_pressed_buttons) && pressed_buttons_ == 0)))
305 if (safety_lockout_) {
308 if (num_axes_ < 2 || num_buttons_ == 0) {
309 safety_combo_[COMBO_IDX_UP] =
true;
310 safety_combo_[COMBO_IDX_DOWN] =
true;
311 safety_combo_[COMBO_IDX_RIGHT] =
true;
312 safety_combo_[COMBO_IDX_LEFT] =
true;
313 safety_combo_[COMBO_IDX_RELEASE] =
true;
315 if (pressed_buttons_ & cfg_safety_button_mask_) {
316 if (axis_values_[0] > 0.9) safety_combo_[COMBO_IDX_UP] =
true;
317 if (axis_values_[0] < -0.9) safety_combo_[COMBO_IDX_DOWN] =
true;
318 if (axis_values_[1] > 0.9) safety_combo_[COMBO_IDX_RIGHT] =
true;
319 if (axis_values_[1] < -0.9) safety_combo_[COMBO_IDX_LEFT] =
true;
321 if (safety_combo_[COMBO_IDX_UP] && safety_combo_[COMBO_IDX_DOWN] &&
322 safety_combo_[COMBO_IDX_LEFT] && safety_combo_[COMBO_IDX_RIGHT] &&
323 pressed_buttons_ == 0) {
324 safety_combo_[COMBO_IDX_RELEASE] =
true;
328 if (safety_combo_[COMBO_IDX_UP] && safety_combo_[COMBO_IDX_DOWN] &&
329 safety_combo_[COMBO_IDX_LEFT] && safety_combo_[COMBO_IDX_RIGHT] &&
330 safety_combo_[COMBO_IDX_RELEASE])
333 safety_lockout_ =
false;
346 open_forcefeedback();
367 if (new_data_ || just_connected_) {
368 just_connected_ =
false;
412 return joystick_name_;
422 if (! safety_lockout_) {
423 return pressed_buttons_;
424 }
else if (pressed_buttons_ & cfg_safety_bypass_button_mask_) {
425 return pressed_buttons_ & cfg_safety_bypass_button_mask_;
438 if (safety_lockout_) {
439 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.
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.
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.
operate in continuous mode (default)
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.