Make 2023 camera start first try
We had an order of operations problem with some of the pin
configuration. Fix that and set an exposure + gain as well.
Change-Id: Ic2447b28656f9fa3bee3762ad752d86c17836e2a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/vision/media_device.cc b/frc971/vision/media_device.cc
index 1813b6f..4c227a8 100644
--- a/frc971/vision/media_device.cc
+++ b/frc971/vision/media_device.cc
@@ -365,8 +365,12 @@
Link *MediaDevice::FindLink(std::string_view source, int source_pad_index,
std::string_view sink, int sink_pad_index) {
- Entity *source_entity = CHECK_NOTNULL(FindEntity(source));
- Entity *sink_entity = CHECK_NOTNULL(FindEntity(sink));
+ Entity *source_entity = FindEntity(source);
+ Entity *sink_entity = FindEntity(sink);
+
+ CHECK(source_entity != nullptr) << ": Failed to find source " << source;
+ CHECK(sink_entity != nullptr) << ": Failed to find sink " << sink;
+
Pad *source_pad = source_entity->pads()[source_pad_index];
Pad *sink_pad = sink_entity->pads()[sink_pad_index];
for (size_t i = 0; i < source_pad->links_size(); ++i) {
diff --git a/frc971/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
index e549ae5..63e33e6 100644
--- a/frc971/vision/v4l2_reader.cc
+++ b/frc971/vision/v4l2_reader.cc
@@ -92,7 +92,7 @@
PCHECK(Ioctl(VIDIOC_STREAMON, &type) == 0);
}
-bool V4L2ReaderBase::ReadLatestImage() {
+void V4L2ReaderBase::MaybeEnqueue() {
// First, enqueue any old buffer we already have. This is the one which
// may have been sent.
if (saved_buffer_) {
@@ -100,6 +100,11 @@
saved_buffer_.Clear();
}
ftrace_.FormatMessage("Enqueued previous buffer %d", saved_buffer_.index);
+}
+
+bool V4L2ReaderBase::ReadLatestImage() {
+ MaybeEnqueue();
+
while (true) {
const BufferInfo previous_buffer = saved_buffer_;
saved_buffer_ = DequeueBuffer();
@@ -303,8 +308,14 @@
RockchipV4L2Reader::RockchipV4L2Reader(aos::EventLoop *event_loop,
aos::internal::EPoll *epoll,
- const std::string &device_name)
- : V4L2ReaderBase(event_loop, device_name), epoll_(epoll) {
+ const std::string &device_name,
+ const std::string &image_sensor_subdev)
+ : V4L2ReaderBase(event_loop, device_name),
+ epoll_(epoll),
+ image_sensor_fd_(open(image_sensor_subdev.c_str(), O_RDWR | O_NONBLOCK)) {
+ PCHECK(image_sensor_fd_.get() != -1)
+ << " Failed to open device " << device_name;
+
StreamOn();
epoll_->OnReadable(fd().get(), [this]() { OnImageReady(); });
}
@@ -317,5 +328,31 @@
SendLatestImage();
}
+int RockchipV4L2Reader::ImageSensorIoctl(unsigned long number, void *arg) {
+ return ioctl(image_sensor_fd_.get(), number, arg);
+}
+
+void RockchipV4L2Reader::SetExposure(size_t duration) {
+ v4l2_control exposure_control;
+ exposure_control.id = V4L2_CID_EXPOSURE;
+ exposure_control.value = static_cast<int>(duration);
+ PCHECK(ImageSensorIoctl(VIDIOC_S_CTRL, &exposure_control) == 0);
+}
+
+void RockchipV4L2Reader::SetGain(size_t gain) {
+ struct v4l2_ext_controls controls;
+ memset(&controls, 0, sizeof(controls));
+ struct v4l2_ext_control control[1];
+ memset(&control, 0, sizeof(control));
+
+ controls.ctrl_class = V4L2_CTRL_CLASS_IMAGE_SOURCE;
+ controls.count = 1;
+ controls.controls = control;
+ control[0].id = V4L2_CID_ANALOGUE_GAIN;
+ control[0].value = gain;
+
+ PCHECK(ImageSensorIoctl(VIDIOC_S_EXT_CTRLS, &controls) == 0);
+}
+
} // namespace vision
} // namespace frc971
diff --git a/frc971/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
index 334ad81..2013113 100644
--- a/frc971/vision/v4l2_reader.h
+++ b/frc971/vision/v4l2_reader.h
@@ -31,6 +31,8 @@
// 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
@@ -46,7 +48,7 @@
// Sets the exposure duration of the camera. duration is the number of 100
// microsecond units.
- void SetExposure(size_t duration);
+ virtual void SetExposure(size_t duration);
// Switches from manual to auto exposure.
void UseAutoExposure();
@@ -143,12 +145,21 @@
class RockchipV4L2Reader : public V4L2ReaderBase {
public:
RockchipV4L2Reader(aos::EventLoop *event_loop, aos::internal::EPoll *epoll,
- const std::string &device_name);
+ 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