Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
acquisition_thread.cpp
1 
2 /***************************************************************************
3  * acqusition_thread.cpp - Thread that retrieves the joystick data
4  *
5  * Created: Sat Nov 22 18:14:55 2008
6  * Copyright 2008 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "acquisition_thread.h"
24 #include "force_feedback.h"
25 
26 #include <core/threading/mutex.h>
27 #include <core/exceptions/system.h>
28 
29 #include <algorithm>
30 #include <linux/joystick.h>
31 #include <cstdlib>
32 #include <sys/types.h>
33 #include <sys/stat.h>
34 #include <fcntl.h>
35 #include <cerrno>
36 #include <cstring>
37 #include <unistd.h>
38 
39 using namespace fawkes;
40 
41 
42 /** @class JoystickAcquisitionThread "acquisition_thread.h"
43  * Joystick acqusition thread for Linux joystick API.
44  * @see Linux Kernel Documentation (joystick-api.txt)
45  * @author Tim Niemueller
46  */
47 
48 /** Constructor. */
50  : Thread("JoystickAcquisitionThread", Thread::OPMODE_CONTINUOUS)
51 {
53  __data_mutex = NULL;
54  __axis_values = NULL;
55  __bbhandler = NULL;
56  __ff = NULL;
57  logger = NULL;
58 }
59 
60 
61 /** Alternative constructor.
62  * This constructor is meant to be used to create an instance that is used
63  * outside of Fawkes.
64  * @param device_file joystick device file
65  * @param handler BlackBoard handler that will post data to the BlackBoard
66  * @param logger logging instance
67  */
70  Logger *logger)
71  : Thread("JoystickAcquisitionThread", Thread::OPMODE_CONTINUOUS)
72 {
74  __data_mutex = NULL;
75  __axis_values = NULL;
76  __ff = NULL;
77  __bbhandler = handler;
78  this->logger = logger;
79  init(device_file);
80 }
81 
82 
83 void
85 {
86  try {
87  __cfg_device_file = config->get_string("/hardware/joystick/device_file");
88 
89  } catch (Exception &e) {
90  e.append("Could not read all required config values for %s", name());
91  throw;
92  }
93 
94  init(__cfg_device_file);
95 }
96 
97 
98 void
99 JoystickAcquisitionThread::open_joystick()
100 {
101  __fd = open(__cfg_device_file.c_str(), O_RDONLY);
102  if ( __fd == -1 ) {
103  throw CouldNotOpenFileException(__cfg_device_file.c_str(), errno,
104  "Opening the joystick device file failed");
105  }
106 
107  if ( ioctl(__fd, JSIOCGNAME(sizeof(__joystick_name)), __joystick_name) < 0) {
108  throw Exception(errno, "Failed to get name of joystick");
109  }
110  if ( ioctl(__fd, JSIOCGAXES, &__num_axes) < 0 ) {
111  throw Exception(errno, "Failed to get number of axes for joystick");
112  }
113  if ( ioctl(__fd, JSIOCGBUTTONS, &__num_buttons) < 0 ) {
114  throw Exception(errno, "Failed to get number of buttons for joystick");
115  }
116 
117  if (__axis_values == NULL) {
118  // memory had not been allocated
119  // minimum of 8 because there are 8 axes in the interface
120  __axis_array_size = std::max((int)__num_axes, 8);
121  __axis_values = (float *)malloc(sizeof(float) * __axis_array_size);
122  } else if ( __num_axes > std::max((int)__axis_array_size, 8) ) {
123  // We loose axes as we cannot increase BB interface on-the-fly
124  __num_axes = __axis_array_size;
125  }
126 
127  logger->log_debug(name(), "Joystick device: %s", __cfg_device_file.c_str());
128  logger->log_debug(name(), "Joystick name: %s", __joystick_name);
129  logger->log_debug(name(), "Number of Axes: %i", __num_axes);
130  logger->log_debug(name(), "Number of Buttons: %i", __num_buttons);
131  logger->log_debug(name(), "Axis Array Size: %u", __axis_array_size);
132 
133  memset(__axis_values, 0, sizeof(float) * __axis_array_size);
134  __pressed_buttons = 0;
135 
136  if ( __bbhandler ) {
137  __bbhandler->joystick_plugged(__num_axes, __num_buttons);
138  }
139  __connected = true;
140 }
141 
142 void
143 JoystickAcquisitionThread::open_forcefeedback()
144 {
145  __ff = new JoystickForceFeedback(__joystick_name);
146  logger->log_debug(name(), "Force Feedback: %s", (__ff) ? "Yes" : "No");
147  logger->log_debug(name(), "Supported effects:");
148 
149  if (__ff->can_rumble()) logger->log_debug(name(), " rumble");
150  if (__ff->can_periodic()) logger->log_debug(name(), " periodic");
151  if (__ff->can_constant()) logger->log_debug(name(), " constant");
152  if (__ff->can_spring()) logger->log_debug(name(), " spring");
153  if (__ff->can_friction()) logger->log_debug(name(), " friction");
154  if (__ff->can_damper()) logger->log_debug(name(), " damper");
155  if (__ff->can_inertia()) logger->log_debug(name(), " inertia");
156  if (__ff->can_ramp()) logger->log_debug(name(), " ramp");
157  if (__ff->can_square()) logger->log_debug(name(), " square");
158  if (__ff->can_triangle()) logger->log_debug(name(), " triangle");
159  if (__ff->can_sine()) logger->log_debug(name(), " sine");
160  if (__ff->can_saw_up()) logger->log_debug(name(), " saw up");
161  if (__ff->can_saw_down()) logger->log_debug(name(), " saw down");
162  if (__ff->can_custom()) logger->log_debug(name(), " custom");
163 }
164 
165 void
166 JoystickAcquisitionThread::init(std::string device_file)
167 {
168  __new_data = false;
169  __cfg_device_file = device_file;
170  open_joystick();
171  try {
172  open_forcefeedback();
173  } catch (Exception &e) {
174  logger->log_warn(name(), "Initializing force feedback failed, disabling");
175  logger->log_warn(name(), e);
176  }
177  __data_mutex = new Mutex();
178 }
179 
180 
181 void
183 {
184  if ( __fd >= 0 ) close(__fd);
185  free(__axis_values);
186  delete __data_mutex;
187 }
188 
189 
190 void
192 {
193  if ( __connected ) {
194  struct js_event e;
195 
196  if ( read(__fd, &e, sizeof(struct js_event)) < (int)sizeof(struct js_event) ) {
197  logger->log_warn(name(), "Joystick removed, will try to reconnect.");
198  close(__fd);
199  __fd = -1;
200  __connected = false;
201  if ( __bbhandler ) {
202  __bbhandler->joystick_unplugged();
203  }
204  return;
205  }
206 
207  __data_mutex->lock();
208  __new_data = true;
209 
210  if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON) {
211  //logger->log_debug(name(), "Button %u button event: %f", e.number, e.value);
212  if (e.number <= 32) {
213  if (e.value) {
214  __pressed_buttons |= (1 << e.number);
215  } else {
216  __pressed_buttons &= ~(1 << e.number);
217  }
218  } else {
219  logger->log_warn(name(), "Button value for button > 32, ignoring");
220  }
221  } else if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_AXIS) {
222  if ( e.number >= __axis_array_size ) {
223  logger->log_warn(name(),
224  "Got value for axis %u, but only %u axes registered. "
225  "Plugged in a different joystick? Ignoring.",
226  e.number + 1 /* natural numbering */, __axis_array_size);
227  } else {
228  // Joystick axes usually go positive right, down, twist right, min speed,
229  // hat right, and hat down. In the Fawkes coordinate system we actually
230  // want opposite directions, hence multiply each value by -1
231  __axis_values[e.number] = (e.value == 0) ? 0. : (e.value / -32767.f);
232 
233  //logger->log_debug(name(), "Axis %u new X: %f",
234  // axis_index, __axis_values[e.number]);
235  }
236  }
237 
238  __data_mutex->unlock();
239 
240  if ( __bbhandler ) {
241  __bbhandler->joystick_changed(__pressed_buttons, __axis_values);
242  }
243  } else {
244  // Connection to joystick has been lost
245  try {
246  open_joystick();
247  logger->log_warn(name(), "Joystick plugged in. Delivering data again.");
248  try {
249  open_forcefeedback();
250  } catch (Exception &e) {
251  logger->log_warn(name(), "Initializing force feedback failed, disabling");
252  }
253  } catch (...) {
254  // ignored
255  }
256  }
257 }
258 
259 
260 /** Lock data if fresh.
261  * If new data has been received since get_distance_data() or get_echo_data()
262  * was called last the data is locked, no new data can arrive until you call
263  * unlock(), otherwise the lock is immediately released after checking.
264  * @return true if the lock was acquired and there is new data, false otherwise
265  */
266 bool
268 {
269  __data_mutex->lock();
270  if (__new_data) {
271  return true;
272  } else {
273  __data_mutex->unlock();
274  return false;
275  }
276 }
277 
278 
279 /** Unlock data. */
280 void
282 {
283  __new_data = false;
284  __data_mutex->unlock();
285 }
286 
287 
288 /** Get number of axes.
289  * @return number of axes.
290  */
291 char
293 {
294  return __num_axes;
295 }
296 
297 
298 /** Get number of buttons.
299  * @return number of buttons.
300  */
301 char
303 {
304  return __num_buttons;
305 }
306 
307 
308 /** Get joystick name.
309  * @return joystick name
310  */
311 const char *
313 {
314  return __joystick_name;
315 }
316 
317 
318 /** Pressed buttons.
319  * @return bit field where each set bit represents a pressed button.
320  */
321 unsigned int
323 {
324  return __pressed_buttons;
325 }
326 
327 
328 /** Get values for the axes.
329  * @return array of axis values.
330  */
331 float *
333 {
334  return __axis_values;
335 }