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>
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>
53i2c_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);
65i2c_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;
75i2c_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);
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_);
152NaoCamera::~NaoCamera()
154 free(i2c_device_name_);
163NaoCamera::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");
183NaoCamera::close_dev(
int dev,
const char *error)
196NaoCamera::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");
211NaoCamera::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");
226NaoCamera::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"));
Base class for exceptions in Fawkes.
Expected parameter is missing.
bool has(std::string s) const
Check if an parameter was given.
std::string get(std::string s) const
Get the value of the given parameter.
virtual void size(unsigned int &width, unsigned int &height)
Get the current image size.
NaoCamera(const CameraArgumentParser *cap)
Constructor.
virtual unsigned char source()
Return which cam is currently being used.
virtual bool exposure_auto()
Return whether auto exposure is enabled.
virtual void set_source(unsigned char source)
Switch currently used camera.
virtual void set_exposure_auto(bool enabled)
Enable/disable auto exposure.
Video4Linux 2 camera access implementation.
virtual void close()
Close camera.
virtual void set_one_control(const char *ctrl, unsigned int id, int value)
Set one Camera control value.
char * _device_name
Device name.
virtual void open()
Open the camera.
virtual int get_one_control(const char *ctrl, unsigned int id)
Get one Camera control value.
Fawkes library namespace.