Fawkes API  Fawkes Development Version
nao.cpp
1 
2 /***************************************************************************
3  * nao.h - V4L2 camera with Nao specific extensions
4  *
5  * Created: Sun Feb 01 13:57:43 2009
6  * Copyright 2008 Tobias Kellner
7  * 2009 Tim Niemueller [www.niemueller.de]
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version. A runtime exception applies to
15  * this software (see LICENSE.GPL_WRE file mentioned below for details).
16  *
17  * This program is distributed in the hope that it will be useful,
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20  * GNU Library General Public License for more details.
21  *
22  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
23  */
24 
25 #include <fvcams/nao.h>
26 #include <fvutils/system/camargp.h>
27 #include <logging/liblogger.h>
28 #include <core/exceptions/software.h>
29 
30 #include <fcntl.h>
31 #include <sys/ioctl.h>
32 #include <linux/i2c-dev.h>
33 #include <linux/types.h>
34 
35 #include <cstring>
36 #include <cstdlib>
37 #include <vector>
38 
39 using namespace fawkes;
40 
41 #define V4L2_CID_AUTOEXPOSURE (V4L2_CID_BASE+32)
42 #define V4L2_CID_CAM_INIT (V4L2_CID_BASE+33)
43 #define V4L2_CID_EXPOSURE_CORRECTION (V4L2_CID_BASE+34)
44 #define V4L2_CID_AEC_ALGORITHM (V4L2_CID_BASE+35)
45 
46 #ifndef I2C_FUNC_SMBUS_READ_BYTE_DATA
47 #include <linux/i2c.h>
48 /// @cond I2C_INTERNALS
49 
50 // this is the bare minimum of I2C code required to build this thing without
51 // the extended i2c.h header from i2c-tools, copied straight from version 3.0.1.
52 
53 static inline __s32
54 i2c_smbus_access(int file, char read_write, __u8 command,
55  int size, union i2c_smbus_data *data)
56 {
57  struct i2c_smbus_ioctl_data args;
58 
59  args.read_write = read_write;
60  args.command = command;
61  args.size = size;
62  args.data = data;
63  return ioctl(file,I2C_SMBUS,&args);
64 }
65 
66 static inline __s32
67 i2c_smbus_read_byte_data(int file, __u8 command)
68 {
69  union i2c_smbus_data data;
70  if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
71  I2C_SMBUS_BYTE_DATA,&data))
72  return -1;
73  else
74  return 0x0FF & data.byte;
75 }
76 
77 static inline __s32
78 i2c_smbus_write_block_data(int file, __u8 command,
79  __u8 length, __u8 *values)
80 {
81  union i2c_smbus_data data;
82  int i;
83  if (length > 32)
84  length = 32;
85  for (i = 1; i <= length; i++)
86  data.block[i] = values[i-1];
87  data.block[0] = length;
88  return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
89  I2C_SMBUS_BLOCK_DATA, &data);
90 }
91 
92 /// @endcond
93 #endif
94 
95 namespace firevision {
96 #if 0 /* just to make Emacs auto-indent happy */
97 }
98 #endif
99 
100 /** @class NaoCamera <fvcams/nao.h>
101  * Video4Linux 2 camera with Nao-specific extensions.
102  *
103  * @author Tobias Kellner
104  * @author Tim Niemueller
105  */
106 
107 
108 /** Constructor.
109  * Initialize camera with parameters from camera argument parser.
110  * Supported arguments (additionally to V4L2Camera arguments):
111  * *Required:
112  * - i2c_device=DEV, i2c device file, for example /dev/i2c-0 (required)
113  * *Optional:
114  * - cam=brow/mouth string to identify camera, default is mouth
115  * @param cap camera argument parser
116  */
117 NaoCamera::NaoCamera(const CameraArgumentParser *cap)
118  : V4L2Camera(cap)
119 {
120  if (cap->has("i2c_device")) __i2c_device_name = strdup(cap->get("i2c_device").c_str());
121  else throw MissingParameterException("NaoCamera: Missing I2C device");
122 
123  __can_switch_cam = false;
124  __cam_id = 2;
125 
126  if (cap->has("cam"))
127  {
128  if (strcasecmp(cap->get("cam").c_str(), "brow") == 0) __cam_id = 1;
129  }
130 
131  int dev = open_dev(__i2c_device_name);
132 
133  // Get dsPIC version (in order to know Nao version)
134  int val = i2c_smbus_read_byte_data(dev, 170);
135  if (val == -1) close_dev(dev, "NaoCamera: Error reading dsPic version from I2C");
136  if (val < 2)
137  {
138  LibLogger::log_info("NaoCamera", "Nao V2 found - No camera switching possible");
139  close_dev(dev);
140  return;
141  }
142  __can_switch_cam = true;
143  LibLogger::log_debug("NaoCamera", "Nao V3 found - Trying to switch to camera %d", __cam_id);
144 
145  val = get_open_cam_id(dev);
146 
147  if (val == __cam_id)
148  {
149  LibLogger::log_debug("NaoCamera", "Correct camera already chosen");
150  }
151  else
152  {
153  // Switch to other camera
154  switch_to_cam_id(dev, __cam_id);
155  }
156  close_dev(dev);
157 
158  // Connect to the chosen camera and initialize it
159  // FIXME: Maybe not needed? Try it!
160  init_cam(_device_name);
161 }
162 
163 NaoCamera::~NaoCamera()
164 {
165  free(__i2c_device_name);
166 }
167 
168 /**
169  * Helper function to open the I2C device
170  * @param i2c I2C device name
171  * @return device handle
172  */
173 int NaoCamera::open_dev(const char *i2c)
174 {
175  // Connect to dsPIC through I2C
176  int dev = ::open(i2c, O_RDWR);
177  if (dev < 0) throw Exception("NaoCamera: Error opening I2C for connection to dsPIC");
178  if (ioctl(dev, I2C_SLAVE, DSPIC_I2C_ADDR) < 0) close_dev(dev, "NaoCamera: Can't connect I2C to dsPIC");
179  return dev;
180 }
181 
182 /**
183  * Helper function called when something fails during camera switching.
184  * Closes the opened device and reports an error (if any) by throwing
185  * an exception.
186  *
187  * @param dev the device to close
188  * @param error null if no error, an error string otherwise
189  */
190 void NaoCamera::close_dev(int dev, const char *error)
191 {
192  if (::close(dev) < 0) throw fawkes::Exception("NaoCamera: Error closing device");
193  if (error) throw fawkes::Exception(error);
194 }
195 
196 /**
197  * Helper function to get the ID of the currently opened camera
198  * @param dev I2C device handle
199  */
200 int NaoCamera::get_open_cam_id(int dev)
201 {
202  // Ask dsPIC which camera is active
203  int cid = i2c_smbus_read_byte_data(dev, 220);
204  if (cid == -1) close_dev(dev, "Error reading active cam from I2C");
205  return cid;
206 }
207 
208 /**
209  * Helper function to switch to another camera
210  * @param dev I2C device handle
211  * @param cam_id ID of the camera to open
212  */
213 void NaoCamera::switch_to_cam_id(int dev, int cam_id)
214 {
215  unsigned char cmd[2];
216  cmd[0] = cam_id;
217  cmd[1] = 0;
218  int size = i2c_smbus_write_block_data(dev, 220, 1, cmd);
219  if (size == -1) close_dev(dev, "NaoCamera: Error switching to other camera");
220 }
221 
222 /**
223  * Helper function to initialize a camera
224  * @param cam Camera device name
225  */
226 void NaoCamera::init_cam(const char *cam)
227 {
228  int dev = ::open(cam, O_RDWR);
229  if (dev < 0) throw Exception("NaoCamera: Error opening Camera");
230 
231  struct v4l2_control control;
232  memset(&control, 0, sizeof(control));
233 
234  control.id = V4L2_CID_CAM_INIT;
235  control.value = 0;
236 
237  if (ioctl(dev, VIDIOC_S_CTRL, &control)) close_dev(dev, "Error setting other camera to default parameters");
238 
239  close_dev(dev);
240 }
241 
242 /**
243  * Return which cam is currently being used.
244  * 1: brow-cam
245  * 2: mouth-cam
246  * @return ID of camera currently in use
247  */
248 unsigned char NaoCamera::source()
249 {
250  int dev = open_dev(__i2c_device_name);
251  __cam_id = get_open_cam_id(dev);
252  close_dev(dev);
253 
254  return static_cast<unsigned char>(__cam_id);
255 }
256 
257 /**
258  * Switch currently used camera.
259  * Valid arguments:
260  * 1: brow-cam
261  * 2: mouth-cam
262  * @param source ID of the camera to use
263  */
264 void NaoCamera::set_source(unsigned char source)
265 {
266  if (source == __cam_id)
267  {
268  LibLogger::log_debug("NaoCamera", "Correct camera already chosen");
269  return;
270  }
271 
272  int dev = open_dev(__i2c_device_name);
273  switch_to_cam_id(dev, source);
274  close_dev(dev);
275  init_cam(_device_name);
276 }
277 
278 /**
279  * Return whether auto exposure is enabled.
280  * @return true if auto exposure is enabled
281  */
283 {
284  return get_one_control("AEC", V4L2_CID_AUTOEXPOSURE);
285 }
286 
287 /**
288  * Enable/disable auto exposure.
289  * @param enabled whether auto exposure should be enabled
290  */
292 {
293  LibLogger::log_debug("NaoCamera", (enabled ? "enabling AEC" : "disabling AEC"));
294 
295  set_one_control("AEC", V4L2_CID_AUTOEXPOSURE, (enabled ? 1 : 0));
296 }
297 
298 } // end namespace firevision
299 
char * _device_name
Device name.
Definition: v4l2.h:149
virtual bool exposure_auto()
Return whether auto exposure is enabled.
Definition: nao.cpp:282
virtual void close()
Close camera.
Definition: v4l2.cpp:993
virtual void set_source(unsigned char source)
Switch currently used camera.
Definition: nao.cpp:264
Fawkes library namespace.
Video4Linux 2 camera access implementation.
Definition: v4l2.h:46
Camera argument parser.
Definition: camargp.h:38
bool has(std::string s) const
Check if an parameter was given.
Definition: camargp.cpp:152
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual unsigned char source()
Return which cam is currently being used.
Definition: nao.cpp:248
virtual void set_one_control(const char *ctrl, unsigned int id, int value)
Set one Camera control value.
Definition: v4l2.cpp:824
virtual int get_one_control(const char *ctrl, unsigned int id)
Get one Camera control value.
Definition: v4l2.cpp:863
virtual void size(unsigned int &width, unsigned int &height)
Get the current image size.
Definition: image.cpp:96
virtual void open()
Open the camera.
Definition: v4l2.cpp:413
virtual void set_exposure_auto(bool enabled)
Enable/disable auto exposure.
Definition: nao.cpp:291
std::string get(std::string s) const
Get the value of the given parameter.
Definition: camargp.cpp:164
Expected parameter is missing.
Definition: software.h:76