frc971/vision/v4l2_reader.h

#ifndef FRC971_VISION_V4L2_READER_H_
#define FRC971_VISION_V4L2_READER_H_

#include <array>
#include <string>

#include "absl/types/span.h"
#include "aos/events/epoll.h"
#include "aos/events/event_loop.h"
#include "aos/ftrace.h"
#include "aos/scoped/scoped_fd.h"
#include "frc971/vision/vision_generated.h"
#include "glog/logging.h"

namespace frc971 {
namespace vision {

// Reads images from a V4L2 capture device (aka camera).
class V4L2ReaderBase {
 public:
  // device_name is the name of the device file (like "/dev/video0").
  V4L2ReaderBase(aos::EventLoop *event_loop, const std::string &device_name);

  V4L2ReaderBase(const V4L2ReaderBase &) = delete;
  V4L2ReaderBase &operator=(const V4L2ReaderBase &) = delete;

  // Reads the latest image.
  //
  // Returns false if no image was available since the last image was read.
  // Call LatestImage() to get a reference to the data, which will be valid
  // until this method is called again.
  bool ReadLatestImage();

  void MaybeEnqueue();

  // Sends the latest image.
  //
  // ReadLatestImage() must have returned a non-empty span the last time it was
  // called. After calling this, the data which was returned from
  // ReadLatestImage() will no longer be valid.
  void SendLatestImage();

  const CameraImage &LatestImage() {
    Buffer *const buffer = &buffers_[saved_buffer_.index];
    return *flatbuffers::GetTemporaryPointer(*buffer->builder.fbb(),
                                             buffer->message_offset);
  }

  // Sets the exposure duration of the camera. duration is the number of 100
  // microsecond units.
  virtual void SetExposure(size_t duration);

  // Switches from manual to auto exposure.
  void UseAutoExposure();

 protected:
  void StreamOff();
  void StreamOn();

  int Ioctl(unsigned long number, void *arg);

  bool multiplanar() const { return multiplanar_; }

  // TODO(Brian): This concept won't exist once we start using variable-size
  // H.264 frames.
  size_t ImageSize() const { return rows_ * cols_ * 2 /* bytes per pixel */; }

  const aos::ScopedFD &fd() { return fd_; };

 private:
  static constexpr int kNumberBuffers = 4;

  struct Buffer {
    void InitializeMessage(size_t max_image_size);

    void PrepareMessage(int rows, int cols, size_t image_size,
                        aos::monotonic_clock::time_point monotonic_eof);

    void Send() {
      (void)builder.Send(message_offset);
      message_offset = flatbuffers::Offset<CameraImage>();
    }

    absl::Span<const char> DataSpan(size_t image_size) {
      return absl::Span<const char>(
          reinterpret_cast<char *>(CHECK_NOTNULL(data_pointer)), image_size);
    }

    aos::Sender<CameraImage> sender;
    aos::Sender<CameraImage>::Builder builder;
    flatbuffers::Offset<CameraImage> message_offset;

    uint8_t *data_pointer = nullptr;
  };

  struct BufferInfo {
    int index = -1;
    aos::monotonic_clock::time_point monotonic_eof =
        aos::monotonic_clock::min_time;

    explicit operator bool() const { return index != -1; }

    void Clear() {
      index = -1;
      monotonic_eof = aos::monotonic_clock::min_time;
    }
  };

  // Attempts to dequeue a buffer (nonblocking). Returns the index of the new
  // buffer, or BufferInfo() if there wasn't a frame to dequeue.
  BufferInfo DequeueBuffer();

  void EnqueueBuffer(int buffer);

  // The mmaped V4L2 buffers.
  std::array<Buffer, kNumberBuffers> buffers_;

  // If this is non-negative, it's the buffer number we're currently holding
  // onto.
  BufferInfo saved_buffer_;

  bool multiplanar_ = false;

  int rows_ = 0;
  int cols_ = 0;

  aos::ScopedFD fd_;

  aos::EventLoop *event_loop_;
  aos::Ftrace ftrace_;
};

// Generic V4L2 reader for pi's and older.
class V4L2Reader : public V4L2ReaderBase {
 public:
  V4L2Reader(aos::EventLoop *event_loop, const std::string &device_name);

 private:
  const int rows_ = 480;
  const int cols_ = 640;
};

// Rockpi specific v4l2 reader.  This assumes that the media device has been
// properly configured before this class is constructed.
class RockchipV4L2Reader : public V4L2ReaderBase {
 public:
  RockchipV4L2Reader(aos::EventLoop *event_loop, aos::internal::EPoll *epoll,
                     const std::string &device_name,
                     const std::string &image_sensor_subdev);

  void SetExposure(size_t duration) override;

  void SetGain(size_t gain);

 private:
  void OnImageReady();

  int ImageSensorIoctl(unsigned long number, void *arg);

  aos::internal::EPoll *epoll_;

  aos::ScopedFD image_sensor_fd_;
};

}  // namespace vision
}  // namespace frc971

#endif  // FRC971_VISION_V4L2_READER_H_