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
diff --git a/y2023/vision/camera_reader.cc b/y2023/vision/camera_reader.cc
index 7004225..5105e86 100644
--- a/y2023/vision/camera_reader.cc
+++ b/y2023/vision/camera_reader.cc
@@ -23,31 +23,20 @@
media_device->Reset();
- media_device->Enable(
- media_device->FindLink("ov5647 4-0036", 0, "rkisp1_csi", 0));
- media_device->Enable(
- media_device->FindLink("rkisp1_csi", 1, "rkisp1_isp", 0));
- media_device->Enable(
- media_device->FindLink("rkisp1_isp", 2, "rkisp1_resizer_selfpath", 0));
- media_device->Enable(
- media_device->FindLink("rkisp1_isp", 2, "rkisp1_resizer_mainpath", 0));
-
- media_device->FindEntity("ov5647 4-0036")
- ->pads()[0]
- ->SetSubdevFormat(1296, 972, MEDIA_BUS_FMT_SBGGR10_1X10);
+ Entity *ov5647 = media_device->FindEntity("ov5647 4-0036");
+ ov5647->pads()[0]->SetSubdevFormat(1296, 972, MEDIA_BUS_FMT_SBGGR10_1X10);
Entity *rkisp1_csi = media_device->FindEntity("rkisp1_csi");
rkisp1_csi->pads()[0]->SetSubdevFormat(1296, 972, MEDIA_BUS_FMT_SBGGR10_1X10);
rkisp1_csi->pads()[1]->SetSubdevFormat(1296, 972, MEDIA_BUS_FMT_SBGGR10_1X10);
// TODO(austin): Should we set this on the link?
- // TODO(austin): Need to update crop too.
Entity *rkisp1_isp = media_device->FindEntity("rkisp1_isp");
- rkisp1_isp->pads(0)->SetSubdevCrop(1296, 972);
rkisp1_isp->pads(0)->SetSubdevFormat(1296, 972, MEDIA_BUS_FMT_SBGGR10_1X10);
+ rkisp1_isp->pads(0)->SetSubdevCrop(1296, 972);
- rkisp1_isp->pads(2)->SetSubdevCrop(1296, 972);
rkisp1_isp->pads(2)->SetSubdevFormat(1296, 972, MEDIA_BUS_FMT_YUYV8_2X8);
+ rkisp1_isp->pads(2)->SetSubdevCrop(1296, 972);
Entity *rkisp1_resizer_selfpath =
media_device->FindEntity("rkisp1_resizer_selfpath");
@@ -61,9 +50,10 @@
media_device->FindEntity("rkisp1_resizer_mainpath");
rkisp1_resizer_mainpath->pads(0)->SetSubdevFormat(1296, 972,
MEDIA_BUS_FMT_YUYV8_2X8);
+ rkisp1_resizer_mainpath->pads(0)->SetSubdevCrop(1296, 972);
+
rkisp1_resizer_mainpath->pads(1)->SetSubdevFormat(1296 / 2, 972 / 2,
MEDIA_BUS_FMT_YUYV8_2X8);
- rkisp1_resizer_mainpath->pads(0)->SetSubdevCrop(1296 / 2, 972 / 2);
Entity *rkisp1_mainpath = media_device->FindEntity("rkisp1_mainpath");
rkisp1_mainpath->SetFormat(1296 / 2, 972 / 2, V4L2_PIX_FMT_YUV422P);
@@ -71,6 +61,15 @@
Entity *rkisp1_selfpath = media_device->FindEntity("rkisp1_selfpath");
rkisp1_selfpath->SetFormat(1296, 972, V4L2_PIX_FMT_YUYV);
+ media_device->Enable(
+ media_device->FindLink("ov5647 4-0036", 0, "rkisp1_csi", 0));
+ media_device->Enable(
+ media_device->FindLink("rkisp1_csi", 1, "rkisp1_isp", 0));
+ media_device->Enable(
+ media_device->FindLink("rkisp1_isp", 2, "rkisp1_resizer_selfpath", 0));
+ media_device->Enable(
+ media_device->FindLink("rkisp1_isp", 2, "rkisp1_resizer_mainpath", 0));
+
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
@@ -79,23 +78,12 @@
event_loop.SetRuntimeRealtimePriority(55);
event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({2}));
- RockchipV4L2Reader v4l2_reader(&event_loop, event_loop.epoll(),
- rkisp1_selfpath->device());
- // TODO(austin): Figure out exposure and stuff.
- /*
- const uint32_t exposure =
- (FLAGS_use_outdoors ? FLAGS_outdoors_exposure : FLAGS_exposure);
- if (exposure > 0) {
- LOG(INFO) << "Setting camera to Manual Exposure mode with exposure = "
- << exposure << " or " << static_cast<double>(exposure) / 10.0
- << " ms";
- v4l2_reader.SetExposure(exposure);
- } else {
- LOG(INFO) << "Setting camera to use Auto Exposure";
- v4l2_reader.UseAutoExposure();
- }
- */
+ RockchipV4L2Reader v4l2_reader(&event_loop, event_loop.epoll(),
+ rkisp1_selfpath->device(), ov5647->device());
+
+ v4l2_reader.SetGain(1000);
+ v4l2_reader.SetExposure(1000);
event_loop.Run();
}