From 14766561afed011fde781c5eb0f3db34eea16ade Mon Sep 17 00:00:00 2001 From: Alejandro Soto Date: Wed, 13 Sep 2023 03:56:25 -0600 Subject: [PATCH] pin_control: implement v4l2 capture --- pin_control/CMakeLists.txt | 4 +- pin_control/include/pin_control.h | 8 +- pin_control/src/v4l2.c | 226 ++++++++++++++++++++++++++++++ 3 files changed, 230 insertions(+), 8 deletions(-) create mode 100644 pin_control/src/v4l2.c diff --git a/pin_control/CMakeLists.txt b/pin_control/CMakeLists.txt index 93a5805..7ce7bd8 100644 --- a/pin_control/CMakeLists.txt +++ b/pin_control/CMakeLists.txt @@ -4,7 +4,7 @@ project(PinControl) find_package(PkgConfig REQUIRED) pkg_check_modules(LIBGPIOD REQUIRED libgpiod) -add_library(pin_control SHARED src/pin_control.c) +add_library(pin_control SHARED src/pin_control.c src/v4l2.c) target_include_directories(pin_control PUBLIC include/ PRIVATE ${LIBGPIOD_INCLUDE_DIRS}) target_link_libraries(pin_control PUBLIC ${LIBGPIOD_LIBRARIES}) @@ -13,4 +13,4 @@ set_target_properties(pin_control PROPERTIES POSITION_INDEPENDENT_CODE ON) target_compile_options(pin_control PUBLIC -Wall -Wextra -Werror) install(TARGETS pin_control LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) -install(FILES include/pin_control.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) \ No newline at end of file +install(FILES include/pin_control.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) diff --git a/pin_control/include/pin_control.h b/pin_control/include/pin_control.h index 13467e4..8684c6a 100644 --- a/pin_control/include/pin_control.h +++ b/pin_control/include/pin_control.h @@ -2,19 +2,15 @@ #define GPIO_CONTROL_H int init_gpio(); - void cleanup_gpio(); int turn_on_pin(int pin); - int turn_off_pin(int pin); - int probe_pin(int pin); - int turn_on_all_pins(); - int turn_off_all_pins(); - int wait_for_button_press(const unsigned pins[static 5]); +int capture_image(const char *out); + #endif /* GPIO_CONTROL_H */ diff --git a/pin_control/src/v4l2.c b/pin_control/src/v4l2.c new file mode 100644 index 0000000..dca554d --- /dev/null +++ b/pin_control/src/v4l2.c @@ -0,0 +1,226 @@ +// Derivado de https://gist.github.com/jayrambhia/5866483 + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +static int xioctl(int fd, int request, void *arg) +{ + int r; + + do r = ioctl (fd, request, arg); + while (-1 == r && EINTR == errno); + + return r; +} + +static int print_caps(int fd) +{ + struct v4l2_capability caps = {}; + if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &caps)) + { + perror("Querying Capabilities"); + return 1; + } + + printf( "Driver Caps:\n" + " Driver: \"%s\"\n" + " Card: \"%s\"\n" + " Bus: \"%s\"\n" + " Version: %d.%d\n" + " Capabilities: %08x\n", + caps.driver, + caps.card, + caps.bus_info, + (caps.version>>16)&&0xff, + (caps.version>>24)&&0xff, + caps.capabilities); + + + struct v4l2_cropcap cropcap = {0}; + cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (-1 == xioctl (fd, VIDIOC_CROPCAP, &cropcap)) + { + perror("Querying Cropping Capabilities"); + return 1; + } + + printf( "Camera Cropping:\n" + " Bounds: %dx%d+%d+%d\n" + " Default: %dx%d+%d+%d\n" + " Aspect: %d/%d\n", + cropcap.bounds.width, cropcap.bounds.height, cropcap.bounds.left, cropcap.bounds.top, + cropcap.defrect.width, cropcap.defrect.height, cropcap.defrect.left, cropcap.defrect.top, + cropcap.pixelaspect.numerator, cropcap.pixelaspect.denominator); + + struct v4l2_fmtdesc fmtdesc = {0}; + fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + char fourcc[5] = {0}; + char c, e; + printf(" FMT : CE Desc\n--------------------\n"); + while (0 == xioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc)) + { + strncpy(fourcc, (char *)&fmtdesc.pixelformat, 4); + c = fmtdesc.flags & 1? 'C' : ' '; + e = fmtdesc.flags & 2? 'E' : ' '; + printf(" %s: %c%c %s\n", fourcc, c, e, fmtdesc.description); + fmtdesc.index++; + } + + struct v4l2_format fmt = {0}; + fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + fmt.fmt.pix.width = 640; + fmt.fmt.pix.height = 480; + //fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_BGR24; + //fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY; + fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; + fmt.fmt.pix.field = V4L2_FIELD_NONE; + + if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt)) + { + perror("Setting Pixel Format"); + return 1; + } + + strncpy(fourcc, (char *)&fmt.fmt.pix.pixelformat, 4); + printf( "Selected Camera Mode:\n" + " Width: %d\n" + " Height: %d\n" + " PixFmt: %s\n" + " Field: %d\n", + fmt.fmt.pix.width, + fmt.fmt.pix.height, + fourcc, + fmt.fmt.pix.field); + return 0; +} + +static int init_mmap(int fd, uint8_t **buffer, size_t *buffer_len) +{ + struct v4l2_requestbuffers req = {0}; + req.count = 1; + req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + req.memory = V4L2_MEMORY_MMAP; + + if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) + { + perror("Requesting Buffer"); + return 1; + } + + struct v4l2_buffer buf = {0}; + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + buf.index = 0; + if(-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf)) + { + perror("Querying Buffer"); + return 1; + } + + *buffer = mmap (NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset); + *buffer_len = buf.length; + + printf("Length: %d\nAddress: %p\n", buf.length, *buffer); + printf("Image Length: %d\n", buf.bytesused); + + return 0; +} + +static int capture_frame(int fd) +{ + struct v4l2_buffer buf = {0}; + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + buf.index = 0; + if(-1 == xioctl(fd, VIDIOC_QBUF, &buf)) + { + perror("Query Buffer"); + return 1; + } + + if(-1 == xioctl(fd, VIDIOC_STREAMON, &buf.type)) + { + perror("Start Capture"); + return 1; + } + + fd_set fds; + FD_ZERO(&fds); + FD_SET(fd, &fds); + struct timeval tv = {0}; + tv.tv_sec = 2; + int r = select(fd+1, &fds, NULL, NULL, &tv); + if(-1 == r) + { + perror("Waiting for Frame"); + return 1; + } + + if(-1 == xioctl(fd, VIDIOC_DQBUF, &buf)) + { + perror("Retrieving Frame"); + return 1; + } + + return 0; +} + +int save_image(const char *out, uint8_t *buffer, size_t buffer_len) +{ + printf ("saving image\n"); + + int fd = open(out, O_WRONLY | O_CREAT | O_TRUNC, 0600); + if (fd < 0) { + fprintf(stderr, "open(\"%s\"): %m\n", out); + return -1; + } + + int ret = 0; + while (buffer_len) { + ssize_t written = write(fd, buffer, buffer_len); + if (written < 0) { + perror("write()"); + ret = -1; + break; + } + + buffer += written; + buffer_len -= written; + } + + close(fd); + return ret; +} + +int capture_image(const char *out) +{ + int fd = open("/dev/video0", O_RDWR); + if (fd < 0) { + perror("Opening video device"); + return -1; + } + + uint8_t *buffer = NULL; + size_t buffer_len; + + int ret = print_caps(fd) + || init_mmap(fd, &buffer, &buffer_len) + || capture_frame(fd) + || save_image(out, buffer, buffer_len); + + close(fd); + if (buffer) + munmap(buffer, buffer_len); + + return ret ? -1 : 0; +}