Hard forking reader.h because jevois kernel is weird.
Change-Id: I5f59dca9b245050aa17bca7faf3643370ddc698a
diff --git a/y2019/jevois/camera/reader.cc b/y2019/jevois/camera/reader.cc
new file mode 100644
index 0000000..25b5a3f
--- /dev/null
+++ b/y2019/jevois/camera/reader.cc
@@ -0,0 +1,303 @@
+#include "y2019/jevois/camera/reader.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <malloc.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include "aos/logging/logging.h"
+#include "aos/time/time.h"
+
+#define CLEAR(x) memset(&(x), 0, sizeof(x))
+
+namespace y2019 {
+namespace camera {
+
+using ::camera::xioctl;
+
+struct Reader::Buffer {
+ void *start;
+ size_t length; // for munmap
+};
+
+aos::vision::CameraParams MakeCameraParams(int32_t width, int32_t height,
+ int32_t exposure, int32_t brightness,
+ int32_t gain, int32_t fps) {
+ aos::vision::CameraParams cam;
+ cam.set_width(width);
+ cam.set_height(height);
+ cam.set_exposure(exposure);
+ cam.set_brightness(brightness);
+ cam.set_gain(gain);
+ cam.set_fps(fps);
+ return cam;
+}
+
+Reader::Reader(const std::string &dev_name, ProcessCb process,
+ aos::vision::CameraParams params)
+ : dev_name_(dev_name), process_(std::move(process)), params_(params) {
+ struct stat st;
+ if (stat(dev_name.c_str(), &st) == -1) {
+ PLOG(FATAL, "Cannot identify '%s'", dev_name.c_str());
+ }
+ if (!S_ISCHR(st.st_mode)) {
+ PLOG(FATAL, "%s is no device\n", dev_name.c_str());
+ }
+
+ fd_ = open(dev_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
+ if (fd_ == -1) {
+ PLOG(FATAL, "Cannot open '%s'", dev_name.c_str());
+ }
+
+ Init();
+
+ InitMMap();
+ LOG(INFO, "Bat Vision Successfully Initialized.\n");
+}
+
+void Reader::QueueBuffer(v4l2_buffer *buf) {
+ if (xioctl(fd_, VIDIOC_QBUF, buf) == -1) {
+ PLOG(WARNING,
+ "ioctl VIDIOC_QBUF(%d, %p)."
+ " losing buf #%" PRIu32 "\n",
+ fd_, &buf, buf->index);
+ } else {
+ ++queued_;
+ }
+}
+
+void Reader::HandleFrame() {
+ v4l2_buffer buf;
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+
+ if (xioctl(fd_, VIDIOC_DQBUF, &buf) == -1) {
+ if (errno != EAGAIN) {
+ PLOG(ERROR, "ioctl VIDIOC_DQBUF(%d, %p)", fd_, &buf);
+ }
+ return;
+ }
+ --queued_;
+
+ ++tick_id_;
+ // Get a timestamp now as proxy for when the image was taken
+ // TODO(ben): the image should come with a timestamp, parker
+ // will know how to get it.
+ auto time = aos::monotonic_clock::now();
+
+ process_(aos::vision::DataRef(
+ reinterpret_cast<const char *>(buffers_[buf.index].start),
+ buf.bytesused),
+ time);
+
+ QueueBuffer(&buf);
+}
+
+void Reader::MMapBuffers() {
+ buffers_ = new Buffer[kNumBuffers];
+ v4l2_buffer buf;
+ for (unsigned int n = 0; n < kNumBuffers; ++n) {
+ memset(&buf, 0, sizeof(buf));
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = n;
+ if (xioctl(fd_, VIDIOC_QUERYBUF, &buf) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_QUERYBUF(%d, %p)", fd_, &buf);
+ }
+ buffers_[n].length = buf.length;
+ buffers_[n].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE,
+ MAP_SHARED, fd_, buf.m.offset);
+ if (buffers_[n].start == MAP_FAILED) {
+ PLOG(FATAL,
+ "mmap(NULL, %zd, PROT_READ | PROT_WRITE, MAP_SHARED, %d, %jd)",
+ (size_t)buf.length, fd_, static_cast<intmax_t>(buf.m.offset));
+ }
+ }
+}
+
+void Reader::InitMMap() {
+ v4l2_requestbuffers req;
+ CLEAR(req);
+ req.count = kNumBuffers;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_MMAP;
+ if (xioctl(fd_, VIDIOC_REQBUFS, &req) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s does not support memory mapping\n", dev_name_.c_str());
+ } else {
+ PLOG(FATAL, "ioctl VIDIOC_REQBUFS(%d, %p)\n", fd_, &req);
+ }
+ }
+ queued_ = kNumBuffers;
+ if (req.count != kNumBuffers) {
+ LOG(FATAL, "Insufficient buffer memory on %s\n", dev_name_.c_str());
+ }
+}
+
+// Sets one of the camera's user-control values.
+// Prints the old and new values.
+// Just prints a message if the camera doesn't support this control or value.
+bool Reader::SetCameraControl(uint32_t id, const char *name, int value) {
+ struct v4l2_control getArg = {id, 0U};
+ int r;
+ // Used to be: r = xioctl(fd_, VIDIOC_S_CTRL, &getArg);
+ // Jevois wants this incorrect number below:.
+ r = xioctl(fd_, 0xc00c561b, &getArg);
+ if (r == 0) {
+ if (getArg.value == value) {
+ LOG(DEBUG, "Camera control %s was already %d\n", name, getArg.value);
+ return true;
+ }
+ } else if (errno == EINVAL) {
+ LOG(DEBUG, "Camera control %s is invalid\n", name);
+ errno = 0;
+ return false;
+ }
+
+ struct v4l2_control setArg = {id, value};
+ // Should be: r = xioctl(fd_, VIDIOC_S_CTRL, &setArg);
+ // Jevois wants this incorrect number below:.
+ r = xioctl(fd_, 0xc00c561c, &setArg);
+ if (r == 0) {
+ LOG(DEBUG, "Set camera control %s from %d to %d\n", name, getArg.value,
+ value);
+ return true;
+ }
+
+ LOG(DEBUG, "Couldn't set camera control %s to %d", name, value);
+ errno = 0;
+ return false;
+}
+
+void Reader::Init() {
+ v4l2_capability cap;
+ if (xioctl(fd_, VIDIOC_QUERYCAP, &cap) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s is no V4L2 device\n", dev_name_.c_str());
+ } else {
+ PLOG(FATAL, "ioctl VIDIOC_QUERYCAP(%d, %p)", fd_, &cap);
+ }
+ }
+ if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+ LOG(FATAL, "%s is no video capture device\n", dev_name_.c_str());
+ }
+ if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
+ LOG(FATAL, "%s does not support streaming i/o\n", dev_name_.c_str());
+ }
+
+ int camidx = -1;
+ struct v4l2_input inp = {};
+ while (true) {
+ if (xioctl(fd_, VIDIOC_ENUMINPUT, &inp) == -1) {
+ break;
+ }
+ if (inp.type == V4L2_INPUT_TYPE_CAMERA) {
+ if (camidx == -1) camidx = inp.index;
+ printf("Input %d [ %s ] is a camera sensor\n", inp.index, inp.name);
+ } else
+ printf("Input %d [ %s ] is not a camera sensor\n", inp.index, inp.name);
+ ++inp.index;
+ }
+
+ if (xioctl(fd_, VIDIOC_S_INPUT, &camidx) == -1) {
+ LOG(FATAL, "ioctl VIDIOC_S_INPUT(%d) failed with %d: %s\n", fd_, errno,
+ strerror(errno));
+ }
+ printf("camera idx: %d\n", camidx);
+
+ /* Select video input, video standard and tune here. */
+
+ v4l2_format fmt;
+ CLEAR(fmt);
+
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_G_FMT, &fmt) == -1) {
+ LOG(FATAL, "ioctl VIDIC_G_FMT(%d, %p) failed with %d: %s\n", fd_, &fmt,
+ errno, strerror(errno));
+ }
+
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.width = params_.width();
+ fmt.fmt.pix.height = params_.height();
+ printf("setting format: %d, %d\n", params_.width(), params_.height());
+ fmt.fmt.pix.field = V4L2_FIELD_NONE;
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+ if (xioctl(fd_, VIDIOC_S_FMT, &fmt) == -1) {
+ LOG(FATAL, "ioctl VIDIC_S_FMT(%d, %p) failed with %d: %s\n", fd_, &fmt,
+ errno, strerror(errno));
+ }
+ /* Note VIDIOC_S_FMT may change width and height. */
+
+ /* Buggy driver paranoia. */
+ unsigned int min = fmt.fmt.pix.width * 2;
+ if (fmt.fmt.pix.bytesperline < min) fmt.fmt.pix.bytesperline = min;
+ min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
+ if (fmt.fmt.pix.sizeimage < min) fmt.fmt.pix.sizeimage = min;
+
+ // set framerate
+ struct v4l2_streamparm *setfps;
+ setfps = (struct v4l2_streamparm *)calloc(1, sizeof(struct v4l2_streamparm));
+ memset(setfps, 0, sizeof(struct v4l2_streamparm));
+ setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ setfps->parm.capture.timeperframe.numerator = 1;
+ setfps->parm.capture.timeperframe.denominator = params_.fps();
+ if (xioctl(fd_, VIDIOC_S_PARM, setfps) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_S_PARM(%d, %p)\n", fd_, setfps);
+ }
+ LOG(INFO, "framerate ended up at %d/%d\n",
+ setfps->parm.capture.timeperframe.numerator,
+ setfps->parm.capture.timeperframe.denominator);
+
+ for (int j = 0; j < 2; ++j) {
+ if (!SetCameraControl(V4L2_CID_EXPOSURE_AUTO, "V4L2_CID_EXPOSURE_AUTO",
+ V4L2_EXPOSURE_MANUAL)) {
+ LOG(FATAL, "Failed to set exposure\n");
+ }
+
+ if (!SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
+ "V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure())) {
+ LOG(FATAL, "Failed to set exposure\n");
+ }
+ sleep(1);
+ }
+}
+
+aos::vision::ImageFormat Reader::get_format() {
+ struct v4l2_format fmt;
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_G_FMT, &fmt) == -1) {
+ PLOG(FATAL, "ioctl VIDIC_G_FMT(%d, %p)\n", fd_, &fmt);
+ }
+
+ return aos::vision::ImageFormat{(int)fmt.fmt.pix.width,
+ (int)fmt.fmt.pix.height};
+}
+
+void Reader::Start() {
+ LOG(DEBUG, "queueing buffers for the first time\n");
+ v4l2_buffer buf;
+ for (unsigned int i = 0; i < kNumBuffers; ++i) {
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+ QueueBuffer(&buf);
+ }
+ LOG(DEBUG, "done with first queue\n");
+
+ v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_STREAMON, &type) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_STREAMON(%d, %p)\n", fd_, &type);
+ }
+}
+
+} // namespace camera
+} // namespace y2019