AP_HAL_Linux: add get_pixel_formats() to VideoIn

This function is being added in order to be used in the next commits,
where we will add support for OpticalFlow on MinnowBoardMax.
This commit is contained in:
Ricardo de Almeida Gonzaga 2015-05-11 00:42:41 +00:00 committed by Lucas De Marchi
parent 979184607d
commit 27f909319e
2 changed files with 16 additions and 0 deletions

View File

@ -159,6 +159,20 @@ bool VideoIn::allocate_buffers(uint32_t nbufs)
return true; return true;
} }
void VideoIn::get_pixel_formats(std::vector<uint32_t> *formats)
{
struct v4l2_fmtdesc fmtdesc;
memset(&fmtdesc, 0, sizeof fmtdesc);
fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
while (ioctl(_fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
formats->insert(formats->begin(), fmtdesc.pixelformat);
fmtdesc.index++;
}
}
bool VideoIn::set_format(uint32_t *width, uint32_t *height, uint32_t *format, bool VideoIn::set_format(uint32_t *width, uint32_t *height, uint32_t *format,
uint32_t *bytesperline, uint32_t *sizeimage) uint32_t *bytesperline, uint32_t *sizeimage)
{ {

View File

@ -16,6 +16,7 @@
#include "AP_HAL_Linux.h" #include "AP_HAL_Linux.h"
#include <linux/videodev2.h> #include <linux/videodev2.h>
#include <vector>
struct buffer { struct buffer {
unsigned int size; unsigned int size;
@ -43,6 +44,7 @@ public:
void init(); void init();
bool open_device(const char *device_path, uint32_t memtype); bool open_device(const char *device_path, uint32_t memtype);
bool allocate_buffers(uint32_t nbufs); bool allocate_buffers(uint32_t nbufs);
void get_pixel_formats(std::vector<uint32_t> *formats);
bool set_format(uint32_t *width, uint32_t *height, uint32_t *format, bool set_format(uint32_t *width, uint32_t *height, uint32_t *format,
uint32_t *bytesperline, uint32_t *sizeimage); uint32_t *bytesperline, uint32_t *sizeimage);
bool set_crop(uint32_t left, uint32_t top, bool set_crop(uint32_t left, uint32_t top,