int v4l2DownControl(struct vdIn *vd, int control) {
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int min, max, current, step, val_def;
int err;
if (isv4l2Control(vd, control, &queryctrl) < 0)
return -1;
min = queryctrl.minimum;
max = queryctrl.maximum;
step = queryctrl.step;
val_def = queryctrl.default_value;
if ( (current = v4l2GetControl(vd, control)) == -1 )
return -1;
current -= step;
//fprintf(stderr, "max %d, min %d, step %d, default %d ,current %d \n",max,min,step,val_def,current);
if (current >= min) {
control_s.id = control;
control_s.value = current;
if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
return -1;
}
//fprintf(stderr, "Control name:%s set to value:%d\n", queryctrl.name, control_s.value);
}
else {
return -1;
//fprintf(stderr, "Control name:%s already has min value:%d \n", queryctrl.name, min);
}
return 0;
}
int
v4l2DownControl (struct vdIn *vd, int control)
{
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int min, max, current, step, val_def;
int err;
if (isv4l2Control (vd, control, &queryctrl) < 0)
return -1;
min = queryctrl.minimum;
max = queryctrl.maximum;
step = queryctrl.step;
val_def = queryctrl.default_value;
current = v4l2GetControl (vd, control);
current -= step;
if (current >= min) {
control_s.id = control;
control_s.value = current;
if ((err = ioctl (vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
fprintf (stderr, "ioctl set control error\n");
return -1;
}
}
return control_s.value;
}
int v4l2GetControl(struct vdIn *vd, int control) {
struct v4l2_queryctrl queryctrl;
struct v4l2_control control_s;
int err;
if ((err = isv4l2Control(vd, control, &queryctrl)) < 0) {
return -1;
}
control_s.id = control;
if ((err = ioctl(vd->fd, VIDIOC_G_CTRL, &control_s)) < 0) {
return -1;
}
return control_s.value;
}
int
v4l2GetControl (struct vdIn *vd, int control)
{
struct v4l2_queryctrl queryctrl;
struct v4l2_control control_s;
int err;
if (isv4l2Control (vd, control, &queryctrl) < 0)
return -1;
control_s.id = control;
if ((err = ioctl (vd->fd, VIDIOC_G_CTRL, &control_s)) < 0) {
fprintf (stderr, "ioctl get control error\n");
return -1;
}
return control_s.value;
}
int v4l2ResetControl(struct vdIn *vd, int control) {
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int val_def;
int err;
if (isv4l2Control(vd, control, &queryctrl) < 0)
return -1;
val_def = queryctrl.default_value;
control_s.id = control;
control_s.value = val_def;
if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
return -1;
}
return 0;
}
int
v4l2ToggleControl (struct vdIn *vd, int control)
{
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int current;
int err;
if (isv4l2Control (vd, control, &queryctrl) != 1)
return -1;
current = v4l2GetControl (vd, control);
control_s.id = control;
control_s.value = !current;
if ((err = ioctl (vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
fprintf (stderr, "ioctl toggle control error\n");
return -1;
}
return control_s.value;
}
int v4l2ToggleControl(struct vdIn *vd, int control) {
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int current;
int err;
if (isv4l2Control(vd, control, &queryctrl) != 1)
return -1;
if ( (current = v4l2GetControl(vd, control)) == -1 )
return -1;
control_s.id = control;
control_s.value = !current;
if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
return -1;
}
return 0;
}
int
v4l2ResetPanTilt (struct vdIn *vd, int pantilt)
{
int control = V4L2_CID_PANTILT_RESET;
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
unsigned char val;
int err;
if (isv4l2Control (vd, control, &queryctrl) < 0)
return -1;
val = (unsigned char) pantilt;
control_s.id = control;
control_s.value = val;
if ((err = ioctl (vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
fprintf (stderr, "ioctl reset Pan control error\n");
return -1;
}
return 0;
}
int
v4L2UpDownTilt (struct vdIn *vd, short inc)
{
int control = V4L2_CID_PANTILT_RELATIVE;
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int err;
union pantilt pan;
control_s.id = control;
if (isv4l2Control (vd, control, &queryctrl) < 0)
return -1;
pan.s16.pan = 0;
pan.s16.tilt = inc;
control_s.value = pan.value;
if ((err = ioctl (vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
fprintf (stderr, "ioctl tiltupdown control error\n");
return -1;
}
return 0;
}
int v4l2SetControl(struct vdIn *vd, int control, int value) {
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int min, max, step, val_def;
int err;
if (isv4l2Control(vd, control, &queryctrl) < 0)
return -1;
min = queryctrl.minimum;
max = queryctrl.maximum;
step = queryctrl.step;
val_def = queryctrl.default_value;
if ((value >= min) && (value <= max)) {
control_s.id = control;
control_s.value = value;
if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
return -1;
}
}
return 0;
}
bool Worker::init()
{
rDebug("Initiating V4l2Capture...");
/// We shold be checking params before!!!
if (busparams.devices.size() == 0 ) qFatal ( "V4L2Capture::init() fatal error: device is NULL" );
if (busparams.width == 0 || busparams.height == 0 )
{
qFatal ("V4L2Capture::init() fatal error: Frame size is 0");
return -1;
}
int grabmethod = 1; // mmap
int format = V4L2_PIX_FMT_YUYV;
for (int i=0; i<busparams.numCameras; ++i)
{
qDebug() << "Configuring camera:" << i;
memset(&vd[i], 0, sizeof(struct vdIn));
vd[i].videodevice = (char *)calloc(busparams.devices[i].size()+1, sizeof(char));
vd[i].status = (char *)calloc(1, 100*sizeof(char));
//Esto debería hacerse con una expresión regular
snprintf(vd[i].videodevice, 12, "%s", busparams.devices[i].c_str());
printf ( "V4L2Capture::init() info: current video device %s \n", vd[i].videodevice );
vd[i].width = busparams.width;
vd[i].height = busparams.height;
vd[i].formatIn = format;
vd[i].grabmethod = grabmethod;
vd[i].fps = busparams.FPS;
if ((vd[i].fd = open(vd[i].videodevice, O_RDWR)) == -1 )
qFatal ( "V4L2Capture::init() fatal error: Opening V4L interface" );
memset(&vd[i].cap, 0, sizeof(struct v4l2_capability));
if (ioctl(vd[i].fd, VIDIOC_QUERYCAP, &vd[i].cap) < 0)
qFatal ( "V4L2Capture::init() fatal error: Unable to query device %s ", vd[i].videodevice );
if ((vd[i].cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)==0)
qFatal ( "V4L2Capture::init() fatal error: video capture not supported %s ", vd[i].videodevice );
if (vd[i].grabmethod)
{
if (!(vd[i].cap.capabilities & V4L2_CAP_STREAMING))
qFatal ( "V4L2Capture::init() fatal error: %s does not support streaming", vd[i].videodevice );
}
else
{
if (!(vd[i].cap.capabilities & V4L2_CAP_READWRITE) )
qFatal ( "V4L2Capture::init() fatal error: %s does not support read i/o ", vd[i].videodevice );
}
struct v4l2_fmtdesc fmt;
memset(&fmt, 0, sizeof(fmt));
fmt.index = 0;
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
while (ioctl(vd[i].fd, VIDIOC_ENUM_FMT, &fmt) == 0)
{
fmt.index++;
printf("{ pixelformat = '%c%c%c%c', description = '%s' }\n",fmt.pixelformat & 0xFF, (fmt.pixelformat >> 8) & 0xFF, (fmt.pixelformat >> 16) & 0xFF, (fmt.pixelformat >> 24) & 0xFF, fmt.description);
if (enum_frame_sizes(vd[i].fd, fmt.pixelformat) != 0)
printf(" Unable to enumerate frame sizes.\n");
}
// set format in
memset (&vd[i].fmt, 0, sizeof(struct v4l2_format));
vd[i].fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd[i].fmt.fmt.pix.width = vd[i].width;
vd[i].fmt.fmt.pix.height = vd[i].height;
vd[i].fmt.fmt.pix.pixelformat = vd[i].formatIn;
vd[i].fmt.fmt.pix.field = V4L2_FIELD_ANY;
if (ioctl(vd[i].fd, VIDIOC_S_FMT, &vd[i].fmt) < 0)
qFatal ( "V4L2Capture::init() fatal error: Unable to set format through VIDIOC_S_FMT" );
if ( (vd[i].fmt.fmt.pix.width!=(uint)vd[i].width) || (vd[i].fmt.fmt.pix.height!=(uint)vd[i].height) )
qFatal ( "V4L2Capture::init() fatal error: Size %dx%d is not available. Suggested %dx%d", vd[i].width, vd[i].height, vd[i].fmt.fmt.pix.width, vd[i].fmt.fmt.pix.height);
/* set framerate */
// struct v4l2_streamparm* setfps;
// setfps=(struct v4l2_streamparm *) calloc(1, sizeof(struct v4l2_streamparm));
// memset(setfps, 0, sizeof(struct v4l2_streamparm));
// setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
// setfps->parm.capture.timeperframe.numerator=1;
// setfps->parm.capture.timeperframe.denominator=30;
// if( ioctl(vd[i].fd, VIDIOC_S_PARM, setfps) < 0 )
// qFatal ( "V4L2Capture::init() fatal error: Unable to set frame rate through VIDIOC_S_PARM" );
// request buffers
memset(&vd[i].rb, 0, sizeof(struct v4l2_requestbuffers));
vd[i].rb.count = NB_BUFFER;
vd[i].rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd[i].rb.memory = V4L2_MEMORY_MMAP;
if (ioctl(vd[i].fd, VIDIOC_REQBUFS, &vd[i].rb) < 0)
qFatal ( "V4L2Capture::init() fatal error: Unable to allocate buffers through VIDIOC_REQBUFS" );
// map the buffers
for (int ii=0; ii<NB_BUFFER; ii++)
{
memset(&vd[i].buf, 0, sizeof(struct v4l2_buffer));
vd[i].buf.index = ii;
vd[i].buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd[i].buf.memory = V4L2_MEMORY_MMAP;
if (ioctl(vd[i].fd, VIDIOC_QUERYBUF, &vd[i].buf ) < 0)
qFatal ( "V4L2Capture::init() fatal error: Unable to query buffer through VIDIOC_QUERYBUF. Error number %d ", errno );
vd[i].mem[ii] = mmap(0, vd[i].buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, vd[i].fd, vd[i].buf.m.offset );
if (vd[i].mem[ii] == MAP_FAILED)
qFatal ( "V4L2Capture::init() fatal error: Unable to map the buffer through mmap. Error number: %d ",errno );
}
// Queue the buffers.
for (int ii = 0; ii < NB_BUFFER; ++ii)
{
memset(&vd[i].buf, 0, sizeof(struct v4l2_buffer));
vd[i].buf.index = ii;
vd[i].buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd[i].buf.memory = V4L2_MEMORY_MMAP;
if (ioctl(vd[i].fd, VIDIOC_QBUF, &vd[i].buf) < 0)
qFatal ( "V4L2Capture::init() fatal error: Unable to queue buffers through VIDIOC_QBUF. Error number: %d",errno );
}
// alloc a temp buffer to reconstruct the pict
vd[i].framesizeIn = ( vd[i].width * vd[i].height << 1 );
switch ( vd[i].formatIn )
{
case V4L2_PIX_FMT_MJPEG:
qFatal("MJPEG not supported");
// if ((vd[i].tmpbuffer=(unsigned char *)calloc(1, (size_t)vd[i].framesizeIn)) == 0)
// qFatal ( "V4L2Capture::init() fatal error: not enough memory to allocate vd[i].tmpbuffer" );
// vd[i].framebuffer = (unsigned char *)calloc(1, (size_t)vd[i].width*(vd[i].height + 8)*2);
break;
case V4L2_PIX_FMT_YUYV:
vd[i].framebuffer = (unsigned char *)calloc(1, ( size_t ) vd[i].framesizeIn);
break;
case V4L2_PIX_FMT_GREY:
vd[i].framebuffer = (unsigned char *)calloc(1, ( size_t ) vd[i].framesizeIn);
break;
default:
printf("V4LCapture::init() V4L2 error: Format not recognized!!\n");
return false;
break;
}
if (vd[i].framebuffer == 0)
qFatal ( "V4L2Capture::init() fatal error: not enough memory to allocate vd[i].framebuffer" );
int control;
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int err;
// Set the power line frequency value
control = V4L2_CID_POWER_LINE_FREQUENCY;
control_s.id = control;
if (isv4l2Control(&vd[i], control, &queryctrl) < 0) return false;
if (cameraParamsList[i].lineFreq == 60) control_s.value = V4L2_CID_POWER_LINE_FREQUENCY_60HZ;
else control_s.value = V4L2_CID_POWER_LINE_FREQUENCY_50HZ;
if ((err = ioctl(vd[i].fd, VIDIOC_S_CTRL, &control_s)) < 0)
qFatal("V4LCapture::init() V4L2 error: ioctl set_light_frequency_filter error\n");
printf("CameraComp: Power line frequency: %dHz\n", cameraParamsList[i].lineFreq);
// Set the saturation value
control = V4L2_CID_SATURATION;
control_s.id = control;
if (isv4l2Control(&vd[i], control, &queryctrl) < 0) return false;
control_s.value = cameraParamsList[i].saturation;
if ((err = ioctl(vd[i].fd, VIDIOC_S_CTRL, &control_s)) < 0)
qFatal("V4LCapture::init() V4L2 error: ioctl set_saturation error\n");
printf("CameraComp: Saturation: %d\n", cameraParamsList[i].saturation);
// Set the auto exposure off for max speed
control = V4L2_CID_EXPOSURE_AUTO;
control_s.id = control;
// if (isv4l2Control(&vd[i], control, &queryctrl) < 0) return false;
// control_s.value = 8;
// if ((err = ioctl(vd[i].fd, VIDIOC_S_CTRL, &control_s)) < 0)
// qFatal("V4LCapture::init() V4L2 error: ioctl set_exposureOff error\n");
// printf("CameraComp: AutoExposure: %d\n", 1);
//
// // Set the auto exposure off for max speed
// control = V4L2_CID_EXPOSURE_ABSOLUTE;
// control_s.id = control;
// if (isv4l2Control(&vd[i], control, &queryctrl) < 0) return false;
// control_s.value = 200;
// if ((err = ioctl(vd[i].fd, VIDIOC_S_CTRL, &control_s)) < 0)
// qFatal("V4LCapture::init() V4L2 error: ioctl set_exposureOff error\n");
// printf("CameraComp: AutoExposure: %d\n", 900);
}
return true;
}
