Fawkes API  Fawkes Development Version
nao.cpp
00001 
00002 /***************************************************************************
00003  *  nao.h - V4L2 camera with Nao specific extensions
00004  *
00005  *  Created: Sun Feb 01 13:57:43 2009
00006  *  Copyright  2008 Tobias Kellner
00007  *             2009 Tim Niemueller [www.niemueller.de]
00008  *
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version. A runtime exception applies to
00015  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00016  *
00017  *  This program is distributed in the hope that it will be useful,
00018  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00019  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00020  *  GNU Library General Public License for more details.
00021  *
00022  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00023  */
00024 
00025 #include <cams/nao.h>
00026 #include <fvutils/system/camargp.h>
00027 #include <utils/logging/liblogger.h>
00028 #include <core/exceptions/software.h>
00029 
00030 #include <fcntl.h>
00031 #include <sys/ioctl.h>
00032 #include <linux/i2c-dev.h>
00033 #include <linux/types.h>
00034 
00035 #include <cstring>
00036 #include <cstdlib>
00037 #include <vector>
00038 
00039 using namespace fawkes;
00040 
00041 #define V4L2_CID_AUTOEXPOSURE           (V4L2_CID_BASE+32)
00042 #define V4L2_CID_CAM_INIT               (V4L2_CID_BASE+33)
00043 #define V4L2_CID_EXPOSURE_CORRECTION    (V4L2_CID_BASE+34)
00044 #define V4L2_CID_AEC_ALGORITHM          (V4L2_CID_BASE+35)
00045 
00046 #ifndef I2C_FUNC_SMBUS_READ_BYTE_DATA
00047 #include <linux/i2c.h>
00048 /// @cond I2C_INTERNALS
00049 
00050 // this is the bare minimum of I2C code required to build this thing without
00051 // the extended i2c.h header from i2c-tools, copied straight from version 3.0.1.
00052 
00053 static inline __s32
00054 i2c_smbus_access(int file, char read_write, __u8 command,
00055                  int size, union i2c_smbus_data *data)
00056 {
00057   struct i2c_smbus_ioctl_data args;
00058 
00059   args.read_write = read_write;
00060   args.command = command;
00061   args.size = size;
00062   args.data = data;
00063   return ioctl(file,I2C_SMBUS,&args);
00064 }
00065 
00066 static inline __s32
00067 i2c_smbus_read_byte_data(int file, __u8 command)
00068 {
00069   union i2c_smbus_data data;
00070   if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
00071                        I2C_SMBUS_BYTE_DATA,&data))
00072     return -1;
00073   else
00074     return 0x0FF & data.byte;
00075 }
00076 
00077 static inline __s32
00078 i2c_smbus_write_block_data(int file, __u8 command,
00079                            __u8 length, __u8 *values)
00080 {
00081   union i2c_smbus_data data;
00082   int i;
00083   if (length > 32)
00084     length = 32;
00085   for (i = 1; i <= length; i++)
00086     data.block[i] = values[i-1];
00087   data.block[0] = length;
00088   return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
00089                           I2C_SMBUS_BLOCK_DATA, &data);
00090 }
00091 
00092 /// @endcond
00093 #endif
00094 
00095 namespace firevision {
00096 #if 0 /* just to make Emacs auto-indent happy */
00097 }
00098 #endif
00099 
00100 /** @class NaoCamera <cams/nao.h>
00101  * Video4Linux 2 camera with Nao-specific extensions.
00102  *
00103  * @author Tobias Kellner
00104  * @author Tim Niemueller
00105  */
00106 
00107 
00108 /** Constructor.
00109  * Initialize camera with parameters from camera argument parser.
00110  * Supported arguments (additionally to V4L2Camera arguments):
00111  * *Required:
00112  * - i2c_device=DEV, i2c device file, for example /dev/i2c-0 (required)
00113  * *Optional:
00114  * - cam=brow/mouth string to identify camera, default is mouth
00115  * @param cap camera argument parser
00116  */
00117 NaoCamera::NaoCamera(const CameraArgumentParser *cap)
00118   : V4L2Camera(cap)
00119 {
00120   if (cap->has("i2c_device")) __i2c_device_name = strdup(cap->get("i2c_device").c_str());
00121   else throw MissingParameterException("NaoCamera: Missing I2C device");
00122 
00123   __can_switch_cam = false;
00124   __cam_id = 2;
00125 
00126   if (cap->has("cam"))
00127   {
00128     if (strcasecmp(cap->get("cam").c_str(), "brow") == 0) __cam_id = 1;
00129   }
00130 
00131   int dev = open_dev(__i2c_device_name);
00132 
00133   // Get dsPIC version (in order to know Nao version)
00134   int val = i2c_smbus_read_byte_data(dev, 170);
00135   if (val == -1) close_dev(dev, "NaoCamera: Error reading dsPic version from I2C");
00136   if (val < 2)
00137   {
00138     LibLogger::log_info("NaoCamera", "Nao V2 found - No camera switching possible");
00139     close_dev(dev);
00140     return;
00141   }
00142   __can_switch_cam = true;
00143   LibLogger::log_debug("NaoCamera", "Nao V3 found - Trying to switch to camera %d", __cam_id);
00144 
00145   val = get_open_cam_id(dev);
00146 
00147   if (val == __cam_id)
00148   {
00149     LibLogger::log_debug("NaoCamera", "Correct camera already chosen");
00150   }
00151   else
00152   {
00153     // Switch to other camera
00154     switch_to_cam_id(dev, __cam_id);
00155   }
00156   close_dev(dev);
00157 
00158   // Connect to the chosen camera and initialize it
00159   // FIXME: Maybe not needed? Try it!
00160   init_cam(_device_name);
00161 }
00162 
00163 NaoCamera::~NaoCamera()
00164 {
00165   free(__i2c_device_name);
00166 }
00167 
00168 /**
00169  * Helper function to open the I2C device
00170  * @param i2c I2C device name
00171  * @return device handle
00172  */
00173 int NaoCamera::open_dev(const char *i2c)
00174 {
00175   // Connect to dsPIC through I2C
00176   int dev = ::open(i2c, O_RDWR);
00177   if (dev < 0) throw Exception("NaoCamera: Error opening I2C for connection to dsPIC");
00178   if (ioctl(dev, I2C_SLAVE, DSPIC_I2C_ADDR) < 0) close_dev(dev, "NaoCamera: Can't connect I2C to dsPIC");
00179   return dev;
00180 }
00181 
00182 /**
00183  * Helper function called when something fails during camera switching.
00184  * Closes the opened device and reports an error (if any) by throwing
00185  * an exception.
00186  *
00187  * @param dev the device to close
00188  * @param error null if no error, an error string otherwise
00189  */
00190 void NaoCamera::close_dev(int dev, const char *error)
00191 {
00192   if (::close(dev) < 0) throw fawkes::Exception("NaoCamera: Error closing device");
00193   if (error) throw fawkes::Exception(error);
00194 }
00195 
00196 /**
00197  * Helper function to get the ID of the currently opened camera
00198  * @param dev I2C device handle
00199  */
00200 int NaoCamera::get_open_cam_id(int dev)
00201 {
00202   // Ask dsPIC which camera is active
00203   int cid = i2c_smbus_read_byte_data(dev, 220);
00204   if (cid == -1) close_dev(dev, "Error reading active cam from I2C");
00205   return cid;
00206 }
00207 
00208 /**
00209  * Helper function to switch to another camera
00210  * @param dev I2C device handle
00211  * @param cam_id ID of the camera to open
00212  */
00213 void NaoCamera::switch_to_cam_id(int dev, int cam_id)
00214 {
00215   unsigned char cmd[2];
00216   cmd[0] = cam_id;
00217   cmd[1] = 0;
00218   int size = i2c_smbus_write_block_data(dev, 220, 1, cmd);
00219   if (size == -1) close_dev(dev, "NaoCamera: Error switching to other camera");
00220 }
00221 
00222 /**
00223  * Helper function to initialize a camera
00224  * @param cam Camera device name
00225  */
00226 void NaoCamera::init_cam(const char *cam)
00227 {
00228   int dev = ::open(cam, O_RDWR);
00229   if (dev < 0)  throw Exception("NaoCamera: Error opening Camera");
00230 
00231   struct v4l2_control control;
00232   memset(&control, 0, sizeof(control));
00233 
00234   control.id    = V4L2_CID_CAM_INIT;
00235   control.value = 0;
00236 
00237   if (ioctl(dev, VIDIOC_S_CTRL, &control)) close_dev(dev, "Error setting other camera to default parameters");
00238 
00239   close_dev(dev);
00240 }
00241 
00242 /**
00243  * Return which cam is currently being used.
00244  * 1: brow-cam
00245  * 2: mouth-cam
00246  * @return ID of camera currently in use
00247  */
00248 unsigned char NaoCamera::source()
00249 {
00250   int dev = open_dev(__i2c_device_name);
00251   __cam_id = get_open_cam_id(dev);
00252   close_dev(dev);
00253 
00254   return static_cast<unsigned char>(__cam_id);
00255 }
00256 
00257 /**
00258  * Switch currently used camera.
00259  * Valid arguments:
00260  * 1: brow-cam
00261  * 2: mouth-cam
00262  * @param source ID of the camera to use
00263  */
00264 void NaoCamera::set_source(unsigned char source)
00265 {
00266   if (source == __cam_id)
00267   {
00268     LibLogger::log_debug("NaoCamera", "Correct camera already chosen");
00269     return;
00270   }
00271 
00272   int dev = open_dev(__i2c_device_name);
00273   switch_to_cam_id(dev, source);
00274   close_dev(dev);
00275   init_cam(_device_name);
00276 }
00277 
00278 /**
00279  * Return whether auto exposure is enabled.
00280  * @return true if auto exposure is enabled
00281  */
00282 bool NaoCamera::auto_exposure()
00283 {
00284   return get_one_control("AEC", V4L2_CID_AUTOEXPOSURE);
00285 }
00286 
00287 /**
00288  * Enable/disable auto exposure.
00289  * @param enabled whether auto exposure should be enabled
00290  */
00291 void NaoCamera::set_auto_exposure(bool enabled)
00292 {
00293   LibLogger::log_debug("NaoCamera", (enabled ? "enabling AEC" : "disabling AEC"));
00294 
00295   set_one_control("AEC", V4L2_CID_AUTOEXPOSURE, (enabled ? 1 : 0));
00296 }
00297 
00298 } // end namespace firevision
00299