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
38using 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
52static inline s32_
53i2c_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
64static inline s32_
65i2c_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
74static inline s32_
75i2c_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
90namespace 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 */
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
152NaoCamera::~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 */
162int
163NaoCamera::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 */
182void
183NaoCamera::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 */
195int
196NaoCamera::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 */
210void
211NaoCamera::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 */
225void
226NaoCamera::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 */
250unsigned 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 */
267void
268NaoCamera::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 */
285bool
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 */
295void
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
Base class for exceptions in Fawkes.
Definition: exception.h:36
Expected parameter is missing.
Definition: software.h:74
Camera argument parser.
Definition: camargp.h:36
bool has(std::string s) const
Check if an parameter was given.
Definition: camargp.cpp:145
std::string get(std::string s) const
Get the value of the given parameter.
Definition: camargp.cpp:156
virtual void size(unsigned int &width, unsigned int &height)
Get the current image size.
Definition: image.cpp:89
NaoCamera(const CameraArgumentParser *cap)
Constructor.
Definition: nao.cpp:108
virtual unsigned char source()
Return which cam is currently being used.
Definition: nao.cpp:251
virtual bool exposure_auto()
Return whether auto exposure is enabled.
Definition: nao.cpp:286
virtual void set_source(unsigned char source)
Switch currently used camera.
Definition: nao.cpp:268
virtual void set_exposure_auto(bool enabled)
Enable/disable auto exposure.
Definition: nao.cpp:296
Video4Linux 2 camera access implementation.
Definition: v4l2.h:43
virtual void close()
Close camera.
Definition: v4l2.cpp:1031
virtual void set_one_control(const char *ctrl, unsigned int id, int value)
Set one Camera control value.
Definition: v4l2.cpp:864
char * _device_name
Device name.
Definition: v4l2.h:140
virtual void open()
Open the camera.
Definition: v4l2.cpp:416
virtual int get_one_control(const char *ctrl, unsigned int id)
Get one Camera control value.
Definition: v4l2.cpp:903
Fawkes library namespace.