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