static int camera_query_support_format(CameraDevice *thiz)
{
int ret;
struct v4l2_fmtdesc fmt;
memset(&fmt, 0, sizeof(struct v4l2_fmtdesc));
fmt.index = 0;
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
printf("camera format ->\n");
while ((ret = ioctl(thiz->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);
ret = enum_frame_sizes(thiz->fd, fmt.pixelformat);
if (ret != 0)
{
printf(" Unable to enumerate frame sizes.\n");
}
}
if (errno != EINVAL)
{
printf("ERROR enumerating frame formats: %d\n", errno);
return errno;
}
return 0;
}
/* enumerate frames (formats, sizes and fps)
* args:
* width: current selected width
* height: current selected height
* fd: device file descriptor
*
* returns: pointer to LFormats struct containing list of available frame formats */
LFormats *enum_frame_formats(int *width, int *height, int fd)
{
int ret=0;
int fmtind=0;
struct v4l2_fmtdesc fmt;
memset(&fmt, 0, sizeof(fmt));
fmt.index = 0;
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
LFormats *listFormats = NULL;
listFormats = (LFormats *)malloc (sizeof( LFormats));
listFormats->listVidFormats = NULL;
while ((ret = xioctl(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);
/*check if format is supported by guvcview, set hardware flag and allocate on device list*/
if((ret=set_SupPixFormat(fmt.pixelformat)) >= 0)
{
fmtind++;
listFormats->listVidFormats = (VidFormats*) realloc(listFormats->listVidFormats,
sizeof(VidFormats) * fmtind);
listFormats->listVidFormats[fmtind-1].format=fmt.pixelformat;
snprintf(listFormats->listVidFormats[fmtind-1].fourcc,5,"%c%c%c%c",
fmt.pixelformat & 0xFF, (fmt.pixelformat >> 8) & 0xFF,
(fmt.pixelformat >> 16) & 0xFF, (fmt.pixelformat >> 24) & 0xFF);
//enumerate frame sizes
ret = enum_frame_sizes(listFormats->listVidFormats, fmt.pixelformat, fmtind, width, height, fd);
if (ret != 0)
perror(" Unable to enumerate frame sizes.\n");
}
else
{
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;
}
