Ignore:
Timestamp:
Feb 19, 2020, 2:19:28 PM (5 years ago)
Author:
Sanahuja Guillaume
Message:

V4l modifs

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSensorActuator/src/V4LCamera.cpp

    r349 r350  
    4040V4LCamera::V4LCamera(string name,uint8_t camera_index, uint16_t width, uint16_t height,
    4141                     Image::Type::Format format, uint8_t priority)
    42     : Thread(getFrameworkManager(), name, priority),
     42    : Thread(getFrameworkManager(), name, priority,1024*1024),
    4343      Camera(name, width, height, format) {
    4444 
    … …  
    5252
    5353  if(format == Image::Type::Format::UYVY) {
    54     if(init(width,height,V4L2_PIX_FMT_UYVY) == -1) {
     54    if(Init(width,height,V4L2_PIX_FMT_UYVY) == -1) {
    5555     Thread::Err("initialisation failed\n");
    5656    }
    5757  } else if (format == Image::Type::Format::YUYV) {
    58     if(init(width,height,V4L2_PIX_FMT_YUYV) == -1) {
     58    if(Init(width,height,V4L2_PIX_FMT_YUYV) == -1) {
    5959     Thread::Err("initialisation failed\n");
    6060    }
    … …  
    6363  }
    6464 
    65 
    66   allocBuffers();
    67 
     65  dQueuedBuffer.index=-1;//mark current buffer not valid for initialisation
     66  useMemoryUsrPtr=false;
     67  if(useMemoryUsrPtr) {
     68    AllocUserBuffers();
     69  } else {
     70    AllocV4LBuffers();
     71  }
     72 
    6873  for (int i=0;i < nbBuffers;i++) {
    69     struct v4l2_buffer buf;
    70     memset(&buf, 0, sizeof (v4l2_buffer));
    71 
    72     buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    73     buf.memory      = V4L2_MEMORY_MMAP;
    74     buf.index       = (unsigned long)i;
    75 
    76     if (-1 == xioctl (device, VIDIOC_QBUF, &buf)) {
    77       Thread::Err("VIDIOC_QBUF error\n");
    78     }
     74    QueueBuffer(i);
    7975  }
    8076
    … …  
    10399
    104100V4LCamera::~V4LCamera() {
    105   for (int i = 0; i < nbBuffers; i++) {
    106        //FreeFunction((char*)buffers[i].start);
     101  if(useMemoryUsrPtr) {
     102    for (int i = 0; i < nbBuffers; i++) {
     103      FreeFunction((char*)buffers[i]);
     104    }
     105  } else {
     106      FreeFunction(imageData);
    107107  }
    108108  SafeStop();
    … …  
    111111}
    112112
    113 int V4LCamera::init(int width, int height,unsigned long colorspace) {
     113int V4LCamera::Init(int width, int height,unsigned long colorspace) {
    114114  struct v4l2_capability cap;
    115115  memset(&cap, 0, sizeof (v4l2_capability));
    … …  
    245245}
    246246*/
    247 int V4LCamera::allocBuffers() {
     247int V4LCamera::AllocV4LBuffers() {
    248248  struct v4l2_requestbuffers req;
    249249  memset(&req, 0, sizeof (v4l2_requestbuffers));
    … …  
    309309};
    310310
    311 int V4LCamera::AllocBuffers(void) {
     311int V4LCamera::AllocUserBuffers(void) {
    312312  struct v4l2_requestbuffers req;
    313313  memset(&req, 0, sizeof (v4l2_requestbuffers));
    … …  
    333333  return 1;
    334334};
    335 
    336 int V4LCamera::cvGrabFrame(void) {
    337     unsigned int count;
    338 
    339     count = 1;
    340 
    341     while (count-- > 0) {
    342         for (;;) {
    343             fd_set fds;
    344             struct timeval tv;
    345             int r;
    346 
    347             FD_ZERO (&fds);
    348             FD_SET (device, &fds);
    349 
    350             /* Timeout. */
    351             tv.tv_sec = 2;
    352             tv.tv_usec = 0;
    353 
    354             r = select (device+1, &fds, NULL, NULL, &tv);
    355 
    356             if (-1 == r) {
    357                 if (EINTR == errno)
    358                     continue;
    359 
    360                 perror ("select");
    361             }
    362 
    363             if (0 == r) {
    364                 fprintf (stderr, "select timeout\n");
    365 
    366                 /* end the infinite loop */
    367                 break;
    368             }
    369 
    370             if (read_frame_v4l2 ())
    371                 break;
    372         }
    373     }
    374     return(1);
    375 }
    376 
    377 int V4LCamera::read_frame_v4l2(void) {
     335/*
     336int V4LCamera::GrabFrame(void) {
     337    //queue previous buffer
     338    if(QueueBuffer(bufferIndex)<0) return -1;
     339   
     340    fd_set fds;
     341    struct timeval tv;
     342    FD_ZERO (&fds);
     343    FD_SET (device, &fds);
     344
     345    tv.tv_sec = 0;
     346    tv.tv_usec = 100000;
     347
     348    int r = select (device+1, &fds, NULL, NULL, &tv);
     349
     350    if (-1 == r) {
     351        char errorMsg[256];
     352        Thread::Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
     353        return -1;
     354    }
     355
     356    if (0 == r) {
     357        Thread::Err("select timeout\n");
     358        return -1;
     359    }
     360
    378361    struct v4l2_buffer buf;
    379362    memset(&buf, 0, sizeof (v4l2_buffer));
    380 
    381363    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    382     buf.memory = V4L2_MEMORY_MMAP;
    383 
    384     if (-1 == xioctl (device, VIDIOC_DQBUF, &buf)) {
    385         switch (errno) {
    386         case EAGAIN:
    387             return 0;
    388 
    389         case EIO:
    390             /* Could ignore EIO, see spec. */
    391 
    392             /* fall through */
    393 
    394         default:
    395             /* display the error and stop processing */
    396             perror ("VIDIOC_DQBUF");
    397             return 1;
    398         }
    399    }
    400 
    401    if(buf.index >= nbBuffers) {
    402      Thread::Err("buf.index >= capture->req.count\n");
    403    }
    404 
    405    bufferIndex = buf.index;
    406 
    407    if (-1 == xioctl (device, VIDIOC_QBUF, &buf))
    408        perror ("VIDIOC_QBUF");
    409 
     364    buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
     365
     366    //get last captured image
     367    int prevDQbuf=-1;
     368    for(int i=0;i<nbBuffers;i++) {
     369        if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
     370            if (errno==EAGAIN) {
     371                break;
     372            } else {
     373                Thread::Err("VIDIOC_DQBUF xioctl\n");
     374                return -1;
     375            }
     376       } else {
     377           if(prevDQbuf!=-1) {
     378               QueueBuffer(prevDQbuf);
     379           }
     380           for (int i=0; i<nbBuffers; i++) {
     381               if((void*)(buf.m.userptr)==buffers[i]) {
     382                   prevDQbuf=i;
     383                   bufferIndex=i;
     384                   break;
     385               }
     386           }
     387       }
     388    }
     389   
    410390   return 1;
     391}
     392*/
     393int V4LCamera::cvGrabFrame(void) {
     394  if(QueueBuffer(&dQueuedBuffer)<0) return -1;
     395   
     396  fd_set fds;
     397  struct timeval tv;
     398  FD_ZERO (&fds);
     399  FD_SET (device, &fds);
     400
     401  tv.tv_sec = 0;
     402  tv.tv_usec = 100000;
     403
     404  int r=select(device+1, &fds, NULL, NULL, &tv);
     405
     406  if (r==-1) {
     407    char errorMsg[256];
     408    Thread::Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
     409    return -1;
     410  }
     411
     412  if (r==0) {
     413    Thread::Err("select timeout\n");
     414    return -1;
     415  }
     416 
     417  struct v4l2_buffer buf;
     418  memset(&buf, 0, sizeof (v4l2_buffer));
     419  buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
     420  if(useMemoryUsrPtr) {
     421    buf.memory=V4L2_MEMORY_USERPTR;
     422  } else {
     423    buf.memory=V4L2_MEMORY_MMAP;
     424  }
     425
     426   //get last captured image
     427  dQueuedBuffer.index=-1;
     428  for(int i=0;i<nbBuffers;i++) {
     429    if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
     430      if (errno==EAGAIN) {//when no new buffer is available for reading (non blocking)
     431        //Printf("%s eagain\n",Thread::ObjectName().c_str());
     432        break;
     433      } else {
     434        Thread::Err("VIDIOC_DQBUF xioctl\n");
     435        return -1;
     436      }
     437    } else {
     438      //Printf("%s %i dqueued\n",Thread::ObjectName().c_str(),buf.index);
     439      //Printf("%i %x %i %i\n",buf.bytesused,buf.flags,buf.field,buf.sequence);
     440      if(dQueuedBuffer.index!=-1) {
     441        //Printf("%s %i queued\n",Thread::ObjectName().c_str(),dQueuedBuffer.index);
     442        QueueBuffer(&dQueuedBuffer);
     443      }
     444      dQueuedBuffer=buf;
     445    }
     446  }
     447  //Printf("%s %i\n",Thread::ObjectName().c_str(),dQueuedBuffer.index);
     448
     449  return 1;
    411450}
    412451
    413452void V4LCamera::Run(void) {
    414453  Time cam_time, new_time, fpsNow, fpsPrev;
    415   //IplImage *img; // raw image
    416454  int fpsCounter = 0;
    417455
    418   // init image old
    419   if (!cvGrabFrame()) {
    420     Printf("Could not grab a frame\n");
    421   }
    422456  cam_time = GetTime();
    423457  fpsPrev = cam_time;
    … …  
    471505   
    472506    // cam pictures
    473     cvRetrieveRawFrame();
    474507    if (!cvGrabFrame()) {
    475508      Printf("Could not grab a frame\n");
    476509    }
    477     new_time = GetTime();
     510   
     511    //copy to CMEM allocated buffer if necessary
     512    if(!useMemoryUsrPtr) {
     513      memcpy(imageData,(char *)buffers[dQueuedBuffer.index],output->GetDataType().GetSize());
     514    }
    478515       
    479516    //check for ps3eye deconnection in hds uav
     517    new_time = GetTime();
    480518    if(new_time-cam_time>100*1000*1000) {
    481       Thread::Warn("delta trop grand\n");
     519      Thread::Warn("delta t trop grand\n");
    482520      hasProblems=true;
    483     }
     521    }/*
     522    if(hasProblems==false) {
     523      struct v4l2_format form;
     524      form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     525      xioctl(device, VIDIOC_G_FMT,&form);
     526      if(xioctl (device, VIDIOC_G_FMT,&form)<0) {
     527        Thread::Warn("camera disconnected\n");
     528        hasProblems=true;
     529      }
     530    }*/
    484531
    485532    output->GetMutex();
    486    
    487533    output->buffer=imageData;
    488534    output->ReleaseMutex();
    … …  
    494540}
    495541
    496 void V4LCamera::cvRetrieveRawFrame(void) {
    497   memcpy(imageData,(char *)buffers[bufferIndex],output->GetDataType().GetSize());
     542int V4LCamera::QueueBuffer(struct v4l2_buffer *buf) {
     543  if(buf->index>=0 && buf->index<nbBuffers) {
     544    int ret=xioctl (device, VIDIOC_QBUF,buf);
     545    if (ret==-1) {
     546      Thread::Err("VIDIOC_QBUF xioctl %s\n",strerror(-ret));
     547      return -1;
     548    }
     549  }
     550  return 0;
    498551}
    499552
    500553int V4LCamera::QueueBuffer(int index) {
    501     struct v4l2_buffer buf;
    502     if(index>=0 && index<nbBuffers) {
    503         memset(&buf, 0, sizeof (v4l2_buffer));
    504         buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    505         buf.memory      = V4L2_MEMORY_USERPTR;
    506         buf.index       = (unsigned long)index;
    507         buf.m.userptr=(unsigned long)(buffers[index]);
    508         buf.length=output->GetDataType().GetSize();
    509        
    510         int ret=xioctl (device, VIDIOC_QBUF, &buf);
    511         if (ret==-1) {
    512             Thread::Err("VIDIOC_QBUF xioctl %s\n",strerror(-ret));
    513             return -1;
    514         }
    515     }
    516     return 0;
    517 }
    518 
    519 int V4LCamera::GrabFrame(void) {
    520     //queue previous buffer
    521     if(QueueBuffer(bufferIndex)<0) return -1;
    522    
    523     fd_set fds;
    524     struct timeval tv;
    525     FD_ZERO (&fds);
    526     FD_SET (device, &fds);
    527 
    528     tv.tv_sec = 0;
    529     tv.tv_usec = 100000;
    530 
    531     int r = select (device+1, &fds, NULL, NULL, &tv);
    532 
    533     if (-1 == r) {
    534         char errorMsg[256];
    535         Thread::Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
    536         return -1;
    537     }
    538 
    539     if (0 == r) {
    540         Thread::Err("select timeout\n");
    541         return -1;
    542     }
    543 
    544     struct v4l2_buffer buf;
    545     memset(&buf, 0, sizeof (v4l2_buffer));
    546     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    547     buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
    548 
    549     //get last captured image
    550     int prevDQbuf=-1;
    551     for(int i=0;i<4;i++) {
    552         if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
    553             if (errno==EAGAIN) {
    554                 break;
    555             } else {
    556                 Thread::Err("VIDIOC_DQBUF xioctl\n");
    557                 return -1;
    558             }
    559        } else {
    560            if(prevDQbuf!=-1) {
    561                QueueBuffer(prevDQbuf);
    562            }
    563            for (int i=0; i<nbBuffers; i++) {
    564                if((void*)(buf.m.userptr)==buffers[i]) {
    565                    prevDQbuf=i;
    566                    bufferIndex=i;
    567                    break;
    568                }
    569            }
    570        }
    571     }
    572    
    573    return 1;
     554  struct v4l2_buffer buf;
     555  memset(&buf, 0, sizeof (v4l2_buffer));
     556 
     557  buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
     558  buf.index=(unsigned long)index;
     559  if(useMemoryUsrPtr) {
     560    buf.memory=V4L2_MEMORY_USERPTR;
     561    buf.m.userptr=(unsigned long)(buffers[index]);
     562    buf.length=output->GetDataType().GetSize();
     563  } else {
     564    buf.memory=V4L2_MEMORY_MMAP;
     565  }
     566  return QueueBuffer(&buf);
    574567}
    575568
Note: See TracChangeset for help on using the changeset viewer.