Fawkes API  Fawkes Development Version
acquisition_thread.cpp
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 }