#include "v4l2_driver.h"
#include "log.h"
#include
#include
#include
#include
#include
#define HEADERFRAME1 0xaf
CVideoDriver::CVideoDriver()
{
logtrace(("CVideoDriver::CVideoDriver begin....\n"));
logtrace(("CVideoDriver::CVideoDriver end ok\n"));
}
CVideoDriver::~CVideoDriver()
{
logtrace(("CVideoDriver::~CVideoDriver begin....\n"));
logtrace(("CVideoDriver::~CVideoDriver end ok\n"));
}
//CVideoDriver* CVideoDriver::globle_ptr = NULL;
//CVideoDriver& CVideoDriver::instance()
//{
// if(globle_ptr == NULL)
// {
// globle_ptr = new CVideoDriver;
// }
// return *globle_ptr;
//}
int CVideoDriver::start(const string& dev,const int& width,const int& height,const int& fps,const int& format)
{
logtrace(("CVideoDriver::start begin....\n"));
usb_video_device_name = dev;
m_cap_width = width;
m_cap_height = height;
m_cap_fps = fps;
m_cap_fmt = format;
m_video_input.width = width;
m_video_input.height = height;
m_video_input.fps = fps;
m_video_input.formatIn = format;
m_video_input.framesizeIn = (m_video_input.width * m_video_input.height << 1);
//m_video_input.tmpbuffer = (unsigned char *) calloc(1, (size_t) m_video_input.framesizeIn);
//m_video_input.framebuffer = (unsigned char *) calloc(1, (size_t) m_video_input.width * (m_video_input.height + 8) * 2);
if(init_device())
{
logerror(("CVideoDriver::start failed when init device\n"));
return -1;
}
logtrace(("CVideoDriver::open end ok\n"));
return 0;
}
int CVideoDriver::close()
{
logtrace(("CVideoDriver::close....\n"));
int ret = 0;
disable();
//free((void*)m_video_input.tmpbuffer);
//m_video_input.tmpbuffer = NULL;
//ret = close(m_video_input.fd);
logtrace(("CVideoDriver::close end ok ret=<%d>\n",ret));
return ret;
}
int CVideoDriver::capture(unsigned char* pimage)
{
logtrace(("CVideoDriver::capture begin....\n"));
int ret = 0;
size_t var_used =0;
if (enalbe())
{
logerror(("CVideoDriver::capture faild when enable\n"));
goto fatal;
}
memset(&m_video_input.buf, 0, sizeof(struct v4l2_buffer));
m_video_input.buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
m_video_input.buf.memory = V4L2_MEMORY_MMAP;
//logdebug(("CVideoDriver::capture begin VIDIOC_DQBUF\n"));
ret = ioctl(m_video_input.fd, VIDIOC_DQBUF, &m_video_input.buf);
if (ret < 0) {
logerror(("CVideoDriver::capture faild when VIDIOC_DQBUF\n"));
m_video_input.isstreaming = false;
disable();
goto fatal;
}
if (m_video_input.buf.bytesused <= HEADERFRAME1) { /* Prevent crash
* on empty image */
logdebug(("CVideoDriver::capture,empty buffer\n"));
ret = ioctl(m_video_input.fd, VIDIOC_QBUF, &m_video_input.buf);
return 0;
}
memcpy((void*)pimage, m_video_input.mem[m_video_input.buf.index], m_video_input.buf.bytesused);
//memcpy(m_video_input.tmpbuffer, m_video_input.mem[m_video_input.buf.index], m_video_input.buf.bytesused);
var_used = m_video_input.buf.bytesused;
//logdebug(("CVideoDriver::capture,buf.bytesused=<%d>.\n", m_video_input.buf.bytesused));
ret = ioctl(m_video_input.fd, VIDIOC_QBUF, &m_video_input.buf);
if(ret)
{
logerror(("CVideoDriver::capture,VIDIOC_QBUF failed\n"));
goto fatal;
}
logtrace(("CVideoDriver::capture end ok ret=<%d>\n",ret));
return var_used;
fatal:
return -1;
}
int CVideoDriver::init_device()
{
logtrace(("CVideoDriver::init_device....\n"));
//int i = 0;
int ret = -1;
//! set formate
if ((m_video_input.fd = open(usb_video_device_name.c_str(), O_RDWR)) == -1) {
logerror(("CVideoDriver::init_device failed when open=<%s>\n",usb_video_device_name.c_str()));
return ret;
}
ret = set_format();
if(ret < 0)
{
logerror(("CVideoDriver::init_device failed when set_format\n"));
return ret;
}
ret = set_framerate();
if(ret < 0)
{
logerror(("CVideoDriver::init_device failed when set_framerate\n"));
return ret;
}
ret = set_map_buffer();
if(ret < 0)
{
logerror(("CVideoDriver::init_device failed when set_map_buffer\n"));
return ret;
}
logtrace(("CVideoDriver::init_device end ok\n"));
return ret;
}
int CVideoDriver::set_format()
{
logtrace(("CVideoDriver::set_format....\n"));
int ret = 0;
memset(&m_video_input.fmt, 0, sizeof(struct v4l2_format));
m_video_input.fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
m_video_input.fmt.fmt.pix.width = m_video_input.width;
m_video_input.fmt.fmt.pix.height = m_video_input.height;
m_video_input.fmt.fmt.pix.pixelformat = m_video_input.formatIn;
m_video_input.fmt.fmt.pix.field = V4L2_FIELD_ANY;
ret = ioctl(m_video_input.fd, VIDIOC_S_FMT, &m_video_input.fmt);
logtrace(("CVideoDriver::set_format end ret=<%d>\n",ret));
return ret;
}
int CVideoDriver::set_framerate()
{
logtrace(("CVideoDriver::close....\n"));
int ret = 0;
struct v4l2_streamparm var_fps;
struct v4l2_streamparm *setfps = &var_fps;
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 = m_video_input.fps;
ret = ioctl(m_video_input.fd, VIDIOC_S_PARM, setfps);
if(ret<0){
logerror(("CVideoDriver::set_framerate failed\n"));
return ret;
}
logtrace(("CVideoDriver::set_framerate end ok ret=<%d>\n",ret));
return ret;
}
int CVideoDriver::set_map_buffer()
{
logtrace(("CVideoDriver::set_map_buffer....\n"));
int i,ret = 0;
video_input* vd = &m_video_input;
/*
* request buffers
*/
memset(&vd->rb, 0, sizeof(struct v4l2_requestbuffers));
vd->rb.count = NB_BUFFER;
vd->rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd->rb.memory = V4L2_MEMORY_MMAP;
ret = ioctl(vd->fd, VIDIOC_REQBUFS, &vd->rb);
if (ret < 0) {
perror("CVideoDriver::set_map_buffer when VIDIOC_REQBUFS");
goto fatal;
}
/****** begin to map *******/
for (i = 0; i < NB_BUFFER; i++) {
memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
vd->buf.index = i;
vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd->buf.memory = V4L2_MEMORY_MMAP;
ret = ioctl(vd->fd, VIDIOC_QUERYBUF, &vd->buf);
if (ret < 0) {
perror("CVideoDriver::set_map_buffer,VIDIOC_QUERYBUF failed\n");
goto fatal;
}
logdebug(("CVideoDriver::set_map_buffer,length=<%d>,offset=<%u>\n", vd->buf.length, vd->buf.m.offset));
vd->mem[i] = mmap( 0 /* start anywhere */ ,
vd->buf.length,
PROT_READ, MAP_SHARED,
vd->fd,
vd->buf.m.offset);
if (vd->mem[i] == MAP_FAILED) {
logerror(("CVideoDriver::set_map_buffer,map operation failed\n"));
goto fatal;
}//!! end if
logdebug(("CVideoDriver::set_map_buffer,mapped address=<%p>", vd->mem[i]));
}//! end for
//! Queue the buffers.
for (i = 0; i < NB_BUFFER; ++i) {
memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
vd->buf.index = i;
vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd->buf.memory = V4L2_MEMORY_MMAP;
ret = ioctl(vd->fd, VIDIOC_QBUF, &vd->buf);
if (ret < 0) {
logerror(("CVideoDriver::set_map_buffer,VIDIOC_QBUF failed\n"));
goto fatal;;
}//! end if
}//! end for
logtrace(("CVideoDriver::set_map_buffer end ok ret=<%d>\n",ret));
return ret;
fatal:
return ret;
}
int CVideoDriver::enalbe()
{
logtrace(("CVideoDriver::enalbe....\n"));
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
int ret =0;
if(m_video_input.isstreaming)
{
//logdebug(("CVideoDriver::enalbe has enabled\n"));
return ret;
}
ret = ioctl(m_video_input.fd, VIDIOC_STREAMON, &type);
if (ret < 0) {
logerror(("CVideoDriver::enalbe failed when VIDIOC_STREAMON\n"));
return ret;
}
m_video_input.isstreaming = true;
logtrace(("CVideoDriver::enalbe end ok ret=<%d>\n",ret));
return ret;
}
int CVideoDriver::disable()
{
logtrace(("CVideoDriver::disable....\n"));
int ret = 0;
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
ret = ioctl(m_video_input.fd, VIDIOC_STREAMOFF, &type);
if (ret < 0) {
logerror(("CVideoDriver::enalbe failed when VIDIOC_STREAMOFF\n"));
return ret;
}
m_video_input.isstreaming = false;
logtrace(("CVideoDriver::disable end ok ret=<%d>\n",ret));
return ret;
}
int CVideoDriver::isv4l2Control(const int& control,struct v4l2_queryctrl& queryctrl)
{
int err =0;
struct v4l2_queryctrl var_queryctrl = queryctrl;
var_queryctrl.id = control;
if ((err= ioctl(m_video_input.fd, VIDIOC_QUERYCTRL, &var_queryctrl)) < 0) {
perror("ioctl querycontrol error \n");
return -1;
}
if (var_queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
fprintf(stderr, "control %s disabled \n", (char *) var_queryctrl.name);
return -1;
}
if (var_queryctrl.type & V4L2_CTRL_TYPE_BOOLEAN) {
return 1;
}
if (var_queryctrl.type & V4L2_CTRL_TYPE_INTEGER) {
return 0;
}
fprintf(stderr, "contol %s unsupported \n", (char *) var_queryctrl.name);
queryctrl = var_queryctrl;
return 1;
}
int CVideoDriver::v4l2GetControl(const int& control)
{
struct v4l2_queryctrl queryctrl;
struct v4l2_control control_s;
int err;
if ((err = isv4l2Control(control, queryctrl)) < 0) {
return -1;
}
control_s.id = control;
if ((err = ioctl(m_video_input.fd, VIDIOC_G_CTRL, &control_s)) < 0) {
return -1;
}
return control_s.value;
}
int CVideoDriver::v4l2_up_control(const int& control)
{
logtrace(("CVideoDriver::v4l2_up_control begin control=<%d>..\n",control));
int ret = 0;
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int min, max, current, step, val_def;
int err;
if (isv4l2Control(control, queryctrl) < 0)
{
logerror(("CVideoDriver::v4l2_up_control failed when isv4l2Control\n"));
return -1;
}
min = queryctrl.minimum;
max = queryctrl.maximum;
step = queryctrl.step;
val_def = queryctrl.default_value;
if ( (current = v4l2GetControl(control)) == -1 )
{
logerror(("CVideoDriver::v4l2_up_control failed when v4l2GetControl\n"));
return -1;
}
logdebug(("CVideoDriver::v4l2_up_control,current=<%d>,step=<%d>,max=<%d>\n",current,step,max));
current = 24;//! max invalid = 30
//fprintf(stderr, "max %d, min %d, step %d, default %d ,current %d \n",max,min,step,val_def,current);
if (current <= max) {
control_s.id = control;
control_s.value = current;
if ((err = ioctl(m_video_input.fd, VIDIOC_S_CTRL, &control_s)) < 0) {
logerror(("CVideoDriver::v4l2_up_control failed when VIDIOC_S_CTRL\n"));
return -1;
}
//fprintf(stderr, "Control name:%s set to value:%d\n", queryctrl.name, control_s.value);
} else {
//fprintf(stderr, "Control name:%s already has max value:%d \n", queryctrl.name, max);
logerror(("CVideoDriver::v4l2_up_control failed when current > max\n"));
return -1;
}
logtrace(("CVideoDriver::v4l2_up_control end ok ret=<%d>\n",ret));
return ret;
}
