src/CaptureThread.c File Reference

#include <stdio.h>
#include <stdlib.h>
#include <linux/ioctl.h>
#include <unistd.h>
#include <string.h>
#include <poll.h>
#include <errno.h>
#include <syslog.h>
#include "CaptureThread.h"
#include "Capture.h"
#include "CaptureDebug.h"
#include "rgb133v4l2.h"

Functions

void * CaptureThreadFunc (void *arg)
int CreateCaptureThread (psCapture pCapture, psCaptureThreadArgs pCaptureThreadArgs)
void JoinCaptureThread (psCapture pCapture)
void FreeCaptureThreadArgs (psCaptureThreadArgs *pCaptureThreadArgs)

Variables

int CaptureStop
int CaptureThreadStop = 0

Function Documentation

void* CaptureThreadFunc ( void *  arg  ) 

Implementation of CaptureThreadFunc.

00042 {
00043    psCaptureThreadArgs pCaptureThreadArgs = (psCaptureThreadArgs)arg;
00044    psCapture pCapture = pCaptureThreadArgs->pCapture;
00045    eIOType IOType = pCapture->ioType;
00046 
00047    struct v4l2_buffer buf;
00048    enum v4l2_buf_type buf_type;
00049 
00050    int i = 0;
00051    int ret = 0;
00052    int frame = 0;
00053 
00054    if(IOType == IO_MMAP)
00055    {
00056       CapError("TODO: Setup mmap options...\n");
00057    }
00058 
00059    switch(IOType)
00060    {
00061       case IO_READ:
00062          CapLog("Start capture on %s: %d\n", pCapture->caps.driver, pCapture->fd);
00063          CapMessage("pData(%p), DataLength(%u)\n", pCapture->pBuffers[0].pData, pCapture->pBuffers[0].DataLength);
00064          while(!CaptureThreadStop)
00065          {
00066             ret = read( pCapture->fd, pCapture->pBuffers[0].pData, pCapture->pBuffers[0].DataLength );
00067             if(ret < 0)
00068             {
00069                switch(errno)
00070                {
00071                   case EWOULDBLOCK:
00072                      //CapError("EWOULDBLOCK...\n");
00073                      break;
00074                   case EIO:
00075                      /* Could ignore EIO, see spec. */
00076                      /* fall through */
00077                   default:
00078                      CapError("Failed to read from Capture Device - %d: %s\n",
00079                            errno, pCapture->caps.driver);
00080                      return 0;
00081                }
00082             }
00083             frame++;
00084             if ((pCapture->capture_count > 0) && (frame >= pCapture->capture_count))
00085             {
00086                CapMessage("Captured %d frames, stopping, as requested.\n", pCapture->capture_count);
00087                CaptureThreadStop = 1;
00088             }
00089          }
00090          break;
00091       case IO_MMAP:
00092          CapLog("Start capture on %s: %d\n", pCapture->caps.driver, pCapture->fd);
00093          CapMessage("pData(%p), DataLength(%u)\n", pCapture->pBuffers[0].pData, pCapture->pBuffers[0].DataLength);
00094 
00095          /* Initial queue of the buffers */
00096          for(i=0; i<pCapture->buffers; ++i)
00097          {
00098              memset( &buf, 0, sizeof(buf) );
00099              buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00100              buf.memory = V4L2_MEMORY_MMAP;
00101              buf.index = i;
00102 
00103              CapError("Q buffer[%d]\n", buf.index);
00104              if((ret = ioctl(pCapture->fd, VIDIOC_QBUF, &buf)) < 0)
00105              {
00106                  CapError("VIDIOC_QBUF failed: %d\n", ret);
00107                  return 0;
00108              }
00109          }
00110 
00111          buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00112              CapError("STREAMON\n");
00113          if((ret = ioctl(pCapture->fd, VIDIOC_STREAMON, &buf_type)) < 0)
00114          {
00115              CapError("VIDIOC_STREAMON failed: %d\n", ret);
00116              return 0;
00117          }
00118 
00119          openlog("CaptureApp", 0, LOG_LOCAL0);
00120 
00121          while(!CaptureThreadStop)
00122          {
00123             struct pollfd fd;
00124             fd.fd = pCapture->fd;
00125             fd.events = POLLIN|POLLPRI;
00126             fd.revents = 0;
00127 
00128             /* Wait for data, max 500ms */
00129             if(poll(&fd, 1, 500) > 0)
00130             {
00131                if( fd.revents & (POLLIN|POLLPRI) )
00132                {
00133                   memset( &buf, 0, sizeof(buf) );
00134                   buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00135                   buf.memory = V4L2_MEMORY_MMAP;
00136 
00137                   /* dequeue frame */
00138                   if((ret = ioctl(pCapture->fd, VIDIOC_DQBUF, &buf)) < 0)
00139                   {
00140                      switch( errno )
00141                      {
00142                      case EAGAIN:
00143                         continue;
00144                      case EIO:
00145                      default:
00146                         CapError("Failed to dequeue buffer[%d]: %d\n", buf.index, ret);
00147                         return 0;
00148                      }
00149                   }
00150 
00151                   if(pCapture->timestamp)
00152                   {
00153                      static int prev = -1, this = -1;
00154                      static struct timeval tv_this = {0, 0}, tv_prev = {0, 0};
00155 
00156                      //CapError("DQ buffer[%d].seq(%d) ts(%d.%06d)\n",
00157                      //      buf.index, buf.sequence,
00158                      //      buf.timestamp.tv_sec, buf.timestamp.tv_usec);
00159                      this = buf.sequence;
00160                      tv_this.tv_sec = buf.timestamp.tv_sec;
00161                      tv_this.tv_usec = buf.timestamp.tv_usec;
00162 
00163                      if(prev != -1)
00164                      {
00165                         if(this != (prev+1))
00166                         {
00167                            CapError("DQ buffer[%d].seq(%d)(%d) mismatch\n", buf.index, this, prev);
00168                         }
00169                      }
00170                      prev = this;
00171 
00172                      if(tv_prev.tv_sec != 0 &&
00173                         tv_prev.tv_usec != 0)
00174                      {
00175                         signed int diff_s = tv_this.tv_sec - tv_prev.tv_sec;
00176                         signed int diff_us = tv_this.tv_usec - tv_prev.tv_usec;
00177                         if(diff_us < 0)
00178                         {
00179                            diff_s--;
00180                            diff_us += 1000000;
00181                         }
00182                         if(diff_s ||
00183                            (diff_us < 16600 || diff_us > 16700))
00184                         {
00185                            CapError("DQ buffer[%d].ts(%d.%06d)(%d.%06d) validity(%d.%06d) frame(%d)\n",
00186                                  buf.index, (int)tv_this.tv_sec, (int)tv_this.tv_usec,
00187                                  (int)tv_prev.tv_sec, (int)tv_prev.tv_usec,
00188                                  diff_s, diff_us, frame);
00189                         }
00190                      }
00191                      tv_prev.tv_sec = tv_this.tv_sec;
00192                      tv_prev.tv_usec = tv_this.tv_usec;
00193                   }
00194 
00195                   CapMessage("Dequeued buffer index: %d\n", buf.index);
00196 
00197                   if(buf.index >= pCapture->buffers ) {
00198                      CapError("Invalid buffer index: %d\n", buf.index);
00199                      return 0;
00200                   }
00201 
00202                   frame++;
00203                   if ((pCapture->capture_count > 0) && (frame >= pCapture->capture_count))
00204                   {
00205                      CapMessage("Captured %d frames, stopping, as requested.\n", pCapture->capture_count);
00206                      CaptureThreadStop = 1;
00207                   }
00208 
00209                   /* Requeue buffer */
00210                   //syslog(LOG_ERR, "*** Requeueing Buffer ***\n");
00211                   CapMessage("Requeuing buffer %d back to driver.\n", buf.index);
00212                   //CapError("Q buffer[%d]\n", buf.index);
00213                   if((ret = ioctl(pCapture->fd, VIDIOC_QBUF, &buf)) < 0)
00214                   {
00215                      CapError("Failed to requeue buffer[%d]: %d\n", buf.index, ret);
00216                      return 0;
00217                   }
00218                }
00219             }
00220             if(pCapture->delay)
00221                usleep(pCapture->delay*1000);
00222          }
00223 
00224          buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00225              CapError("STREAMOFF\n");
00226          if((ret = ioctl(pCapture->fd, VIDIOC_STREAMOFF, &buf_type)) < 0)
00227          {
00228              CapError("VIDIOC_STREAMOFF failed: %d", ret);
00229          }
00230 
00231          /* Make sure every buffer is dequeued */
00232          for(i=0; i<pCapture->buffers; i++)
00233          {
00234              memset( &buf, 0, sizeof(buf) );
00235              buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00236              buf.memory = V4L2_MEMORY_MMAP;
00237              ioctl(pCapture->fd, VIDIOC_DQBUF, &buf);
00238              CapError("DQ buffer[%d]\n", buf.index);
00239          }
00240 
00241          break;
00242       case IO_USERPTR:
00243          CapLog("Start capture on %s: %d\n", pCapture->caps.driver, pCapture->fd);
00244          CapMessage("pData[0](%p), DataLength[0](%u), pData[1](%p), DataLength[1](%u)\n"
00245                     "pData[2](%p), DataLength[2](%u), pData[3](%p), DataLength[3](%u)\n",
00246                pCapture->pBuffers[0].pData, pCapture->pBuffers[0].DataLength,
00247                pCapture->pBuffers[1].pData, pCapture->pBuffers[1].DataLength,
00248                pCapture->pBuffers[2].pData, pCapture->pBuffers[2].DataLength,
00249                pCapture->pBuffers[3].pData, pCapture->pBuffers[3].DataLength);
00250 
00251          /* Initial queue of the buffers */
00252          for(i=0; i<pCapture->buffers; ++i)
00253          {
00254             memset( &buf, 0, sizeof(buf) );
00255             buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00256             buf.memory = V4L2_MEMORY_USERPTR;
00257             buf.index = i;
00258             buf.m.userptr = (unsigned long)pCapture->pBuffers[i].pData;
00259             buf.length = pCapture->pBuffers[i].DataLength;
00260 
00261             CapError("Q buffer[%d]\n", buf.index);
00262             if((ret = ioctl(pCapture->fd, VIDIOC_QBUF, &buf)) < 0)
00263             {
00264                CapError("VIDIOC_QBUF failed: %d\n", ret);
00265                return 0;
00266             }
00267          }
00268 
00269          buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00270              CapError("STREAMON\n");
00271          if((ret = ioctl(pCapture->fd, VIDIOC_STREAMON, &buf_type)) < 0)
00272          {
00273              CapError("VIDIOC_STREAMON failed: %d\n", ret);
00274              return 0;
00275          }
00276 
00277          while(!CaptureThreadStop)
00278          {
00279             struct pollfd fd;
00280             fd.fd = pCapture->fd;
00281             fd.events = POLLIN|POLLPRI;
00282             fd.revents = 0;
00283 
00284             /* Wait for data, max 500ms */
00285             if(poll(&fd, 1, 500) > 0)
00286             {
00287                if( fd.revents & (POLLIN|POLLPRI) )
00288                {
00289                   memset( &buf, 0, sizeof(buf) );
00290                   buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00291                   buf.memory = V4L2_MEMORY_USERPTR;
00292 
00293                   /* dequeue frame */
00294                   if((ret = ioctl(pCapture->fd, VIDIOC_DQBUF, &buf)) < 0)
00295                   {
00296                      switch( errno )
00297                      {
00298                      case EAGAIN:
00299                         continue;
00300                      case EIO:
00301                      default:
00302                         CapError("Failed to dequeue buffer[%d]: %d\n", buf.index, ret);
00303                         return 0;
00304                      }
00305                   }
00306 
00307                   if(pCapture->timestamp)
00308                   {
00309                      static int prev = -1, this = -1;
00310                      static struct timeval tv_this = {0, 0}, tv_prev = {0, 0};
00311 
00312                      this = buf.sequence;
00313                      tv_this.tv_sec = buf.timestamp.tv_sec;
00314                      tv_this.tv_usec = buf.timestamp.tv_usec;
00315 
00316                      if(prev != -1)
00317                      {
00318                         if(this != (prev+1))
00319                         {
00320                            CapError("DQ buffer[%d].seq(%d)(%d) mismatch\n", buf.index, this, prev);
00321                         }
00322                      }
00323                      prev = this;
00324 
00325                      if(tv_prev.tv_sec != 0 &&
00326                         tv_prev.tv_usec != 0)
00327                      {
00328                         signed int diff_s = tv_this.tv_sec - tv_prev.tv_sec;
00329                         signed int diff_us = tv_this.tv_usec - tv_prev.tv_usec;
00330                         if(diff_us < 0)
00331                         {
00332                            diff_s--;
00333                            diff_us += 1000000;
00334                         }
00335                         if(diff_s ||
00336                            (diff_us < 16600 || diff_us > 16700))
00337                         {
00338                            CapError("DQ buffer[%d].ts(%d.%06d)(%d.%06d) validity(%d.%06d) frame(%d)\n",
00339                                  buf.index, (int)tv_this.tv_sec, (int)tv_this.tv_usec,
00340                                  (int)tv_prev.tv_sec, (int)tv_prev.tv_usec,
00341                                  diff_s, diff_us, frame);
00342                         }
00343                      }
00344                      tv_prev.tv_sec = tv_this.tv_sec;
00345                      tv_prev.tv_usec = tv_this.tv_usec;
00346                   }
00347 
00348                   CapMessage("Dequeued buffer index: %d\n", buf.index);
00349 
00350                   if(buf.index >= pCapture->buffers ) {
00351                      CapError("Invalid buffer index: %d\n", buf.index);
00352                      return 0;
00353                   }
00354 
00355                   frame++;
00356                   if ((pCapture->capture_count > 0) && (frame >= pCapture->capture_count))
00357                   {
00358                      CapMessage("Captured %d frames, stopping, as requested.\n", pCapture->capture_count);
00359                      CaptureThreadStop = 1;
00360                   }
00361 
00362                   /* Requeue buffer */
00363                   CapMessage("Requeuing buffer %d back to driver.\n", buf.index);
00364                   if((ret = ioctl(pCapture->fd, VIDIOC_QBUF, &buf)) < 0)
00365                   {
00366                      CapError("Failed to requeue buffer[%d]: %d\n", buf.index, ret);
00367                      return 0;
00368                   }
00369                }
00370             }
00371             if(pCapture->delay)
00372                usleep(pCapture->delay*1000);
00373          }
00374 
00375          buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00376              CapError("STREAMOFF\n");
00377          if((ret = ioctl(pCapture->fd, VIDIOC_STREAMOFF, &buf_type)) < 0)
00378          {
00379              CapError("VIDIOC_STREAMOFF failed: %d", ret);
00380          }
00381 
00382          /* Make sure every buffer is dequeued */
00383          for(i=0; i<pCapture->buffers; i++)
00384          {
00385              memset( &buf, 0, sizeof(buf) );
00386              buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00387              buf.memory = V4L2_MEMORY_USERPTR;
00388              ioctl(pCapture->fd, VIDIOC_DQBUF, &buf);
00389              CapError("DQ buffer[%d]\n", buf.index);
00390          }
00391 
00392          break;
00393       default:
00394          CapError("Invalid IO method: %d\n", IOType);
00395          break;
00396    }
00397 
00398    CapMessage("Capture Thread Finished\n");
00399 
00400    if(CaptureThreadStop)
00401       CaptureStop = 1;
00402 }

int CreateCaptureThread ( psCapture  pCapture,
psCaptureThreadArgs  pCaptureThreadArgs 
)

Implementation of CreateCaptureThread.

00408 {
00409    int ret = 0;
00410 
00411    if(pCaptureThreadArgs)
00412    {
00413       CapError("Capture Thread structure(%p) may be initialised, bailing out\n", pCaptureThreadArgs);
00414       return -1;
00415    }
00416 
00417    pCaptureThreadArgs = malloc(sizeof(sCaptureThreadArgs));
00418    if(pCaptureThreadArgs == 0)
00419       return -1;
00420 
00421    memset(pCaptureThreadArgs, 0, sizeof(sCaptureThreadArgs));
00422    pCaptureThreadArgs->pCapture = pCapture;
00423    ret = pthread_create(&pCapture->CaptureThread, NULL, CaptureThreadFunc, (void*)pCaptureThreadArgs);
00424    return 0;
00425 }

void FreeCaptureThreadArgs ( psCaptureThreadArgs pCaptureThreadArgs  ) 

Implementation of FreeCaptureThreadArgs.

00441 {
00442    if(pCaptureThreadArgs && *pCaptureThreadArgs)
00443    {
00444       free(*pCaptureThreadArgs);
00445       *pCaptureThreadArgs = 0;
00446    }
00447 }

void JoinCaptureThread ( psCapture  pCapture  ) 

Implementation of JoinCaptureThread.

00431 {
00432    CapMessage("Waiting for capture thread to join...\n");
00433    pthread_join(pCapture->CaptureThread, 0);
00434    CapMessage("Capture thread has joined...\n");
00435 }


Variable Documentation

int CaptureStop

int CaptureThreadStop = 0


Generated on Fri Jan 20 10:36:57 2017 for Vision Utils by  doxygen 1.4.7