00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "acquisition_thread.h"
00024
00025 #include <core/threading/mutex.h>
00026 #include <core/exceptions/system.h>
00027
00028 #include <algorithm>
00029 #include <linux/joystick.h>
00030 #include <cstdlib>
00031 #include <sys/types.h>
00032 #include <sys/stat.h>
00033 #include <fcntl.h>
00034 #include <cerrno>
00035 #include <cstring>
00036
00037 using namespace fawkes;
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062 JoystickBlackBoardHandler::~JoystickBlackBoardHandler()
00063 {
00064 }
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074 JoystickAcquisitionThread::JoystickAcquisitionThread()
00075 : Thread("JoystickAcquisitionThread", Thread::OPMODE_CONTINUOUS)
00076 {
00077 set_prepfin_conc_loop(true);
00078 __data_mutex = NULL;
00079 __axis_x_values = NULL;
00080 __axis_y_values = NULL;
00081 }
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091 JoystickAcquisitionThread::JoystickAcquisitionThread(const char *device_file,
00092 JoystickBlackBoardHandler *handler,
00093 Logger *logger)
00094 : Thread("JoystickAcquisitionThread", Thread::OPMODE_CONTINUOUS)
00095 {
00096 __data_mutex = NULL;
00097 __axis_x_values = NULL;
00098 __axis_y_values = NULL;
00099 __bbhandler = handler;
00100 this->logger = logger;
00101 init(device_file);
00102 }
00103
00104
00105 void
00106 JoystickAcquisitionThread::init()
00107 {
00108 try {
00109 __cfg_device_file = config->get_string("/hardware/joystick/device_file");
00110
00111 } catch (Exception &e) {
00112 e.append("Could not read all required config values for %s", name());
00113 throw;
00114 }
00115
00116 init(__cfg_device_file);
00117 }
00118
00119
00120 void
00121 JoystickAcquisitionThread::open_joystick()
00122 {
00123 __fd = open(__cfg_device_file.c_str(), O_RDONLY);
00124 if ( __fd == -1 ) {
00125 throw CouldNotOpenFileException(__cfg_device_file.c_str(), errno,
00126 "Opening the joystick device file failed");
00127 }
00128
00129 if ( ioctl(__fd, JSIOCGNAME(sizeof(__joystick_name)), __joystick_name) < 0) {
00130 throw Exception(errno, "Failed to get name of joystick");
00131 }
00132 if ( ioctl(__fd, JSIOCGAXES, &__num_axes) < 0 ) {
00133 throw Exception(errno, "Failed to get number of axes for joystick");
00134 }
00135 if ( ioctl(__fd, JSIOCGBUTTONS, &__num_buttons) < 0 ) {
00136 throw Exception(errno, "Failed to get number of buttons for joystick");
00137 }
00138
00139 __num_axes = (__num_axes / 2) + (__num_axes % 2);
00140
00141 if ( (__axis_x_values == NULL) && (__axis_y_values == NULL) ) {
00142
00143
00144 __axis_array_size = std::max((int)__num_axes, 4);
00145 __axis_x_values = (float *)malloc(sizeof(float) * __axis_array_size);
00146 __axis_y_values = (float *)malloc(sizeof(float) * __axis_array_size);
00147 } else if ( __num_axes > std::max((int)__axis_array_size, 4) ) {
00148
00149 __num_axes = __axis_array_size;
00150 }
00151
00152 logger->log_debug(name(), "Joystick device: %s", __cfg_device_file.c_str());
00153 logger->log_debug(name(), "Joystick name: %s", __joystick_name);
00154 logger->log_debug(name(), "Number of Axes: %i", __num_axes);
00155 logger->log_debug(name(), "Number of Buttons: %i", __num_buttons);
00156 logger->log_debug(name(), "Axis Array Size: %u", __axis_array_size);
00157
00158 memset(__axis_x_values, 0, sizeof(float) * __axis_array_size);
00159 memset(__axis_y_values, 0, sizeof(float) * __axis_array_size);
00160 __pressed_buttons = 0;
00161
00162 if ( __bbhandler ) {
00163 __bbhandler->joystick_plugged(__num_axes, __num_buttons);
00164 }
00165 __connected = true;
00166 }
00167
00168 void
00169 JoystickAcquisitionThread::init(std::string device_file)
00170 {
00171 __new_data = false;
00172 __cfg_device_file = device_file;
00173 open_joystick();
00174 __data_mutex = new Mutex();
00175
00176 }
00177
00178
00179 void
00180 JoystickAcquisitionThread::finalize()
00181 {
00182 if ( __fd >= 0 ) close(__fd);
00183 free(__axis_x_values);
00184 free(__axis_y_values);
00185 delete __data_mutex;
00186 }
00187
00188
00189 void
00190 JoystickAcquisitionThread::loop()
00191 {
00192 if ( __connected ) {
00193 struct js_event e;
00194
00195 if ( read(__fd, &e, sizeof(struct js_event)) < (int)sizeof(struct js_event) ) {
00196 logger->log_warn(name(), "Joystick removed, will try to reconnect.");
00197 close(__fd);
00198 __fd = -1;
00199 __connected = false;
00200 if ( __bbhandler ) {
00201 __bbhandler->joystick_unplugged();
00202 }
00203 return;
00204 }
00205
00206 __data_mutex->lock();
00207 __new_data = true;
00208
00209 if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON) {
00210
00211 if (e.number <= 32) {
00212 if (e.value) {
00213 __pressed_buttons |= (1 << e.number);
00214 } else {
00215 __pressed_buttons &= ~(1 << e.number);
00216 }
00217 } else {
00218 logger->log_warn(name(), "Button value for button > 32, ignoring");
00219 }
00220 } else if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_AXIS) {
00221 unsigned int axis_index = e.number / 2;
00222 if ( axis_index >= __axis_array_size ) {
00223 logger->log_warn(name(), "Got value for axis %u, but only %u axes registered. "
00224 "Plugged in a different joystick? Ignoring.",
00225 axis_index + 1 , __axis_array_size);
00226 } else {
00227 if ( (e.number % 2) == 0 ) {
00228 __axis_x_values[axis_index] = e.value / 32767.f;
00229
00230 } else {
00231 __axis_y_values[axis_index] = e.value / 32767.f;
00232
00233 }
00234 }
00235 }
00236
00237 __data_mutex->unlock();
00238
00239 if ( __bbhandler ) {
00240 __bbhandler->joystick_changed(__pressed_buttons, __axis_x_values, __axis_y_values);
00241 }
00242 } else {
00243
00244 try {
00245 open_joystick();
00246 logger->log_warn(name(), "Joystick plugged in. Delivering data again.");
00247 } catch (...) {
00248
00249 }
00250 }
00251 }
00252
00253
00254
00255
00256
00257
00258
00259
00260 bool
00261 JoystickAcquisitionThread::lock_if_new_data()
00262 {
00263 __data_mutex->lock();
00264 if (__new_data) {
00265 return true;
00266 } else {
00267 __data_mutex->unlock();
00268 return false;
00269 }
00270 }
00271
00272
00273
00274 void
00275 JoystickAcquisitionThread::unlock()
00276 {
00277 __new_data = false;
00278 __data_mutex->unlock();
00279 }
00280
00281
00282
00283
00284
00285 char
00286 JoystickAcquisitionThread::num_axes() const
00287 {
00288 return __num_axes;
00289 }
00290
00291
00292
00293
00294
00295 char
00296 JoystickAcquisitionThread::num_buttons() const
00297 {
00298 return __num_buttons;
00299 }
00300
00301
00302
00303
00304
00305 const char *
00306 JoystickAcquisitionThread::joystick_name() const
00307 {
00308 return __joystick_name;
00309 }
00310
00311
00312
00313
00314
00315 unsigned int
00316 JoystickAcquisitionThread::pressed_buttons() const
00317 {
00318 return __pressed_buttons;
00319 }
00320
00321
00322
00323
00324
00325 float *
00326 JoystickAcquisitionThread::axis_x_values()
00327 {
00328 return __axis_x_values;
00329 }
00330
00331
00332
00333
00334
00335 float *
00336 JoystickAcquisitionThread::axis_y_values()
00337 {
00338 return __axis_y_values;
00339 }