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>
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)
45 #ifndef I2C_FUNC_SMBUS_READ_BYTE_DATA
46 # include <linux/i2c.h>
53 i2c_smbus_access(
int file,
char read_write, u8_ command,
int size,
union i2c_smbus_data *data)
55 struct i2c_smbus_ioctl_data args;
57 args.read_write = read_write;
58 args.command = command;
61 return ioctl(file, I2C_SMBUS, &args);
65 i2c_smbus_read_byte_data(
int file, u8_ command)
67 union i2c_smbus_data data;
68 if (i2c_smbus_access(file, I2C_SMBUS_READ, command, I2C_SMBUS_BYTE_DATA, &data))
71 return 0x0FF & data.byte;
75 i2c_smbus_write_block_data(
int file, u8_ command, u8_ length, u8_ *values)
77 union i2c_smbus_data data;
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);
90 namespace firevision {
110 if (cap->
has(
"i2c_device"))
111 i2c_device_name_ = strdup(cap->
get(
"i2c_device").c_str());
115 can_switch_cam_ =
false;
118 if (cap->
has(
"cam")) {
119 if (strcasecmp(cap->
get(
"cam").c_str(),
"brow") == 0)
123 int dev = open_dev(i2c_device_name_);
126 int val = i2c_smbus_read_byte_data(dev, 170);
128 close_dev(dev,
"NaoCamera: Error reading dsPic version from I2C");
130 LibLogger::log_info(
"NaoCamera",
"Nao V2 found - No camera switching possible");
134 can_switch_cam_ =
true;
135 LibLogger::log_debug(
"NaoCamera",
"Nao V3 found - Trying to switch to camera %d", cam_id_);
137 val = get_open_cam_id(dev);
139 if (val == cam_id_) {
140 LibLogger::log_debug(
"NaoCamera",
"Correct camera already chosen");
143 switch_to_cam_id(dev, cam_id_);
152 NaoCamera::~NaoCamera()
154 free(i2c_device_name_);
163 NaoCamera::open_dev(
const char *i2c)
166 int dev =
::open(i2c, O_RDWR);
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");
183 NaoCamera::close_dev(
int dev,
const char *error)
185 if (::
close(dev) < 0)
196 NaoCamera::get_open_cam_id(
int dev)
199 int cid = i2c_smbus_read_byte_data(dev, 220);
201 close_dev(dev,
"Error reading active cam from I2C");
211 NaoCamera::switch_to_cam_id(
int dev,
int cam_id)
213 unsigned char cmd[2];
216 int size = i2c_smbus_write_block_data(dev, 220, 1, cmd);
218 close_dev(dev,
"NaoCamera: Error switching to other camera");
226 NaoCamera::init_cam(
const char *cam)
228 int dev =
::open(cam, O_RDWR);
230 throw Exception(
"NaoCamera: Error opening Camera");
232 struct v4l2_control control;
233 memset(&control, 0,
sizeof(control));
235 control.id = V4L2_CID_CAM_INIT;
238 if (ioctl(dev, VIDIOC_S_CTRL, &control))
239 close_dev(dev,
"Error setting other camera to default parameters");
253 int dev = open_dev(i2c_device_name_);
254 cam_id_ = get_open_cam_id(dev);
257 return static_cast<unsigned char>(cam_id_);
271 LibLogger::log_debug(
"NaoCamera",
"Correct camera already chosen");
275 int dev = open_dev(i2c_device_name_);
276 switch_to_cam_id(dev,
source);
298 LibLogger::log_debug(
"NaoCamera", (enabled ?
"enabling AEC" :
"disabling AEC"));