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 
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 }

Generated on Tue Feb 22 13:31:15 2011 for Fawkes API by  doxygen 1.4.7