Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * acqusition_thread.cpp - Thread that retrieves the joystick data 00004 * 00005 * Created: Sat Nov 22 18:14:55 2008 00006 * Copyright 2008 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 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 /** @class JoystickBlackBoardHandler "acquisition_thread.h" 00040 * Handler class for joystick data. 00041 * This interface allows to plug a generic handler to the 00042 * JoystickAcquisitionThread via the alternative constructor. This can be 00043 * used to directly instantiate the acquisition thread outside of Fawkes. 00044 * @author Tim Niemueller 00045 * 00046 * @fn void JoystickBlackBoardHandler::joystick_changed(unsigned int pressed_buttons, float *axis_x_values, float *axis_y_values) = 0 00047 * Joystick data changed. 00048 * @param pressed_buttons the new pressed_buttons array 00049 * @param axis_x_values array of X axis values, the length is at least num_axes() 00050 * @param axis_y_values array of Y axis values, the length is at least num_axes() 00051 * 00052 * @fn void JoystickBlackBoardHandler::joystick_plugged(char num_axes, char num_buttons) 00053 * A (new) joystick has been plugged in 00054 * @param num_axes number of axes 00055 * @param num_buttons number of buttons 00056 * 00057 * @fn void JoystickBlackBoardHandler::joystick_unplugged() 00058 * The joystick has been unplugged and is no longer available. 00059 */ 00060 00061 /** Virtual empty destructor. */ 00062 JoystickBlackBoardHandler::~JoystickBlackBoardHandler() 00063 { 00064 } 00065 00066 /** @class JoystickAcquisitionThread "acquisition_thread.h" 00067 * Joystick acqusition thread for Linux joystick API. 00068 * @see Linux Kernel Documentation (joystick-api.txt) 00069 * @author Tim Niemueller 00070 */ 00071 00072 00073 /** Constructor. */ 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 /** Alternative constructor. 00085 * This constructor is meant to be used to create an instance that is used 00086 * outside of Fawkes. 00087 * @param device_file joystick device file 00088 * @param handler BlackBoard handler that will post data to the BlackBoard 00089 * @param logger logging instance 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 // memory had not been allocated 00143 // minimum of 4 because there are 4 axes in the interface 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 // We loose axes as we cannot increase BB interface on-the-fly 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 //logger->log_debug(name(), "Button %u button event: %f", e.number, e.value); 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 /* natural numbering */, __axis_array_size); 00226 } else { 00227 if ( (e.number % 2) == 0 ) { 00228 __axis_x_values[axis_index] = e.value / 32767.f; 00229 //logger->log_debug(name(), "Axis %u new X: %f", axis_index, __axis_x_values[axis_index]); 00230 } else { 00231 __axis_y_values[axis_index] = e.value / 32767.f; 00232 //logger->log_debug(name(), "Axis %u new Y: %f", axis_index, __axis_y_values[axis_index]); 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 // Connection to joystick has been lost 00244 try { 00245 open_joystick(); 00246 logger->log_warn(name(), "Joystick plugged in. Delivering data again."); 00247 } catch (...) { 00248 // ignored 00249 } 00250 } 00251 } 00252 00253 00254 /** Lock data if fresh. 00255 * If new data has been received since get_distance_data() or get_echo_data() 00256 * was called last the data is locked, no new data can arrive until you call 00257 * unlock(), otherwise the lock is immediately released after checking. 00258 * @return true if the lock was acquired and there is new data, false otherwise 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 /** Unlock data. */ 00274 void 00275 JoystickAcquisitionThread::unlock() 00276 { 00277 __new_data = false; 00278 __data_mutex->unlock(); 00279 } 00280 00281 00282 /** Get number of axes. 00283 * @return number of axes. 00284 */ 00285 char 00286 JoystickAcquisitionThread::num_axes() const 00287 { 00288 return __num_axes; 00289 } 00290 00291 00292 /** Get number of buttons. 00293 * @return number of buttons. 00294 */ 00295 char 00296 JoystickAcquisitionThread::num_buttons() const 00297 { 00298 return __num_buttons; 00299 } 00300 00301 00302 /** Get joystick name. 00303 * @return joystick name 00304 */ 00305 const char * 00306 JoystickAcquisitionThread::joystick_name() const 00307 { 00308 return __joystick_name; 00309 } 00310 00311 00312 /** Pressed buttons. 00313 * @return bit field where each set bit represents a pressed button. 00314 */ 00315 unsigned int 00316 JoystickAcquisitionThread::pressed_buttons() const 00317 { 00318 return __pressed_buttons; 00319 } 00320 00321 00322 /** Get values for X axes. 00323 * @return array of X axis values. 00324 */ 00325 float * 00326 JoystickAcquisitionThread::axis_x_values() 00327 { 00328 return __axis_x_values; 00329 } 00330 00331 00332 /** Get values for Y axes. 00333 * @return array of Y axis values. 00334 */ 00335 float * 00336 JoystickAcquisitionThread::axis_y_values() 00337 { 00338 return __axis_y_values; 00339 }