From 9d3d34c9796124b682985fd330b413a81d60ce8a Mon Sep 17 00:00:00 2001 From: Omar Rizwan Date: Mon, 13 May 2024 19:15:43 -0400 Subject: Continue refactor: remove pi/cUtils.tcl and refs, inline camera --- virtual-programs/camera.folk | 349 ++++++++++++++++++++++++++++++++++++++++++- virtual-programs/images.folk | 1 - virtual-programs/print.folk | 2 - 3 files changed, 348 insertions(+), 4 deletions(-) (limited to 'virtual-programs') diff --git a/virtual-programs/camera.folk b/virtual-programs/camera.folk index 7a4bfc44..373c904a 100644 --- a/virtual-programs/camera.folk +++ b/virtual-programs/camera.folk @@ -14,11 +14,358 @@ if {$::thisNode eq "folk-interact"} { set width $::Camera::WIDTH set height $::Camera::HEIGHT +set makeCamera { + rename [c create] camc + + camc include + camc include + + camc include + camc include + camc include + camc include + camc include + camc include + + camc include + camc include + + camc include + + camc struct buffer_t { + uint8_t* start; + size_t length; + } + camc struct camera_t { + int fd; + + uint32_t width; + uint32_t height; + + int uses_jpeg_format; + + size_t buffer_count; + buffer_t* buffers; + buffer_t head; + } + + camc code { + void quit(const char* msg) { + fprintf(stderr, "[%s] %d: %s\n", msg, errno, strerror(errno)); + exit(1); + } + + int xioctl(int fd, int request, void* arg) { + for (int i = 0; i < 100; i++) { + int r = ioctl(fd, request, arg); + if (r != -1 || errno != EINTR) return r; + printf("[%x][%d] %s\n", request, i, strerror(errno)); + } + return -1; + } + } + defineImageType camc + + camc proc cameraOpen {char* device int width int height int uses_jpeg_format} camera_t* { + printf("device [%s]\n", device); + int fd = open(device, O_RDWR, 0); + if (fd == -1) quit("open"); + camera_t* camera = ckalloc(sizeof (camera_t)); + camera->fd = fd; + camera->width = width; + camera->height = height; + camera->uses_jpeg_format = uses_jpeg_format; + camera->buffer_count = 0; + camera->buffers = NULL; + camera->head.length = 0; + camera->head.start = NULL; + return camera; + } + + camc proc cameraInit {camera_t* camera} void { + struct v4l2_capability cap; + if (xioctl(camera->fd, VIDIOC_QUERYCAP, &cap) == -1) quit("VIDIOC_QUERYCAP"); + if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) quit("no capture"); + if (!(cap.capabilities & V4L2_CAP_STREAMING)) quit("no streaming"); + + struct v4l2_format format = {0}; + format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + format.fmt.pix.width = camera->width; + format.fmt.pix.height = camera->height; + if (camera->uses_jpeg_format) { + // All(?) USB webcams we've encountered use this format. + format.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; + } else { + // Implementing this just for Pi camera via libcamerify. + format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420; + } + format.fmt.pix.field = V4L2_FIELD_NONE; + int ret; + do { + ret = xioctl(camera->fd, VIDIOC_S_FMT, &format); + } while (ret == EBUSY); + if (ret == -1) quit("VIDIOC_S_FMT"); + + if (!camera->uses_jpeg_format && format.fmt.pix.bytesperline != camera->width) { + fprintf(stderr, "cameraInit: interline padding not supported " + "(bytesperline = %u, camera->width = %u)\n", + format.fmt.pix.bytesperline, + camera->width); + exit(1); + } + + struct v4l2_requestbuffers req = {0}; + req.count = 4; + req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + req.memory = V4L2_MEMORY_MMAP; + if (xioctl(camera->fd, VIDIOC_REQBUFS, &req) == -1) quit("VIDIOC_REQBUFS"); + camera->buffer_count = req.count; + camera->buffers = calloc(req.count, sizeof (buffer_t)); + + if (camera->uses_jpeg_format) { + // VIDIOC_G_PARM and VIDIOC_S_PARM are not supported by + // libcamerify. + + struct v4l2_streamparm streamparm = {0}; + streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (xioctl(camera->fd, VIDIOC_G_PARM, &streamparm) == -1) quit("VIDIOC_G_PARM"); + if (streamparm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) { + int req_rate_numerator = 1; + int req_rate_denominator = 60; + streamparm.parm.capture.timeperframe.numerator = req_rate_numerator; + streamparm.parm.capture.timeperframe.denominator = req_rate_denominator; + if (xioctl(camera->fd, VIDIOC_S_PARM, &streamparm) == -1) { quit("VIDIOC_S_PARM"); } + + if (streamparm.parm.capture.timeperframe.numerator != req_rate_denominator || + streamparm.parm.capture.timeperframe.denominator != req_rate_numerator) { + fprintf(stderr, + "the driver changed the time per frame from " + "%d/%d to %d/%d\n", + req_rate_denominator, req_rate_numerator, + streamparm.parm.capture.timeperframe.numerator, + streamparm.parm.capture.timeperframe.denominator); + } + } + } + + size_t buf_max = 0; + for (size_t i = 0; i < camera->buffer_count; i++) { + struct v4l2_buffer buf; + memset(&buf, 0, sizeof buf); + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + buf.index = i; + if (xioctl(camera->fd, VIDIOC_QUERYBUF, &buf) == -1) + quit("VIDIOC_QUERYBUF"); + if (buf.length > buf_max) buf_max = buf.length; + camera->buffers[i].length = buf.length; + camera->buffers[i].start = + mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, + camera->fd, buf.m.offset); + if (camera->buffers[i].start == MAP_FAILED) quit("mmap"); + } + camera->head.start = ckalloc(buf_max); + + printf("camera %d; bufcount %zu\n", camera->fd, camera->buffer_count); + } + + camc proc cameraStart {camera_t* camera} void { + for (size_t i = 0; i < camera->buffer_count; i++) { + struct v4l2_buffer buf; + memset(&buf, 0, sizeof buf); + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + buf.index = i; + if (xioctl(camera->fd, VIDIOC_QBUF, &buf) == -1) quit("VIDIOC_QBUF"); + printf("camera_start(%zu): %s\n", i, strerror(errno)); + } + + enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (xioctl(camera->fd, VIDIOC_STREAMON, &type) == -1) + quit("VIDIOC_STREAMON"); + } + + camc code { + int camera_capture(camera_t* camera) { + struct v4l2_buffer buf; + memset(&buf, 0, sizeof buf); + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + if (xioctl(camera->fd, VIDIOC_DQBUF, &buf) == -1) { + fprintf(stderr, "camera_capture: VIDIOC_DQBUF failed: %d: %s\n", errno, strerror(errno)); + return 0; + } + memcpy(camera->head.start, camera->buffers[buf.index].start, buf.bytesused); + camera->head.length = buf.bytesused; + if (xioctl(camera->fd, VIDIOC_QBUF, &buf) == -1) { + fprintf(stderr, "camera_capture: VIDIOC_QBUF failed: %d: %s\n", errno, strerror(errno)); + return 0; + } + return 1; + } + } + + camc proc cameraFrame {camera_t* camera} int { + struct timeval timeout; + timeout.tv_sec = 1; + timeout.tv_usec = 0; + fd_set fds; + FD_ZERO(&fds); + FD_SET(camera->fd, &fds); + int r = select(camera->fd + 1, &fds, 0, 0, &timeout); + // printf("r: %d\n", r); + if (r == -1) quit("select"); + if (r == 0) { + printf("selection failed of fd %d\n", camera->fd); + return 0; + } + return camera_capture(camera); + } + + camc proc cameraDecompressRgb {camera_t* camera image_t dest} void { + if (!camera->uses_jpeg_format) { fprintf(stderr, "cameraDecompressRgb: non-jpeg not supported\n"); exit(1); } + + struct jpeg_decompress_struct cinfo; + struct jpeg_error_mgr jerr; + cinfo.err = jpeg_std_error(&jerr); + jpeg_create_decompress(&cinfo); + jpeg_mem_src(&cinfo, camera->head.start, camera->head.length); + if (jpeg_read_header(&cinfo, TRUE) != 1) { + printf("Fail\n"); + exit(1); + } + jpeg_start_decompress(&cinfo); + + while (cinfo.output_scanline < cinfo.output_height) { + unsigned char *buffer_array[1]; + buffer_array[0] = dest.data + (cinfo.output_scanline) * dest.width * cinfo.output_components; + jpeg_read_scanlines(&cinfo, buffer_array, 1); + } + jpeg_finish_decompress(&cinfo); + jpeg_destroy_decompress(&cinfo); + } + camc proc cameraDecompressGrayJpeg {camera_t* camera image_t dest} void { + struct jpeg_decompress_struct cinfo; + struct jpeg_error_mgr jerr; + cinfo.err = jpeg_std_error(&jerr); + jpeg_create_decompress(&cinfo); + jpeg_mem_src(&cinfo, camera->head.start, camera->head.length); + if (jpeg_read_header(&cinfo, TRUE) != 1) { + printf("Fail\n"); + exit(1); + } + cinfo.out_color_space = JCS_GRAYSCALE; + jpeg_start_decompress(&cinfo); + + while (cinfo.output_scanline < cinfo.output_height) { + unsigned char *buffer_array[1]; + buffer_array[0] = dest.data + (cinfo.output_scanline) * dest.width * cinfo.output_components; + jpeg_read_scanlines(&cinfo, buffer_array, 1); + } + jpeg_finish_decompress(&cinfo); + jpeg_destroy_decompress(&cinfo); + } + camc proc cameraDecompressGray {camera_t* camera image_t dest} void { + if (camera->uses_jpeg_format) { + cameraDecompressGrayJpeg(camera, dest); + } else { + // Planar Y:U:V 4:2:0 format. Just copy the Y plane. + memcpy(dest.data, camera->head.start, camera->width * camera->height); + } + } + camc proc rgbToGray {image_t rgb} image_t { + uint8_t* gray = calloc(rgb.width * rgb.height, sizeof (uint8_t)); + for (int y = 0; y < rgb.height; y++) { + for (int x = 0; x < rgb.width; x++) { + // we're spending 10-20% of camera time here on Pi ... ?? + + int i = (y * rgb.width + x) * 3; + uint32_t r = rgb.data[i]; + uint32_t g = rgb.data[i + 1]; + uint32_t b = rgb.data[i + 2]; + // from https://mina86.com/2021/rgb-to-greyscale/ + uint32_t yy = 3567664 * r + 11998547 * g + 1211005 * b; + gray[y * rgb.width + x] = ((yy + (1 << 23)) >> 24); + } + } + return (image_t) { + .width = rgb.width, .height = rgb.height, + .components = 1, + .bytesPerRow = rgb.width, + .data = gray + }; + } + + if {[namespace exists ::Heap]} { + camc import ::Heap::cc folkHeapAlloc as folkHeapAlloc + camc import ::Heap::cc folkHeapFree as folkHeapFree + } else { + camc code { + #define folkHeapAlloc malloc + #define folkHeapFree free + } + } + camc proc newImage {int width int height int components} image_t { + uint8_t* data = folkHeapAlloc(width*components*height); + return (image_t) { + .width = width, + .height = height, + .components = components, + .bytesPerRow = width*components, + .data = data + }; + } + camc proc freeImage {image_t image} void { + folkHeapFree(image.data); + } + + if {$::tcl_platform(os) eq "Darwin"} { + c loadlib "/opt/homebrew/lib/libjpeg.dylib" + } else { + c loadlibLd libjpeg.so + } + camc compile + + variable camera + proc init {width height {usesJpegFormat 1}} { + variable camera + variable WIDTH; variable HEIGHT + set WIDTH $width; set HEIGHT $height + set camera [Camera::cameraOpen "/dev/video0" $width $height $usesJpegFormat] + Camera::cameraInit $camera + Camera::cameraStart $camera + # skip 5 frames for booting a cam + for {set i 0} {$i < 5} {incr i} { + Camera::cameraFrame $camera + } + } + proc frame {} { + variable camera + variable WIDTH; variable HEIGHT + if {![cameraFrame $camera]} { + error "Failed to capture from camera" + } + set image [newImage $WIDTH $HEIGHT 3] + cameraDecompressRgb $camera $image + return $image + } + proc grayFrame {} { + variable camera + variable WIDTH; variable HEIGHT + if {![Camera::cameraFrame $camera]} { + error "Failed to capture from camera" + } + set image [Camera::newImage $WIDTH $HEIGHT 1] + Camera::cameraDecompressGray $camera $image + return $image + } +} + Start process "camera" { Wish $::thisProcess shares statements like \ [list /someone/ claims the camera /...anything/] - source pi/Camera.tcl + namespace eval Camera $makeCamera if {[string match "gadget-*" $::thisNode]} { Camera::init $width $height 0 diff --git a/virtual-programs/images.folk b/virtual-programs/images.folk index 002760c0..2fceeb6e 100644 --- a/virtual-programs/images.folk +++ b/virtual-programs/images.folk @@ -33,7 +33,6 @@ namespace eval ::image { } $cc cflags -ljpeg -lpng - source "pi/cUtils.tcl" defineImageType $cc $cc include $cc include diff --git a/virtual-programs/print.folk b/virtual-programs/print.folk index d045ec18..cb0c9afc 100644 --- a/virtual-programs/print.folk +++ b/virtual-programs/print.folk @@ -1,5 +1,3 @@ -source "pi/cUtils.tcl" - set cc [c create] $cc cflags -I$::env(HOME)/apriltag -Wall -Werror $::env(HOME)/apriltag/libapriltag.a c loadlib $::env(HOME)/apriltag/libapriltag.so -- cgit v1.2.3