Merge "Make adis16505 driver work for the rockpi"
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 159c779..4a8d50b 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -28,6 +28,7 @@
visibility = ["//visibility:public"],
deps = [
":vision_fbs",
+ "//aos/events:epoll",
"//aos/events:event_loop",
"//aos/scoped:scoped_fd",
"@com_github_google_glog//:glog",
diff --git a/frc971/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
index f3640db..e549ae5 100644
--- a/frc971/vision/v4l2_reader.cc
+++ b/frc971/vision/v4l2_reader.cc
@@ -302,9 +302,19 @@
}
RockchipV4L2Reader::RockchipV4L2Reader(aos::EventLoop *event_loop,
+ aos::internal::EPoll *epoll,
const std::string &device_name)
- : V4L2ReaderBase(event_loop, device_name) {
+ : V4L2ReaderBase(event_loop, device_name), epoll_(epoll) {
StreamOn();
+ epoll_->OnReadable(fd().get(), [this]() { OnImageReady(); });
+}
+
+void RockchipV4L2Reader::OnImageReady() {
+ if (!ReadLatestImage()) {
+ return;
+ }
+
+ SendLatestImage();
}
} // namespace vision
diff --git a/frc971/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
index a2b4c25..334ad81 100644
--- a/frc971/vision/v4l2_reader.h
+++ b/frc971/vision/v4l2_reader.h
@@ -5,6 +5,7 @@
#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"
@@ -62,6 +63,8 @@
// 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;
@@ -139,7 +142,13 @@
// properly configured before this class is constructed.
class RockchipV4L2Reader : public V4L2ReaderBase {
public:
- RockchipV4L2Reader(aos::EventLoop *event_loop, const std::string &device_name);
+ RockchipV4L2Reader(aos::EventLoop *event_loop, aos::internal::EPoll *epoll,
+ const std::string &device_name);
+
+ private:
+ void OnImageReady();
+
+ aos::internal::EPoll *epoll_;
};
} // namespace vision
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
new file mode 100644
index 0000000..2f93b16
--- /dev/null
+++ b/y2023/vision/BUILD
@@ -0,0 +1,29 @@
+cc_binary(
+ name = "camera_reader",
+ srcs = [
+ "camera_reader.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:media_device",
+ "//frc971/vision:v4l2_reader",
+ ],
+)
+
+cc_binary(
+ name = "viewer",
+ srcs = [
+ "viewer.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ ],
+)
diff --git a/y2023/vision/camera_reader.cc b/y2023/vision/camera_reader.cc
new file mode 100644
index 0000000..0e86c32
--- /dev/null
+++ b/y2023/vision/camera_reader.cc
@@ -0,0 +1,108 @@
+#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;
+
+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, 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();
+ }
+ */
+
+ event_loop.Run();
+}
+
+} // namespace
+} // namespace vision
+} // namespace y2022
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2022::vision::CameraReaderMain();
+}
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
new file mode 100644
index 0000000..990ad7a
--- /dev/null
+++ b/y2023/vision/viewer.cc
@@ -0,0 +1,88 @@
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/time/time.h"
+#include "frc971/vision/vision_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+DEFINE_string(channel, "/camera", "Channel name for the image.");
+
+DEFINE_string(capture, "",
+ "If set, capture a single image and save it to this filename.");
+
+namespace frc971 {
+namespace vision {
+namespace {
+
+aos::Fetcher<CameraImage> image_fetcher;
+bool DisplayLoop() {
+ const CameraImage *image;
+
+ // Read next image
+ if (!image_fetcher.Fetch()) {
+ VLOG(2) << "Couldn't fetch next image";
+ return true;
+ }
+
+ image = image_fetcher.get();
+ CHECK(image != nullptr) << "Couldn't read image";
+
+ // Create color image:
+ cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
+ (void *)image->data()->data());
+ cv::Mat bgr_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
+
+ if (!FLAGS_capture.empty()) {
+ cv::imwrite(FLAGS_capture, bgr_image);
+ return false;
+ }
+
+ cv::imshow("Display", bgr_image);
+ int keystroke = cv::waitKey(1);
+ if ((keystroke & 0xFF) == static_cast<int>('c')) {
+ // Convert again, to get clean image
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
+ std::stringstream name;
+ name << "capture-" << aos::realtime_clock::now() << ".png";
+ cv::imwrite(name.str(), bgr_image);
+ LOG(INFO) << "Saved image file: " << name.str();
+ } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ return false;
+ }
+ return true;
+}
+
+void ViewerMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ image_fetcher = event_loop.MakeFetcher<CameraImage>(FLAGS_channel);
+
+ // Run the display loop
+ event_loop.AddPhasedLoop(
+ [&event_loop](int) {
+ if (!DisplayLoop()) {
+ LOG(INFO) << "Calling event_loop Exit";
+ event_loop.Exit();
+ };
+ },
+ ::std::chrono::milliseconds(100));
+
+ event_loop.Run();
+
+ image_fetcher = aos::Fetcher<CameraImage>();
+}
+
+} // namespace
+} // namespace vision
+} // namespace frc971
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ frc971::vision::ViewerMain();
+}