camerad: allow to log raw camera frames (#24393)

* camerad: log raw camera frames with env var

* dont qlog

* cleaner

* only road camera

* use vision buf len

* use static counter to handle frame skips

* we already have cnt

Co-authored-by: Willem Melching <willem.melching@gmail.com>
old-commit-hash: 05f606c812

selfdrive/camerad/cameras/camera_common.cc

@@ -146,6 +146,8 @@ bool CameraBuf::acquire() {
 
   double start_time = millis_since_boot();
 
+  cur_camera_buf = &camera_bufs[cur_buf_idx];
+
   if (debayer) {
     float gain = 0.0;
     float black_level = 42.0;
@@ -236,6 +238,17 @@ kj::Array<uint8_t> get_frame_image(const CameraBuf *b) {
   return kj::mv(frame_image);
 }
 
+kj::Array<uint8_t> get_raw_frame_image(const CameraBuf *b) {
+  const uint8_t *dat = (const uint8_t *)b->cur_camera_buf->addr;
+
+  kj::Array<uint8_t> frame_image = kj::heapArray<uint8_t>(b->cur_camera_buf->len);
+  uint8_t *resized_dat = frame_image.begin();
+
+  memcpy(resized_dat, dat, b->cur_camera_buf->len);
+
+  return kj::mv(frame_image);
+}
+
 static kj::Array<capnp::byte> yuv420_to_jpeg(const CameraBuf *b, int thumbnail_width, int thumbnail_height) {
   // make the buffer big enough. jpeg_write_raw_data requires 16-pixels aligned height to be used.
   std::unique_ptr<uint8[]> buf(new uint8_t[(thumbnail_width * ((thumbnail_height + 15) & ~15) * 3) / 2]);

selfdrive/camerad/cameras/camera_common.h

@@ -47,6 +47,7 @@ const bool env_disable_road = getenv("DISABLE_ROAD") != NULL;
 const bool env_disable_wide_road = getenv("DISABLE_WIDE_ROAD") != NULL;
 const bool env_disable_driver = getenv("DISABLE_DRIVER") != NULL;
 const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL;
+const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL;
 
 typedef void (*release_cb)(void *cookie, int buf_idx);
 
@@ -111,6 +112,7 @@ public:
   FrameMetadata cur_frame_data;
   VisionBuf *cur_rgb_buf;
   VisionBuf *cur_yuv_buf;
+  VisionBuf *cur_camera_buf;
   std::unique_ptr<VisionBuf[]> camera_bufs;
   std::unique_ptr<FrameMetadata[]> camera_bufs_metadata;
   int rgb_width, rgb_height, rgb_stride;
@@ -129,6 +131,7 @@ typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt);
 
 void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data);
 kj::Array<uint8_t> get_frame_image(const CameraBuf *b);
+kj::Array<uint8_t> get_raw_frame_image(const CameraBuf *b);
 float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip);
 std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback);
 

selfdrive/camerad/cameras/camera_qcom2.cc

@@ -1144,6 +1144,8 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
   fill_frame_data(framed, b->cur_frame_data);
   if ((c == &s->road_cam && env_send_road) || (c == &s->wide_road_cam && env_send_wide_road)) {
     framed.setImage(get_frame_image(b));
+  } else if (env_log_raw_frames && c == &s->road_cam && cnt % 100 == 5) {  // no overlap with qlog decimation
+    framed.setImage(get_raw_frame_image(b));
   }
   LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera");
   if (c == &s->road_cam) {