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 #include "force_feedback.h" 00025 00026 #include <core/threading/mutex.h> 00027 #include <core/exceptions/system.h> 00028 00029 #include <algorithm> 00030 #include <linux/joystick.h> 00031 #include <cstdlib> 00032 #include <sys/types.h> 00033 #include <sys/stat.h> 00034 #include <fcntl.h> 00035 #include <cerrno> 00036 #include <cstring> 00037 #include <unistd.h> 00038 00039 using namespace fawkes; 00040 00041 00042 /** @class JoystickAcquisitionThread "acquisition_thread.h" 00043 * Joystick acqusition thread for Linux joystick API. 00044 * @see Linux Kernel Documentation (joystick-api.txt) 00045 * @author Tim Niemueller 00046 */ 00047 00048 /** Constructor. */ 00049 JoystickAcquisitionThread::JoystickAcquisitionThread() 00050 : Thread("JoystickAcquisitionThread", Thread::OPMODE_CONTINUOUS) 00051 { 00052 set_prepfin_conc_loop(true); 00053 __data_mutex = NULL; 00054 __axis_values = NULL; 00055 __bbhandler = NULL; 00056 __ff = NULL; 00057 logger = NULL; 00058 } 00059 00060 00061 /** Alternative constructor. 00062 * This constructor is meant to be used to create an instance that is used 00063 * outside of Fawkes. 00064 * @param device_file joystick device file 00065 * @param handler BlackBoard handler that will post data to the BlackBoard 00066 * @param logger logging instance 00067 */ 00068 JoystickAcquisitionThread::JoystickAcquisitionThread(const char *device_file, 00069 JoystickBlackBoardHandler *handler, 00070 Logger *logger) 00071 : Thread("JoystickAcquisitionThread", Thread::OPMODE_CONTINUOUS) 00072 { 00073 set_prepfin_conc_loop(true); 00074 __data_mutex = NULL; 00075 __axis_values = NULL; 00076 __ff = NULL; 00077 __bbhandler = handler; 00078 this->logger = logger; 00079 init(device_file); 00080 } 00081 00082 00083 void 00084 JoystickAcquisitionThread::init() 00085 { 00086 try { 00087 __cfg_device_file = config->get_string("/hardware/joystick/device_file"); 00088 00089 } catch (Exception &e) { 00090 e.append("Could not read all required config values for %s", name()); 00091 throw; 00092 } 00093 00094 init(__cfg_device_file); 00095 } 00096 00097 00098 void 00099 JoystickAcquisitionThread::open_joystick() 00100 { 00101 __fd = open(__cfg_device_file.c_str(), O_RDONLY); 00102 if ( __fd == -1 ) { 00103 throw CouldNotOpenFileException(__cfg_device_file.c_str(), errno, 00104 "Opening the joystick device file failed"); 00105 } 00106 00107 if ( ioctl(__fd, JSIOCGNAME(sizeof(__joystick_name)), __joystick_name) < 0) { 00108 throw Exception(errno, "Failed to get name of joystick"); 00109 } 00110 if ( ioctl(__fd, JSIOCGAXES, &__num_axes) < 0 ) { 00111 throw Exception(errno, "Failed to get number of axes for joystick"); 00112 } 00113 if ( ioctl(__fd, JSIOCGBUTTONS, &__num_buttons) < 0 ) { 00114 throw Exception(errno, "Failed to get number of buttons for joystick"); 00115 } 00116 00117 if (__axis_values == NULL) { 00118 // memory had not been allocated 00119 // minimum of 8 because there are 8 axes in the interface 00120 __axis_array_size = std::max((int)__num_axes, 8); 00121 __axis_values = (float *)malloc(sizeof(float) * __axis_array_size); 00122 } else if ( __num_axes > std::max((int)__axis_array_size, 8) ) { 00123 // We loose axes as we cannot increase BB interface on-the-fly 00124 __num_axes = __axis_array_size; 00125 } 00126 00127 logger->log_debug(name(), "Joystick device: %s", __cfg_device_file.c_str()); 00128 logger->log_debug(name(), "Joystick name: %s", __joystick_name); 00129 logger->log_debug(name(), "Number of Axes: %i", __num_axes); 00130 logger->log_debug(name(), "Number of Buttons: %i", __num_buttons); 00131 logger->log_debug(name(), "Axis Array Size: %u", __axis_array_size); 00132 00133 memset(__axis_values, 0, sizeof(float) * __axis_array_size); 00134 __pressed_buttons = 0; 00135 00136 if ( __bbhandler ) { 00137 __bbhandler->joystick_plugged(__num_axes, __num_buttons); 00138 } 00139 __connected = true; 00140 } 00141 00142 void 00143 JoystickAcquisitionThread::open_forcefeedback() 00144 { 00145 __ff = new JoystickForceFeedback(__joystick_name); 00146 logger->log_debug(name(), "Force Feedback: %s", (__ff) ? "Yes" : "No"); 00147 logger->log_debug(name(), "Supported effects:"); 00148 00149 if (__ff->can_rumble()) logger->log_debug(name(), " rumble"); 00150 if (__ff->can_periodic()) logger->log_debug(name(), " periodic"); 00151 if (__ff->can_constant()) logger->log_debug(name(), " constant"); 00152 if (__ff->can_spring()) logger->log_debug(name(), " spring"); 00153 if (__ff->can_friction()) logger->log_debug(name(), " friction"); 00154 if (__ff->can_damper()) logger->log_debug(name(), " damper"); 00155 if (__ff->can_inertia()) logger->log_debug(name(), " inertia"); 00156 if (__ff->can_ramp()) logger->log_debug(name(), " ramp"); 00157 if (__ff->can_square()) logger->log_debug(name(), " square"); 00158 if (__ff->can_triangle()) logger->log_debug(name(), " triangle"); 00159 if (__ff->can_sine()) logger->log_debug(name(), " sine"); 00160 if (__ff->can_saw_up()) logger->log_debug(name(), " saw up"); 00161 if (__ff->can_saw_down()) logger->log_debug(name(), " saw down"); 00162 if (__ff->can_custom()) logger->log_debug(name(), " custom"); 00163 } 00164 00165 void 00166 JoystickAcquisitionThread::init(std::string device_file) 00167 { 00168 __new_data = false; 00169 __cfg_device_file = device_file; 00170 open_joystick(); 00171 try { 00172 open_forcefeedback(); 00173 } catch (Exception &e) { 00174 logger->log_warn(name(), "Initializing force feedback failed, disabling"); 00175 logger->log_warn(name(), e); 00176 } 00177 __data_mutex = new Mutex(); 00178 } 00179 00180 00181 void 00182 JoystickAcquisitionThread::finalize() 00183 { 00184 if ( __fd >= 0 ) close(__fd); 00185 free(__axis_values); 00186 delete __data_mutex; 00187 } 00188 00189 00190 void 00191 JoystickAcquisitionThread::loop() 00192 { 00193 if ( __connected ) { 00194 struct js_event e; 00195 00196 if ( read(__fd, &e, sizeof(struct js_event)) < (int)sizeof(struct js_event) ) { 00197 logger->log_warn(name(), "Joystick removed, will try to reconnect."); 00198 close(__fd); 00199 __fd = -1; 00200 __connected = false; 00201 if ( __bbhandler ) { 00202 __bbhandler->joystick_unplugged(); 00203 } 00204 return; 00205 } 00206 00207 __data_mutex->lock(); 00208 __new_data = true; 00209 00210 if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON) { 00211 //logger->log_debug(name(), "Button %u button event: %f", e.number, e.value); 00212 if (e.number <= 32) { 00213 if (e.value) { 00214 __pressed_buttons |= (1 << e.number); 00215 } else { 00216 __pressed_buttons &= ~(1 << e.number); 00217 } 00218 } else { 00219 logger->log_warn(name(), "Button value for button > 32, ignoring"); 00220 } 00221 } else if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_AXIS) { 00222 if ( e.number >= __axis_array_size ) { 00223 logger->log_warn(name(), 00224 "Got value for axis %u, but only %u axes registered. " 00225 "Plugged in a different joystick? Ignoring.", 00226 e.number + 1 /* natural numbering */, __axis_array_size); 00227 } else { 00228 // Joystick axes usually go positive right, down, twist right, min speed, 00229 // hat right, and hat down. In the Fawkes coordinate system we actually 00230 // want opposite directions, hence multiply each value by -1 00231 __axis_values[e.number] = (e.value == 0) ? 0. : (e.value / -32767.f); 00232 00233 //logger->log_debug(name(), "Axis %u new X: %f", 00234 // axis_index, __axis_values[e.number]); 00235 } 00236 } 00237 00238 __data_mutex->unlock(); 00239 00240 if ( __bbhandler ) { 00241 __bbhandler->joystick_changed(__pressed_buttons, __axis_values); 00242 } 00243 } else { 00244 // Connection to joystick has been lost 00245 try { 00246 open_joystick(); 00247 logger->log_warn(name(), "Joystick plugged in. Delivering data again."); 00248 try { 00249 open_forcefeedback(); 00250 } catch (Exception &e) { 00251 logger->log_warn(name(), "Initializing force feedback failed, disabling"); 00252 } 00253 } catch (...) { 00254 // ignored 00255 } 00256 } 00257 } 00258 00259 00260 /** Lock data if fresh. 00261 * If new data has been received since get_distance_data() or get_echo_data() 00262 * was called last the data is locked, no new data can arrive until you call 00263 * unlock(), otherwise the lock is immediately released after checking. 00264 * @return true if the lock was acquired and there is new data, false otherwise 00265 */ 00266 bool 00267 JoystickAcquisitionThread::lock_if_new_data() 00268 { 00269 __data_mutex->lock(); 00270 if (__new_data) { 00271 return true; 00272 } else { 00273 __data_mutex->unlock(); 00274 return false; 00275 } 00276 } 00277 00278 00279 /** Unlock data. */ 00280 void 00281 JoystickAcquisitionThread::unlock() 00282 { 00283 __new_data = false; 00284 __data_mutex->unlock(); 00285 } 00286 00287 00288 /** Get number of axes. 00289 * @return number of axes. 00290 */ 00291 char 00292 JoystickAcquisitionThread::num_axes() const 00293 { 00294 return __num_axes; 00295 } 00296 00297 00298 /** Get number of buttons. 00299 * @return number of buttons. 00300 */ 00301 char 00302 JoystickAcquisitionThread::num_buttons() const 00303 { 00304 return __num_buttons; 00305 } 00306 00307 00308 /** Get joystick name. 00309 * @return joystick name 00310 */ 00311 const char * 00312 JoystickAcquisitionThread::joystick_name() const 00313 { 00314 return __joystick_name; 00315 } 00316 00317 00318 /** Pressed buttons. 00319 * @return bit field where each set bit represents a pressed button. 00320 */ 00321 unsigned int 00322 JoystickAcquisitionThread::pressed_buttons() const 00323 { 00324 return __pressed_buttons; 00325 } 00326 00327 00328 /** Get values for the axes. 00329 * @return array of axis values. 00330 */ 00331 float * 00332 JoystickAcquisitionThread::axis_values() 00333 { 00334 return __axis_values; 00335 }