25 #include <fvcams/nao.h> 26 #include <fvutils/system/camargp.h> 27 #include <logging/liblogger.h> 28 #include <core/exceptions/software.h> 31 #include <sys/ioctl.h> 32 #include <linux/i2c-dev.h> 33 #include <linux/types.h> 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) 46 #ifndef I2C_FUNC_SMBUS_READ_BYTE_DATA 47 #include <linux/i2c.h> 54 i2c_smbus_access(
int file,
char read_write, __u8 command,
55 int size,
union i2c_smbus_data *data)
57 struct i2c_smbus_ioctl_data args;
59 args.read_write = read_write;
60 args.command = command;
63 return ioctl(file,I2C_SMBUS,&args);
67 i2c_smbus_read_byte_data(
int file, __u8 command)
69 union i2c_smbus_data data;
70 if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
71 I2C_SMBUS_BYTE_DATA,&data))
74 return 0x0FF & data.byte;
78 i2c_smbus_write_block_data(
int file, __u8 command,
79 __u8 length, __u8 *values)
81 union i2c_smbus_data data;
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);
120 if (cap->
has(
"i2c_device")) __i2c_device_name = strdup(cap->
get(
"i2c_device").c_str());
123 __can_switch_cam =
false;
128 if (strcasecmp(cap->
get(
"cam").c_str(),
"brow") == 0) __cam_id = 1;
131 int dev = open_dev(__i2c_device_name);
134 int val = i2c_smbus_read_byte_data(dev, 170);
135 if (val == -1) close_dev(dev,
"NaoCamera: Error reading dsPic version from I2C");
138 LibLogger::log_info(
"NaoCamera",
"Nao V2 found - No camera switching possible");
142 __can_switch_cam =
true;
143 LibLogger::log_debug(
"NaoCamera",
"Nao V3 found - Trying to switch to camera %d", __cam_id);
145 val = get_open_cam_id(dev);
149 LibLogger::log_debug(
"NaoCamera",
"Correct camera already chosen");
154 switch_to_cam_id(dev, __cam_id);
163 NaoCamera::~NaoCamera()
165 free(__i2c_device_name);
173 int NaoCamera::open_dev(
const char *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");
190 void NaoCamera::close_dev(
int dev,
const char *error)
200 int NaoCamera::get_open_cam_id(
int dev)
203 int cid = i2c_smbus_read_byte_data(dev, 220);
204 if (cid == -1) close_dev(dev,
"Error reading active cam from I2C");
213 void NaoCamera::switch_to_cam_id(
int dev,
int cam_id)
215 unsigned char cmd[2];
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");
226 void NaoCamera::init_cam(
const char *cam)
228 int dev =
::open(cam, O_RDWR);
229 if (dev < 0)
throw Exception(
"NaoCamera: Error opening Camera");
231 struct v4l2_control control;
232 memset(&control, 0,
sizeof(control));
234 control.id = V4L2_CID_CAM_INIT;
237 if (ioctl(dev, VIDIOC_S_CTRL, &control)) close_dev(dev,
"Error setting other camera to default parameters");
250 int dev = open_dev(__i2c_device_name);
251 __cam_id = get_open_cam_id(dev);
254 return static_cast<unsigned char>(__cam_id);
266 if (source == __cam_id)
268 LibLogger::log_debug(
"NaoCamera",
"Correct camera already chosen");
272 int dev = open_dev(__i2c_device_name);
273 switch_to_cam_id(dev, source);
293 LibLogger::log_debug(
"NaoCamera", (enabled ?
"enabling AEC" :
"disabling AEC"));
char * _device_name
Device name.
virtual bool exposure_auto()
Return whether auto exposure is enabled.
virtual void close()
Close camera.
virtual void set_source(unsigned char source)
Switch currently used camera.
Fawkes library namespace.
Video4Linux 2 camera access implementation.
bool has(std::string s) const
Check if an parameter was given.
Base class for exceptions in Fawkes.
virtual unsigned char source()
Return which cam is currently being used.
virtual void set_one_control(const char *ctrl, unsigned int id, int value)
Set one Camera control value.
virtual int get_one_control(const char *ctrl, unsigned int id)
Get one Camera control value.
virtual void size(unsigned int &width, unsigned int &height)
Get the current image size.
virtual void open()
Open the camera.
virtual void set_exposure_auto(bool enabled)
Enable/disable auto exposure.
std::string get(std::string s) const
Get the value of the given parameter.
Expected parameter is missing.