static void stop_capturing(Priv *priv)
{
enum v4l2_buf_type type;
switch (priv->io) {
case IO_METHOD_READ:
/* Nothing to do. */
break;
case IO_METHOD_MMAP:
case IO_METHOD_USERPTR:
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl(priv->fd, VIDIOC_STREAMOFF, &type))
errno_exit("VIDIOC_STREAMOFF");
break;
}
}
static void stop_capturing(void)
{
enum v4l2_buf_type type;
switch (io) {
case IO_METHOD_READ:
/* Nothing to do. */
break;
case V4L2_MEMORY_MMAP:
case V4L2_MEMORY_USERPTR:
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (v4l2_ioctl(fd, VIDIOC_STREAMOFF, &type) < 0)
errno_exit("VIDIOC_STREAMOFF");
break;
}
}
/**
mainloop: read frames and process them
*/
static void mainLoop(void)
{
unsigned int count;
unsigned int numberOfTimeouts;
numberOfTimeouts = 0;
count = 3;
while (count-- > 0) {
for (;;) {
fd_set fds;
struct timeval tv;
int r;
FD_ZERO(&fds);
FD_SET(fd, &fds);
/* Timeout. */
tv.tv_sec = 1;
tv.tv_usec = 0;
r = select(fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
if (EINTR == errno)
continue;
errno_exit("select");
}
if (0 == r) {
if (numberOfTimeouts <= 0) {
count++;
} else {
fprintf(stderr, "select timeout\n");
exit(EXIT_FAILURE);
}
}
if (frameRead())
break;
/* EAGAIN - continue select loop. */
}
}
}
static void mainloop (void) {
unsigned int count;
count = G_frames;
while (G_frames == 0 || count-- > 0) {
for (;;) {
fd_set fds;
struct timeval tv;
int r;
FD_ZERO (&fds);
FD_SET (fd, &fds);
/* Timeout. */
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select (fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
if (EINTR == errno)
continue;
errno_exit ("select");
}
if (0 == r) {
//char msg[256];
if (G_paused)
continue;
fprintf (stderr, "select timeout\n");
//ioctl (fd, S2253_VIDIOC_DEBUG, msg);
//fprintf (stderr, "debug: %s\n", msg);
exit (EXIT_FAILURE);
}
if (read_frame ())
break;
/* EAGAIN - continue select loop. */
}
}
}
void capture(int *fd, camera_buffer *cam_bufs, void **img, int camera_count, int isSelect)
{
int i;
void *ptr;
for (i=0;i<camera_count;i++){
for (;;) {
fd_set fds;
struct timeval tv;
int r;
FD_ZERO (&fds);
FD_SET (fd[i], &fds);
/* Timeout. */
tv.tv_sec = 0;
tv.tv_usec = 1000000;
if (isSelect==1)
{
r = select (fd[i] + 1, &fds, NULL, NULL, &tv);
if (-1 == r)
{
if (EINTR == errno)
continue;
errno_exit (__FILE__,__LINE__,"select");
}
if (0 == r)
{
fprintf (stderr, "select timeout\n");
exit (EXIT_FAILURE);
}
}
ptr=read_frame(fd[i],&cam_bufs[i]);
if (ptr){
img[i]=ptr;
break;
}
/* EAGAIN - continue select loop. */
}
}
}
void
video_driver::init_userp (unsigned int buffer_size)
{
struct v4l2_requestbuffers req;
CLEAR (req);
req.count = 4;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_USERPTR;
if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req))
{
if (EINVAL == errno)
{
fprintf (stderr, "%s does not support "
"user pointer i/o\n", dev_name);
exit (EXIT_FAILURE);
}
else
{
errno_exit ("VIDIOC_REQBUFS");
}
}
buffers = (buffer *) calloc (4, sizeof (*buffers));
if (!buffers)
{
fprintf (stderr, "Out of memory\n");
exit (EXIT_FAILURE);
}
for (n_buffers = 0; n_buffers < 4; ++n_buffers)
{
buffers[n_buffers].length = buffer_size;
buffers[n_buffers].start = malloc (buffer_size);
if (!buffers[n_buffers].start)
{
fprintf (stderr, "Out of memory\n");
exit (EXIT_FAILURE);
}
}
}
void
capture_queue_buffer(capture * cap, const void * buffer_data)
{
unsigned int i;
for (i = 0; i < cap->n_buffers; ++i) {
if (cap->buffers[i].start == buffer_data) {
goto found;
}
}
fprintf (stderr, "%s: Tried to release bad buffer %d %p\n", __func__, i, buffer_data);
exit (EXIT_FAILURE);
found:
if (-1 == xioctl(cap->fd, VIDIOC_QBUF, &cap->buffers[i].v4l2buf))
errno_exit("VIDIOC_QBUF");
}
static void stop_capturing (void) {
enum v4l2_buf_type type;
switch (io) {
case IO_METHOD_READ:
/* Nothing to do. */
break;
case IO_METHOD_MMAP:
case IO_METHOD_USERPTR:
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fprintf(stderr, "stream off sent\n");
if (-1 == xioctl (fd, VIDIOC_STREAMOFF, &type))
errno_exit ("VIDIOC_STREAMOFF");
break;
}
}
void Camera::uninit_device(void) {
unsigned int i;
switch (io) {
case IO_METHOD_READ:
free(buffers[0].start);
break;
case IO_METHOD_MMAP:
for (i = 0; i < n_buffers; ++i)
if (-1 == munmap(buffers[i].start, buffers[i].length))
errno_exit("munmap");
break;
case IO_METHOD_USERPTR:
for (i = 0; i < n_buffers; ++i)
free(buffers[i].start);
break;
}
free(buffers);
}
static void init_userp(unsigned int buffer_size)
{
struct v4l2_requestbuffers req;
CLEAR(req);
req.count = 4;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_USERPTR;
if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) {
if (EINVAL == errno) {
fprintf(stderr, "%s does not support "
"user pointer i/o\n", dev_name);
exit(EXIT_FAILURE);
} else {
errno_exit("VIDIOC_REQBUFS");
}
}
buffers = calloc(4, sizeof(*buffers));
if (!buffers) {
fprintf(stderr, "Out of memory\n");
exit(EXIT_FAILURE);
}
if(!init_SharedMemoryRegion(req.count, buffer_size))
{
printf("Shared Memory Region Init Fail!!!\n");
exit(EXIT_FAILURE);
}
for (n_buffers = 0; n_buffers < 4; ++n_buffers) {
// shmPtr += n_buffers*buffer_size;
buffers[n_buffers].length = buffer_size;
buffers[n_buffers].start = shmPtr+n_buffers*buffer_size;
/* buffers[n_buffers].start = malloc(buffer_size);
if (!buffers[n_buffers].start) {
fprintf(stderr, "Out of memory\n");
exit(EXIT_FAILURE);
}*/
}
}
//void mainloop(int fd)
void *thread_mainloop(void * camera)
{
camera_t *cam=(camera_t *)camera;
int fd=cam->fd;
//unsigned int count;
// count = 500;
while (!cam->stopflag) {
for (;;) {
fd_set fds;
struct timeval tv;
int r;
FD_ZERO (&fds);
FD_SET (fd, &fds);
/* Timeout. */ //2秒的超时等待
tv.tv_sec = 2;
tv.tv_usec = 0;
//等待摄像头设备可读
r = select (fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
if (EINTR == errno)
{
printf("#### select CONTINUE \n");
continue;
}
errno_exit ("select");
}
if (0 == r) {
fprintf (stderr, "select timeout, EXIT \n");
exit (EXIT_FAILURE);
}
if (capture_read_frame (cam))
break;
/* EAGAIN - continue select loop. */
}
}
//printf("##IN#Mainloop END ######\n");
}
static void process_image(const void *p, int size) {
if (out_buf) {
FILE *outfile = fopen( out_name, "wb" );
// try to open file for saving
if (!outfile) {
errno_exit("jpeg");
}
// write the image and flush
fwrite(p, size, 1, outfile);
fflush(outfile);
// close output file
fclose(outfile);
fflush(stderr);
fprintf(stderr, ".");
}
}
static void
uninit_device(struct device* dev)
{
unsigned int i;
switch (dev->io) {
case IO_METHOD_READ:
free(dev->buffers[0].start);
break;
case IO_METHOD_MMAP:
for (i = 0; i < dev->n_buffers; ++i)
if (-1 == munmap(dev->buffers[i].start, dev->buffers[i].length))
errno_exit("munmap");
break;
}
free (dev->buffers);
}
bool Camera::GetBuffer(unsigned char *image){
fd_set fds;
struct timeval tv;
int r;
FD_ZERO(&fds);
FD_SET(fd, &fds);
/* Timeout. */
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select(fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
errno_exit("select");
}
if (0 == r) {
fprintf(stderr, "select timeout\n");
exit(EXIT_FAILURE);
}
read_frame(image);
}
static void
init_userp (V4L2WHandler_t * handle, unsigned int buffer_size)
{
struct v4l2_requestbuffers req;
unsigned int page_size;
page_size = getpagesize ();
buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
CLEAR (req);
req.count = 4;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_USERPTR;
if (-1 == xioctl (handle->fd, VIDIOC_REQBUFS, &req)) {
if (EINVAL == errno) {
fprintf (stderr, "%s does not support "
"user pointer i/o\n", handle->dev_name);
exit (EXIT_FAILURE);
} else {
errno_exit ("VIDIOC_REQBUFS");
}
}
handle->buffers = calloc (4, sizeof (*handle->buffers));
if (!handle->buffers) {
fprintf (stderr, "Out of memory\n");
exit (EXIT_FAILURE);
}
for (handle->n_buffers = 0; handle->n_buffers < 4; ++handle->n_buffers) {
handle->buffers[handle->n_buffers].length = buffer_size;
handle->buffers[handle->n_buffers].start = memalign (/* boundary */ page_size,
buffer_size);
if (!handle->buffers[handle->n_buffers].start) {
fprintf (stderr, "Out of memory\n");
exit (EXIT_FAILURE);
}
}
}
void Camera::Stop() {
enum v4l2_buf_type type;
switch(io) {
case IO_METHOD_READ:
/* Nothing to do. */
break;
case IO_METHOD_MMAP:
case IO_METHOD_USERPTR:
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if(-1 == xioctl (fd, VIDIOC_STREAMOFF, &type))
errno_exit ("VIDIOC_STREAMOFF");
break;
}
}
void SimpleV4L2::stopCapturing()
{
enum v4l2_buf_type type;
switch (m_io) {
case IO_METHOD_READ:
/* Nothing to do. */
break;
case IO_METHOD_MMAP:
case IO_METHOD_USERPTR:
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl (m_fd, VIDIOC_STREAMOFF, &type))
errno_exit ("VIDIOC_STREAMOFF");
break;
}
}
int UninitDeviceV4L2(FRAMEGRABBER_V4L2 *fg_v4l2){
unsigned int i;
//=================================
/// Only memory mapped buffers
//=================================
for (i = 0; i < fg_v4l2->n_buffers; ++i){
if (-1 == munmap (fg_v4l2->buffers[i].start, fg_v4l2->buffers[i].length))
errno_exit ("munmap");
//free(fg_v4l2->buffers[i].start);
//free(fg_v4l2->buffers[i].length);
}
free (fg_v4l2->buffers);
return 0;
}
unsigned char* snapFrame()
{
//printf("Start snapFrame\n");
for (;;)
{
fd_set fds;
struct timeval tv;
int r;
FD_ZERO(&fds);
FD_SET(fd, &fds);
/* Timeout. */
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select(fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r)
{
if (EINTR == errno)
continue;
errno_exit("select");
}
if (0 == r)
{
fprintf(stderr, "select timeout\n");
exit(EXIT_FAILURE);
}
if (read_frame())
break;
/* EAGAIN - continue select loop. */
}
//printf("End snapFrame\n");
// v4l2_buffer* pI = ((v4l2_buffer*)buffers[buf.index].start);
//return pI;
return (unsigned char*)buffers[buf.index].start;
}
static void mainloop(void)
{
unsigned int count;
if (frame_count == 0) {
infinit_count = 1;
}
count = frame_count;
while (infinit_count || count-- > 0) {
// printf("%u\n", count);
for (;; ) {
fd_set fds;
struct timeval tv;
int r;
FD_ZERO(&fds);
FD_SET(fd, &fds);
/* Timeout. */
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select(fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
if (EINTR == errno)
continue;
errno_exit("select");
}
if (0 == r) {
fprintf(stderr, "select timeout\n");
exit(EXIT_FAILURE);
}
if (read_frame())
break;
/* EAGAIN - continue select loop. */
}
}
}
static void
uninit_device (ia_v4l_t* v)
{
unsigned int i;
io_method io = v->io;
void* buffer = v->mmap_buffer;
size_t length = v->mmap_length;
struct buffer* buffers = v->buffers;
switch (io) {
case IO_METHOD_MMAP:
if (-1 == ia_munmap (buffer, length))
errno_exit ("munmap");
ia_free (buffers);
break;
default:
break;
}
}
int V4LCapture::load_frame(void **data, struct v4l2_buffer *buf)
{
*buf = buf_next;
if(*data == NULL){
*data = malloc(buf->length);
if(!*data){
errno_exit("calloc error");
}
}
int len = fread(*data, buf->bytesused, 1, video_rec);
#if 0
fprintf(stderr, "load %s %d %d %d bytes\n",
dev_name, buf->sequence, buf->bytesused, buf->length);
#endif
int ret = fread(&buf_next, sizeof(buf_next), 1, time_rec);
if(ret == 0){
buf_next.bytesused = 0;
}
return len;
}
bool Camera::init_mmap(void) {
struct v4l2_requestbuffers req;
CLEAR (req);
req.count = 4;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_MMAP;
if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) {
if (EINVAL == errno) {
fprintf(stderr, "%s does not support "
"memory mapping\n", dev_name);
return false;
} else {
return false;
}
}
if (req.count < 2) {
fprintf(stderr, "Insufficient buffer memory on %s\n", dev_name);
return false;
}
buffers = (buffer*)calloc(req.count, sizeof(*buffers));
if (!buffers) {
fprintf(stderr, "Out of memory\n");
return false;
}
for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
struct v4l2_buffer buf;
CLEAR (buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = n_buffers;
if (-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf))
errno_exit("VIDIOC_QUERYBUF");
buffers[n_buffers].length = buf.length;
buffers[n_buffers].start = mmap(NULL /* start anywhere */, buf.length,
PROT_READ | PROT_WRITE /* required */,
MAP_SHARED /* recommended */, fd, buf.m.offset);
if (MAP_FAILED == buffers[n_buffers].start)
return false;
}
return true;
}
static void mainloop()
{
unsigned long val,count;
time_t elapsed = time(NULL);
val = count = 100;
while (count-- > 0)
{
for (;;)
{
fd_set fds;
struct timeval tv;
int r;
FD_ZERO (&fds);
FD_SET (fd, &fds);
// * Timeout. *
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select (fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r)
{
if (EINTR == errno)
continue;
errno_exit ("select");
}
if (0 == r)
{
fprintf (stderr, "select timeout\n");
exit (EXIT_FAILURE);
}
if (read_frame ())
break;
}
}
elapsed = time(NULL) - elapsed;
fprintf(stdout, "The frame rate (per second) = %lu\n", val/elapsed);
}
void SimpleV4L2::io_init_userp(unsigned int buffer_size)
{
struct v4l2_requestbuffers req;
unsigned int page_size;
page_size = getpagesize ();
buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
CLEAR (req);
req.count = 4;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_USERPTR;
if (-1 == xioctl (m_fd, VIDIOC_REQBUFS, &req)) {
if (EINVAL == errno) {
fprintf (stderr, "%s does not support user pointer i/o\n", m_devName);
exit (EXIT_FAILURE);
} else {
errno_exit ("VIDIOC_REQBUFS");
}
}
m_buffers = (v4l2_simple_buffer*)calloc (4, sizeof (*m_buffers));
if (!m_buffers) {
fprintf (stderr, "Out of memory\n");
exit (EXIT_FAILURE);
}
for (m_numBuffers = 0; m_numBuffers < 4; ++m_numBuffers) {
m_buffers[m_numBuffers].length = buffer_size;
m_buffers[m_numBuffers].start = memalign (/* boundary */ page_size,
buffer_size);
if (!m_buffers[m_numBuffers].start) {
fprintf (stderr, "Out of memory\n");
exit (EXIT_FAILURE);
}
}
}
static void init_userp(unsigned int buffer_size)
{
struct v4l2_requestbuffers req;
unsigned int page_size;
page_size = getpagesize();
buffer_size = (buffer_size+page_size-1)&~(page_size-1);
CLEAR (req);
req.count = 4;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_USERPTR;
if (-1==xioctl(fd, VIDIOC_REQBUFS, &req)) {
if (EINVAL==errno) {
ROS_ERROR("%s does not support "
"user pointer i/o\n", camera_dev);
exit(EXIT_FAILURE);
} else {
errno_exit("VIDIOC_REQBUFS");
}
}
buffers = (buffer*)calloc(4, sizeof(*buffers));
if (!buffers) {
ROS_ERROR("Out of memory\n");
exit(EXIT_FAILURE);
}
for(n_buffers = 0; n_buffers<4; ++n_buffers) {
buffers[n_buffers].length = buffer_size;
buffers[n_buffers].start = memalign(/* boundary */page_size, buffer_size);
if (!buffers[n_buffers].start) {
ROS_ERROR("Out of memory\n");
exit(EXIT_FAILURE);
}
}
}
static void mainloop(void)
{
unsigned int count = capture_count;
printf("Start capture %d frames\n", count);
while (count-- > 0) {
for (;;) {
#if 0
fd_set fds;
struct timeval tv;
int r;
FD_ZERO(&fds);
FD_SET(fd, &fds);
/* Timeout. */
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select(fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
if (EINTR == errno)
continue;
errno_exit("select");
}
if (0 == r) {
fprintf(stderr, "select timeout\n");
exit(EXIT_FAILURE);
}
#endif
if (read_frame())
break;
/* EAGAIN - continue select loop. */
}
}
}
int start_capturing(int fd)
{
unsigned int i;
enum v4l2_buf_type type;
for (i = 0; i < n_buffers; ++i) {
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
// errno_exit("VIDIOC_QBUF");
return -1;
}
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl(fd, VIDIOC_STREAMON, &type))
errno_exit("VIDIOC_STREAMON");
return 1;
}
int V4LCapture::read_frame(uint8_t **data, struct v4l2_buffer *buf)
{
if (param.replay_mode) {
int ret = load_frame(&(buffers->start), &buf_now);
*data = (uint8_t *) buffers->start;
*buf = buf_now;
return ret;
}
if( (int)buf_now.index != -1 ) {
if( -1 == xioctl(fd, VIDIOC_QBUF, &buf_now) ){
errno_exit("VIDIOC_QBUF");
}
}
CLEAR( buf_now );
buf_now.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf_now.memory = V4L2_MEMORY_MMAP;
if( -1 == xioctl(fd, VIDIOC_DQBUF, &buf_now) ){
if (EAGAIN == errno) {
return -1;
errno_exit("EAGAIN");
} else if (EINVAL == errno) {
errno_exit("EINVAL");
} else if (ENOMEM == errno) {
errno_exit("ENOMEM");
} else if (EIO == errno) {
errno_exit("EIO");
} else {
errno_exit("VIDIOC_DQBUF");
}
}
struct buffer *pbuf = &buffers[buf_now.index];
if (param.record_prefix != NULL) {
dump_frame((uint8_t *)pbuf->start, &buf_now);
}
*data = (uint8_t *) pbuf->start;
*buf = buf_now;
return buf_now.bytesused;
}
static void uninit_device(void)
{
int i;
switch (io) {
case IO_METHOD_READ:
free(buffers[0].start);
break;
case V4L2_MEMORY_MMAP:
for (i = 0; i < n_buffers; ++i)
if (-1 ==
v4l2_munmap(buffers[i].start, buffers[i].length))
errno_exit("munmap");
break;
case V4L2_MEMORY_USERPTR:
for (i = 0; i < n_buffers; ++i)
free(buffers[i].start);
break;
}
free(buffers);
}
