Add rockpi camera reader and viewer.
This uses our fancy media pipeline to configure the chip to take images.
Change-Id: I7393b3892e93252d8d6d28fdb905e94d9526cbf1
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/vision/camera_reader.cc b/y2023/vision/camera_reader.cc
new file mode 100644
index 0000000..965296f
--- /dev/null
+++ b/y2023/vision/camera_reader.cc
@@ -0,0 +1,137 @@
+#include "absl/strings/str_cat.h"
+#include "absl/strings/str_split.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/vision/media_device.h"
+#include "frc971/vision/v4l2_reader.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+namespace y2022 {
+namespace vision {
+namespace {
+
+using namespace frc971::vision;
+
+class CameraReader {
+ public:
+ CameraReader(aos::EventLoop *event_loop, V4L2ReaderBase *reader)
+ : event_loop_(event_loop),
+ reader_(reader),
+ read_image_timer_(event_loop->AddTimer([this]() { ReadImage(); })) {
+ event_loop->OnRun(
+ [this]() { read_image_timer_->Setup(event_loop_->monotonic_now()); });
+ }
+
+ void ReadImage() {
+ if (!reader_->ReadLatestImage()) {
+ read_image_timer_->Setup(event_loop_->monotonic_now() +
+ std::chrono::milliseconds(10));
+ return;
+ }
+ reader_->SendLatestImage();
+
+ read_image_timer_->Setup(event_loop_->monotonic_now());
+ }
+
+ private:
+ aos::EventLoop *event_loop_;
+ V4L2ReaderBase *reader_;
+
+ aos::TimerHandler *const read_image_timer_;
+};
+
+void CameraReaderMain() {
+ std::optional<MediaDevice> media_device = FindMediaDevice("platform:rkisp1");
+
+ if (VLOG_IS_ON(1)) {
+ media_device->Log();
+ }
+
+ 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 *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(2)->SetSubdevCrop(1296, 972);
+ rkisp1_isp->pads(2)->SetSubdevFormat(1296, 972, MEDIA_BUS_FMT_YUYV8_2X8);
+
+ Entity *rkisp1_resizer_selfpath =
+ media_device->FindEntity("rkisp1_resizer_selfpath");
+ rkisp1_resizer_selfpath->pads(0)->SetSubdevFormat(1296, 972,
+ MEDIA_BUS_FMT_YUYV8_2X8);
+ rkisp1_resizer_selfpath->pads(1)->SetSubdevFormat(1296, 972,
+ MEDIA_BUS_FMT_YUYV8_2X8);
+ rkisp1_resizer_selfpath->pads(0)->SetSubdevCrop(1296, 972);
+
+ Entity *rkisp1_resizer_mainpath =
+ media_device->FindEntity("rkisp1_resizer_mainpath");
+ rkisp1_resizer_mainpath->pads(0)->SetSubdevFormat(1296, 972,
+ MEDIA_BUS_FMT_YUYV8_2X8);
+ 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);
+
+ Entity *rkisp1_selfpath = media_device->FindEntity("rkisp1_selfpath");
+ rkisp1_selfpath->SetFormat(1296, 972, V4L2_PIX_FMT_YUYV);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ event_loop.SetRuntimeRealtimePriority(55);
+
+ RockchipV4L2Reader v4l2_reader(&event_loop, 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();
+ }
+ */
+
+ CameraReader camera_reader(&event_loop, &v4l2_reader);
+
+ event_loop.Run();
+}
+
+} // namespace
+} // namespace vision
+} // namespace y2022
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2022::vision::CameraReaderMain();
+}