copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/atom_code/camera/Reader.cpp b/aos/atom_code/camera/Reader.cpp
new file mode 100644
index 0000000..9cfee2a
--- /dev/null
+++ b/aos/atom_code/camera/Reader.cpp
@@ -0,0 +1,362 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <malloc.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <sys/mman.h>
+
+#include <string>
+#include <inttypes.h>
+
+#include "aos/aos_core.h"
+#include "aos/atom_code/camera/V4L2.h"
+#include "aos/atom_code/camera/Buffers.h"
+
+#define CLEAR(x) memset(&(x), 0, sizeof(x))
+
+namespace aos {
+namespace camera {
+
+class Reader {
+ static const char *const dev_name;
+
+ // of the camera
+ int fd_;
+ // the bound socket listening for fd requests
+ int server_fd_;
+
+ static const aos_type_sig kRecycleSignature;
+ aos_queue *queue_, *recycle_queue_;
+ // the number of buffers currently queued in v4l2
+ uint32_t queued_;
+ public:
+ Reader() {
+ struct stat st;
+ if (stat(dev_name, &st) == -1) {
+ LOG(FATAL, "Cannot identify '%s' because of %d: %s\n",
+ dev_name, errno, strerror(errno));
+ }
+ if (!S_ISCHR(st.st_mode)) {
+ LOG(FATAL, "%s is no device\n", dev_name);
+ }
+
+ fd_ = open(dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
+ if (fd_ == -1) {
+ LOG(FATAL, "Cannot open '%s' because of %d: %s\n",
+ dev_name, errno, strerror(errno));
+ }
+
+ queue_ = aos_fetch_queue_recycle(Buffers::kQueueName.c_str(), &Buffers::kSignature,
+ &kRecycleSignature, &recycle_queue_);
+ // read off any existing recycled messages
+ while (aos_queue_read_msg(recycle_queue_, NON_BLOCK) != NULL);
+ queued_ = 0;
+
+ InitServer();
+ Init();
+ }
+ private:
+ void InitServer() {
+ if (unlink(Buffers::kFDServerName.c_str()) == -1 && errno != ENOENT) {
+ LOG(WARNING, "unlink(kFDServerName(='%s')) failed with %d: %s\n",
+ Buffers::kFDServerName.c_str(), errno, strerror(errno));
+ }
+ if ((server_fd_ = Buffers::CreateSocket(bind)) == -1) {
+ LOG(FATAL, "creating the IPC socket failed\n");
+ }
+ if (listen(server_fd_, 10) == -1) {
+ LOG(FATAL, "listen(%d, 10) failed with %d: %s\n",
+ server_fd_, errno, strerror(errno));
+ }
+ }
+ void SendFD(const int sock) {
+ int myfds[Buffers::kNumFDs]; /* Contains the file descriptors to pass. */
+ myfds[0] = fd_;
+ char buf[CMSG_SPACE(sizeof(myfds))]; /* ancillary data buffer */
+
+ iovec data;
+ memset(&data, 0, sizeof(data));
+ char dummy = 'A';
+ data.iov_base = &dummy;
+ data.iov_len = sizeof(dummy);
+ msghdr msg;
+ memset(&msg, 0, sizeof(msg));
+ msg.msg_iov = &data;
+ msg.msg_iovlen = 1;
+ msg.msg_control = buf;
+ msg.msg_controllen = sizeof(buf);
+ cmsghdr *const cmsg = CMSG_FIRSTHDR(&msg);
+ cmsg->cmsg_level = SOL_SOCKET;
+ cmsg->cmsg_type = SCM_RIGHTS;
+ cmsg->cmsg_len = CMSG_LEN(sizeof(myfds));
+ /* Initialize the payload: */
+ memcpy(CMSG_DATA(cmsg), myfds, sizeof(myfds));
+ if (sendmsg(sock, &msg, 0) == -1) {
+ LOG(ERROR, "sendmsg(%d, %p, 0) failed with %d: %s\n",
+ sock, &msg, errno, strerror(errno));
+ }
+ // leave it open so that the other end can tell if this process dies
+ }
+
+#if 0
+ // if we ever do want to do any of these things, this is how
+ void Stop() {
+ const v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_STREAMOFF, &type) == -1) {
+ errno_exit("VIDIOC_STREAMOFF");
+ }
+ }
+ void Close() {
+ if (close(fd_) == -1)
+ errno_exit("close");
+ fd_ = -1;
+ }
+#endif
+
+ void QueueBuffer(v4l2_buffer *buf) {
+ if (xioctl(fd_, VIDIOC_QBUF, buf) == -1) {
+ LOG(WARNING, "ioctl VIDIOC_QBUF(%d, %p) failed with %d: %s. losing buf #%"PRIu32"\n",
+ fd_, &buf, errno, strerror(errno), buf->index);
+ } else {
+ LOG(DEBUG, "put buf #%"PRIu32" into driver's queue\n", buf->index);
+ ++queued_;
+ }
+ }
+ void ReadFrame() {
+ v4l2_buffer buf;
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+
+ const Buffers::Message *read;
+ do {
+ read = static_cast<const Buffers::Message *>(
+ // we block waiting for one if we can't dequeue one without leaving
+ // the driver <= 2 (to be safe)
+ aos_queue_read_msg(recycle_queue_, (queued_ <= 2) ? BLOCK : NON_BLOCK));
+ if (read != NULL) {
+ buf.index = read->index;
+ aos_queue_free_msg(recycle_queue_, read);
+ QueueBuffer(&buf);
+ }
+ } while (read != NULL);
+
+ if (xioctl(fd_, VIDIOC_DQBUF, &buf) == -1) {
+ if (errno != EAGAIN) {
+ LOG(ERROR, "ioctl VIDIOC_DQBUF(%d, %p) failed with %d: %s\n",
+ fd_, &buf, errno, strerror(errno));
+ }
+ return;
+ }
+ --queued_;
+ if (buf.index >= Buffers::kNumBuffers) {
+ LOG(ERROR, "buf.index (%"PRIu32") is >= kNumBuffers (%u)\n",
+ buf.index, Buffers::kNumBuffers);
+ return;
+ }
+
+ Buffers::Message *const msg = static_cast<Buffers::Message *>(
+ aos_queue_get_msg(queue_));
+ if (msg == NULL) {
+ LOG(WARNING, "couldn't get a message to send buf #%"PRIu32" from queue %p."
+ " re-queueing now\n", buf.index, queue_);
+ QueueBuffer(&buf);
+ return;
+ }
+ msg->index = buf.index;
+ msg->bytesused = buf.bytesused;
+ memcpy(&msg->timestamp, &buf.timestamp, sizeof(msg->timestamp));
+ msg->sequence = buf.sequence;
+ if (aos_queue_write_msg_free(queue_, msg, OVERRIDE) == -1) {
+ LOG(WARNING, "sending message %p with buf #%"PRIu32" to queue %p failed."
+ " re-queueing now\n", msg, buf.index, queue_);
+ QueueBuffer(&buf);
+ return;
+ } else {
+ LOG(DEBUG, "sent message off to queue %p with buffer #%"PRIu32"\n",
+ queue_, buf.index);
+ }
+ }
+
+ void init_mmap() {
+ v4l2_requestbuffers req;
+ CLEAR(req);
+ req.count = Buffers::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);
+ } else {
+ LOG(FATAL, "ioctl VIDIOC_REQBUFS(%d, %p) failed with %d: %s\n",
+ fd_, &req, errno, strerror(errno));
+ }
+ }
+ queued_ = Buffers::kNumBuffers;
+ if (req.count < Buffers::kNumBuffers) {
+ LOG(FATAL, "Insufficient buffer memory on %s\n", dev_name);
+ }
+ }
+
+ void Init() {
+ v4l2_capability cap;
+ if (xioctl(fd_, VIDIOC_QUERYCAP, &cap) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s is no V4L2 device\n",
+ dev_name);
+ } else {
+ LOG(FATAL, "ioctl VIDIOC_QUERYCAP(%d, %p) failed with %d: %s\n",
+ fd_, &cap, errno, strerror(errno));
+ }
+ }
+ if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+ LOG(FATAL, "%s is no video capture device\n",
+ dev_name);
+ }
+ if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
+ LOG(FATAL, "%s does not support streaming i/o\n",
+ dev_name);
+ }
+
+ /* Select video input, video standard and tune here. */
+
+ v4l2_cropcap cropcap;
+ CLEAR(cropcap);
+ cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_CROPCAP, &cropcap) == 0) {
+ v4l2_crop crop;
+ crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ crop.c = cropcap.defrect; /* reset to default */
+
+ if (xioctl(fd_, VIDIOC_S_CROP, &crop) == -1) {
+ switch (errno) {
+ case EINVAL:
+ /* Cropping not supported. */
+ break;
+ default:
+ /* Errors ignored. */
+ break;
+ }
+ }
+ } else {
+ /* Errors ignored. */
+ }
+
+ v4l2_format fmt;
+ CLEAR(fmt);
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.width = Buffers::kWidth;
+ fmt.fmt.pix.height = Buffers::kHeight;
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
+ fmt.fmt.pix.field = V4L2_FIELD_ANY;
+ //fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+ //fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+ 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;
+
+#if 0
+ // 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 = 20;
+ if (xioctl(fd_, VIDIOC_S_PARM, setfps) == -1) {
+ LOG(ERROR, "ioctl VIDIOC_S_PARM(%d, %p) failed with %d: %s\n",
+ fd_, setfps, errno, strerror(errno));
+ exit(EXIT_FAILURE);
+ }
+ LOG(INFO, "framerate ended up at %d/%d\n",
+ setfps->parm.capture.timeperframe.numerator,
+ setfps->parm.capture.timeperframe.denominator);
+#endif
+
+ init_mmap();
+ }
+
+ void Start() {
+ LOG(DEBUG, "queueing buffers for the first time\n");
+ v4l2_buffer buf;
+ for (unsigned int i = 0; i < Buffers::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) {
+ LOG(FATAL, "ioctl VIDIOC_STREAMON(%d, %p) failed with %d: %s\n",
+ fd_, &type, errno, strerror(errno));
+ }
+ }
+
+ public:
+ void Run() {
+ Start();
+
+ fd_set fds;
+ timeval tv;
+ while (true) {
+ // HAVE TO DO THIS EACH TIME THROUGH THE LOOP
+ tv.tv_sec = 2;
+ tv.tv_usec = 0;
+
+ FD_ZERO(&fds);
+ FD_SET(fd_, &fds);
+ FD_SET(server_fd_, &fds);
+ switch (select(std::max(fd_, server_fd_) + 1, &fds, NULL, NULL, &tv)) {
+ case -1:
+ if (errno != EINTR) {
+ LOG(ERROR, "select(%d, %p, NULL, NULL, %p) failed with %d: %s\n",
+ std::max(fd_, server_fd_) + 1, &fds, &tv, errno, strerror(errno));
+ }
+ continue;
+ case 0:
+ LOG(WARNING, "select timed out\n");
+ continue;
+ }
+
+ if (FD_ISSET(fd_, &fds)) {
+ ReadFrame();
+ }
+ if (FD_ISSET(server_fd_, &fds)) {
+ const int sock = accept4(server_fd_, NULL, NULL, SOCK_NONBLOCK);
+ if (sock == -1) {
+ LOG(ERROR, "accept4(%d, NULL, NULL, SOCK_NONBLOCK(=%d) failed with %d: %s\n",
+ server_fd_, SOCK_NONBLOCK, errno, strerror(errno));
+ } else {
+ SendFD(sock);
+ }
+ }
+ }
+ }
+};
+const char *const Reader::dev_name = "/dev/video0";
+const aos_type_sig Reader::kRecycleSignature{
+ sizeof(Buffers::Message), 1, Buffers::kNumBuffers};
+
+} // namespace camera
+} // namespace aos
+
+AOS_RUN_NRT(aos::camera::Reader)
+