blob: 3fbcd1dd4e34f7d6243762a0288cd053418a44d0 [file] [log] [blame]
Parker Schuh24ee58d2017-03-11 16:13:23 -08001#include "aos/vision/image/reader.h"
2
Parker Schuh24ee58d2017-03-11 16:13:23 -08003#include <fcntl.h>
4#include <malloc.h>
Parker Schuh24ee58d2017-03-11 16:13:23 -08005#include <sys/mman.h>
6#include <sys/stat.h>
7#include <sys/time.h>
8#include <sys/types.h>
9#include <unistd.h>
Parker Schuh44f86922017-01-03 23:59:50 -080010
Tyler Chatowbf0609c2021-07-31 16:13:27 -070011#include <cerrno>
12#include <cstdio>
13#include <cstdlib>
14#include <cstring>
15
John Park33858a32018-09-28 23:05:48 -070016#include "aos/logging/logging.h"
17#include "aos/time/time.h"
Parker Schuh44f86922017-01-03 23:59:50 -080018
19#define CLEAR(x) memset(&(x), 0, sizeof(x))
20
21namespace camera {
22
23struct Reader::Buffer {
24 void *start;
25 size_t length; // for munmap
26};
27
Parker Schuh24ee58d2017-03-11 16:13:23 -080028aos::vision::CameraParams MakeCameraParams(int32_t width, int32_t height,
29 int32_t exposure, int32_t brightness,
30 int32_t gain, int32_t fps) {
31 aos::vision::CameraParams cam;
32 cam.set_width(width);
33 cam.set_height(height);
34 cam.set_exposure(exposure);
35 cam.set_brightness(brightness);
36 cam.set_gain(gain);
37 cam.set_fps(fps);
38 return cam;
39}
40
Parker Schuh44f86922017-01-03 23:59:50 -080041Reader::Reader(const std::string &dev_name, ProcessCb process,
Parker Schuh24ee58d2017-03-11 16:13:23 -080042 aos::vision::CameraParams params)
Parker Schuh44f86922017-01-03 23:59:50 -080043 : dev_name_(dev_name), process_(std::move(process)), params_(params) {
44 struct stat st;
45 if (stat(dev_name.c_str(), &st) == -1) {
Austin Schuhf257f3c2019-10-27 21:00:43 -070046 AOS_PLOG(FATAL, "Cannot identify '%s'", dev_name.c_str());
Parker Schuh44f86922017-01-03 23:59:50 -080047 }
48 if (!S_ISCHR(st.st_mode)) {
Austin Schuhf257f3c2019-10-27 21:00:43 -070049 AOS_PLOG(FATAL, "%s is no device\n", dev_name.c_str());
Parker Schuh44f86922017-01-03 23:59:50 -080050 }
51
52 fd_ = open(dev_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
53 if (fd_ == -1) {
Austin Schuhf257f3c2019-10-27 21:00:43 -070054 AOS_PLOG(FATAL, "Cannot open '%s'", dev_name.c_str());
Parker Schuh44f86922017-01-03 23:59:50 -080055 }
56
57 Init();
Parker Schuh309dd722017-02-25 11:31:18 -080058
59 InitMMap();
Tyler Chatowfdd7fbf2019-04-13 21:14:05 -070060
61 SetExposure(params.exposure());
Austin Schuhf257f3c2019-10-27 21:00:43 -070062 AOS_LOG(INFO, "Bat Vision Successfully Initialized.\n");
Parker Schuh44f86922017-01-03 23:59:50 -080063}
64
65void Reader::QueueBuffer(v4l2_buffer *buf) {
66 if (xioctl(fd_, VIDIOC_QBUF, buf) == -1) {
Austin Schuhf257f3c2019-10-27 21:00:43 -070067 AOS_PLOG(WARNING,
68 "ioctl VIDIOC_QBUF(%d, %p)."
69 " losing buf #%" PRIu32 "\n",
70 fd_, &buf, buf->index);
Parker Schuh44f86922017-01-03 23:59:50 -080071 } else {
Austin Schuhf257f3c2019-10-27 21:00:43 -070072 // AOS_LOG(DEBUG, "put buf #%" PRIu32 " into driver's queue\n",
73 // buf->index);
Parker Schuh44f86922017-01-03 23:59:50 -080074 ++queued_;
75 }
76}
77
78void Reader::HandleFrame() {
79 v4l2_buffer buf;
80 CLEAR(buf);
81 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
82 buf.memory = V4L2_MEMORY_MMAP;
83
84 if (xioctl(fd_, VIDIOC_DQBUF, &buf) == -1) {
85 if (errno != EAGAIN) {
Austin Schuhf257f3c2019-10-27 21:00:43 -070086 AOS_PLOG(ERROR, "ioctl VIDIOC_DQBUF(%d, %p)", fd_, &buf);
Parker Schuh44f86922017-01-03 23:59:50 -080087 }
88 return;
89 }
90 --queued_;
91
Parker Schuh309dd722017-02-25 11:31:18 -080092 ++tick_id_;
Parker Schuh44f86922017-01-03 23:59:50 -080093 // Get a timestamp now as proxy for when the image was taken
94 // TODO(ben): the image should come with a timestamp, parker
95 // will know how to get it.
96 auto time = aos::monotonic_clock::now();
97
98 process_(aos::vision::DataRef(
99 reinterpret_cast<const char *>(buffers_[buf.index].start),
100 buf.bytesused),
101 time);
Parker Schuh309dd722017-02-25 11:31:18 -0800102
Parker Schuh44f86922017-01-03 23:59:50 -0800103 QueueBuffer(&buf);
104}
105
106void Reader::MMapBuffers() {
107 buffers_ = new Buffer[kNumBuffers];
108 v4l2_buffer buf;
109 for (unsigned int n = 0; n < kNumBuffers; ++n) {
110 memset(&buf, 0, sizeof(buf));
111 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
112 buf.memory = V4L2_MEMORY_MMAP;
113 buf.index = n;
114 if (xioctl(fd_, VIDIOC_QUERYBUF, &buf) == -1) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700115 AOS_PLOG(FATAL, "ioctl VIDIOC_QUERYBUF(%d, %p)", fd_, &buf);
Parker Schuh44f86922017-01-03 23:59:50 -0800116 }
117 buffers_[n].length = buf.length;
118 buffers_[n].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE,
119 MAP_SHARED, fd_, buf.m.offset);
120 if (buffers_[n].start == MAP_FAILED) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700121 AOS_PLOG(FATAL,
122 "mmap(NULL, %zd, PROT_READ | PROT_WRITE, MAP_SHARED, %d, %jd)",
123 (size_t)buf.length, fd_, static_cast<intmax_t>(buf.m.offset));
Parker Schuh44f86922017-01-03 23:59:50 -0800124 }
125 }
126}
127
128void Reader::InitMMap() {
129 v4l2_requestbuffers req;
130 CLEAR(req);
131 req.count = kNumBuffers;
132 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
133 req.memory = V4L2_MEMORY_MMAP;
134 if (xioctl(fd_, VIDIOC_REQBUFS, &req) == -1) {
135 if (EINVAL == errno) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700136 AOS_LOG(FATAL, "%s does not support memory mapping\n", dev_name_.c_str());
Parker Schuh44f86922017-01-03 23:59:50 -0800137 } else {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700138 AOS_PLOG(FATAL, "ioctl VIDIOC_REQBUFS(%d, %p)\n", fd_, &req);
Parker Schuh44f86922017-01-03 23:59:50 -0800139 }
140 }
141 queued_ = kNumBuffers;
Parker Schuh309dd722017-02-25 11:31:18 -0800142 if (req.count != kNumBuffers) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700143 AOS_LOG(FATAL, "Insufficient buffer memory on %s\n", dev_name_.c_str());
Parker Schuh44f86922017-01-03 23:59:50 -0800144 }
145}
146
147// Sets one of the camera's user-control values.
148// Prints the old and new values.
149// Just prints a message if the camera doesn't support this control or value.
150bool Reader::SetCameraControl(uint32_t id, const char *name, int value) {
151 struct v4l2_control getArg = {id, 0U};
152 int r = xioctl(fd_, VIDIOC_G_CTRL, &getArg);
153 if (r == 0) {
154 if (getArg.value == value) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700155 AOS_LOG(DEBUG, "Camera control %s was already %d\n", name, getArg.value);
Parker Schuh44f86922017-01-03 23:59:50 -0800156 return true;
157 }
158 } else if (errno == EINVAL) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700159 AOS_LOG(DEBUG, "Camera control %s is invalid\n", name);
Parker Schuh44f86922017-01-03 23:59:50 -0800160 errno = 0;
161 return false;
162 }
163
164 struct v4l2_control setArg = {id, value};
165 r = xioctl(fd_, VIDIOC_S_CTRL, &setArg);
166 if (r == 0) {
Parker Schuh44f86922017-01-03 23:59:50 -0800167 return true;
168 }
169
Austin Schuhf257f3c2019-10-27 21:00:43 -0700170 AOS_LOG(DEBUG, "Couldn't set camera control %s to %d", name, value);
Parker Schuh44f86922017-01-03 23:59:50 -0800171 errno = 0;
172 return false;
173}
174
Tyler Chatowfdd7fbf2019-04-13 21:14:05 -0700175bool Reader::SetExposure(int abs_exp) {
176 return SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
177 "V4L2_CID_EXPOSURE_ABSOLUTE", abs_exp);
178}
179
Parker Schuh44f86922017-01-03 23:59:50 -0800180void Reader::Init() {
181 v4l2_capability cap;
182 if (xioctl(fd_, VIDIOC_QUERYCAP, &cap) == -1) {
183 if (EINVAL == errno) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700184 AOS_LOG(FATAL, "%s is no V4L2 device\n", dev_name_.c_str());
Parker Schuh44f86922017-01-03 23:59:50 -0800185 } else {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700186 AOS_PLOG(FATAL, "ioctl VIDIOC_QUERYCAP(%d, %p)", fd_, &cap);
Parker Schuh44f86922017-01-03 23:59:50 -0800187 }
188 }
189 if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700190 AOS_LOG(FATAL, "%s is no video capture device\n", dev_name_.c_str());
Parker Schuh44f86922017-01-03 23:59:50 -0800191 }
192 if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700193 AOS_LOG(FATAL, "%s does not support streaming i/o\n", dev_name_.c_str());
Parker Schuh44f86922017-01-03 23:59:50 -0800194 }
195
196 /* Select video input, video standard and tune here. */
197
198 v4l2_cropcap cropcap;
199 CLEAR(cropcap);
200 cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
201 if (xioctl(fd_, VIDIOC_CROPCAP, &cropcap) == 0) {
202 v4l2_crop crop;
203 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
204 crop.c = cropcap.defrect; /* reset to default */
205
206 if (xioctl(fd_, VIDIOC_S_CROP, &crop) == -1) {
207 switch (errno) {
208 case EINVAL:
209 /* Cropping not supported. */
210 break;
211 default:
212 /* Errors ignored. */
Austin Schuhf257f3c2019-10-27 21:00:43 -0700213 AOS_PLOG(WARNING, "xioctl VIDIOC_S_CROP");
Parker Schuh44f86922017-01-03 23:59:50 -0800214 break;
215 }
216 }
217 } else {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700218 AOS_PLOG(WARNING, "xioctl VIDIOC_CROPCAP");
Parker Schuh44f86922017-01-03 23:59:50 -0800219 }
220
221 v4l2_format fmt;
222 CLEAR(fmt);
223 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
Parker Schuh24ee58d2017-03-11 16:13:23 -0800224 fmt.fmt.pix.width = params_.width();
225 fmt.fmt.pix.height = params_.height();
Parker Schuh44f86922017-01-03 23:59:50 -0800226 fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
227 fmt.fmt.pix.field = V4L2_FIELD_ANY;
228 // fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
229 // fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
230 if (xioctl(fd_, VIDIOC_S_FMT, &fmt) == -1) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700231 AOS_LOG(FATAL, "ioctl VIDIC_S_FMT(%d, %p) failed with %d: %s\n", fd_, &fmt,
232 errno, strerror(errno));
Parker Schuh44f86922017-01-03 23:59:50 -0800233 }
234 /* Note VIDIOC_S_FMT may change width and height. */
235
236 /* Buggy driver paranoia. */
237 unsigned int min = fmt.fmt.pix.width * 2;
238 if (fmt.fmt.pix.bytesperline < min) fmt.fmt.pix.bytesperline = min;
239 min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
240 if (fmt.fmt.pix.sizeimage < min) fmt.fmt.pix.sizeimage = min;
241
242 if (!SetCameraControl(V4L2_CID_EXPOSURE_AUTO, "V4L2_CID_EXPOSURE_AUTO",
243 V4L2_EXPOSURE_MANUAL)) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700244 AOS_LOG(FATAL, "Failed to set exposure\n");
Parker Schuh44f86922017-01-03 23:59:50 -0800245 }
246
247 if (!SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
Parker Schuh24ee58d2017-03-11 16:13:23 -0800248 "V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure())) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700249 AOS_LOG(FATAL, "Failed to set exposure\n");
Parker Schuh44f86922017-01-03 23:59:50 -0800250 }
251
252 if (!SetCameraControl(V4L2_CID_BRIGHTNESS, "V4L2_CID_BRIGHTNESS",
Parker Schuh24ee58d2017-03-11 16:13:23 -0800253 params_.brightness())) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700254 AOS_LOG(FATAL, "Failed to set up camera\n");
Parker Schuh44f86922017-01-03 23:59:50 -0800255 }
256
Parker Schuh24ee58d2017-03-11 16:13:23 -0800257 if (!SetCameraControl(V4L2_CID_GAIN, "V4L2_CID_GAIN", params_.gain())) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700258 AOS_LOG(FATAL, "Failed to set up camera\n");
Parker Schuh44f86922017-01-03 23:59:50 -0800259 }
260
Parker Schuh44f86922017-01-03 23:59:50 -0800261 // set framerate
262 struct v4l2_streamparm *setfps;
263 setfps = (struct v4l2_streamparm *)calloc(1, sizeof(struct v4l2_streamparm));
264 memset(setfps, 0, sizeof(struct v4l2_streamparm));
265 setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
266 setfps->parm.capture.timeperframe.numerator = 1;
Parker Schuh24ee58d2017-03-11 16:13:23 -0800267 setfps->parm.capture.timeperframe.denominator = params_.fps();
Parker Schuh44f86922017-01-03 23:59:50 -0800268 if (xioctl(fd_, VIDIOC_S_PARM, setfps) == -1) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700269 AOS_PLOG(FATAL, "ioctl VIDIOC_S_PARM(%d, %p)\n", fd_, setfps);
Parker Schuh44f86922017-01-03 23:59:50 -0800270 }
Austin Schuhf257f3c2019-10-27 21:00:43 -0700271 AOS_LOG(INFO, "framerate ended up at %d/%d\n",
272 setfps->parm.capture.timeperframe.numerator,
273 setfps->parm.capture.timeperframe.denominator);
Parker Schuh44f86922017-01-03 23:59:50 -0800274}
275
276aos::vision::ImageFormat Reader::get_format() {
277 struct v4l2_format fmt;
278 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
279 if (xioctl(fd_, VIDIOC_G_FMT, &fmt) == -1) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700280 AOS_PLOG(FATAL, "ioctl VIDIC_G_FMT(%d, %p)\n", fd_, &fmt);
Parker Schuh44f86922017-01-03 23:59:50 -0800281 }
282
283 return aos::vision::ImageFormat{(int)fmt.fmt.pix.width,
284 (int)fmt.fmt.pix.height};
285}
286
287void Reader::Start() {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700288 AOS_LOG(DEBUG, "queueing buffers for the first time\n");
Parker Schuh44f86922017-01-03 23:59:50 -0800289 v4l2_buffer buf;
290 for (unsigned int i = 0; i < kNumBuffers; ++i) {
291 CLEAR(buf);
292 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
293 buf.memory = V4L2_MEMORY_MMAP;
294 buf.index = i;
295 QueueBuffer(&buf);
296 }
Austin Schuhf257f3c2019-10-27 21:00:43 -0700297 AOS_LOG(DEBUG, "done with first queue\n");
Parker Schuh44f86922017-01-03 23:59:50 -0800298
299 v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
300 if (xioctl(fd_, VIDIOC_STREAMON, &type) == -1) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700301 AOS_PLOG(FATAL, "ioctl VIDIOC_STREAMON(%d, %p)\n", fd_, &type);
Parker Schuh44f86922017-01-03 23:59:50 -0800302 }
303}
304
305} // namespace camera