Fawkes API
Fawkes Development Version
|
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 <fvcams/nao.h> 00026 #include <fvutils/system/camargp.h> 00027 #include <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 <fvcams/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