sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8

openpilot/system/loggerd/encoder/jpeg_encoder.cc

@@ -0,0 +1,115 @@
+#include "system/loggerd/encoder/jpeg_encoder.h"
+
+#include <cassert>
+#include <cstring>
+
+#include "common/swaglog.h"
+
+// Lower qscale = higher quality / bigger files for MJPEG.
+constexpr int MJPEG_QSCALE = 7;
+
+JpegEncoder::JpegEncoder(const std::string &publish_name, int width, int height)
+    : publish_name(publish_name), thumbnail_width(width), thumbnail_height(height) {
+  yuv_buffer.resize((thumbnail_width * thumbnail_height * 3) / 2);
+  pm = std::make_unique<PubMaster>(std::vector{publish_name.c_str()});
+
+  const AVCodec *codec = avcodec_find_encoder(AV_CODEC_ID_MJPEG);
+  assert(codec);
+
+  codec_ctx = avcodec_alloc_context3(codec);
+  assert(codec_ctx);
+  codec_ctx->width = thumbnail_width;
+  codec_ctx->height = thumbnail_height;
+  codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
+  codec_ctx->time_base = (AVRational){1, 1};
+  codec_ctx->color_range = AVCOL_RANGE_JPEG;
+  codec_ctx->flags |= AV_CODEC_FLAG_QSCALE;
+  codec_ctx->global_quality = FF_QP2LAMBDA * MJPEG_QSCALE;
+
+  int err = avcodec_open2(codec_ctx, codec, NULL);
+  assert(err >= 0);
+
+  frame = av_frame_alloc();
+  assert(frame);
+  frame->format = codec_ctx->pix_fmt;
+  frame->width = thumbnail_width;
+  frame->height = thumbnail_height;
+  frame->linesize[0] = thumbnail_width;
+  frame->linesize[1] = thumbnail_width / 2;
+  frame->linesize[2] = thumbnail_width / 2;
+  frame->color_range = AVCOL_RANGE_JPEG;
+
+  pkt = av_packet_alloc();
+  assert(pkt);
+}
+
+JpegEncoder::~JpegEncoder() {
+  av_packet_free(&pkt);
+  av_frame_free(&frame);
+  avcodec_free_context(&codec_ctx);
+}
+
+void JpegEncoder::pushThumbnail(VisionBuf *buf, const VisionIpcBufExtra &extra) {
+  generateThumbnail(buf->y, buf->uv, buf->width, buf->height, buf->stride);
+
+  MessageBuilder msg;
+  auto thumbnaild = msg.initEvent().initThumbnail();
+  thumbnaild.setFrameId(extra.frame_id);
+  thumbnaild.setTimestampEof(extra.timestamp_eof);
+  thumbnaild.setThumbnail({out_buffer.data(), out_buffer.size()});
+
+  pm->send(publish_name.c_str(), msg);
+}
+
+void JpegEncoder::generateThumbnail(const uint8_t *y_addr, const uint8_t *uv_addr, int width, int height, int stride) {
+  int downscale = width / thumbnail_width;
+  assert(downscale * thumbnail_height == height);
+
+  uint8_t *y_plane = yuv_buffer.data();
+  uint8_t *u_plane = y_plane + thumbnail_width * thumbnail_height;
+  uint8_t *v_plane = u_plane + (thumbnail_width * thumbnail_height) / 4;
+  {
+    // subsampled conversion from nv12 to yuv420p
+    for (int hy = 0; hy < thumbnail_height / 2; hy++) {
+      for (int hx = 0; hx < thumbnail_width / 2; hx++) {
+        int ix = hx * downscale + (downscale - 1) / 2;
+        int iy = hy * downscale + (downscale - 1) / 2;
+        y_plane[(hy * 2 + 0) * thumbnail_width + (hx * 2 + 0)] = y_addr[(iy * 2 + 0) * stride + ix * 2 + 0];
+        y_plane[(hy * 2 + 0) * thumbnail_width + (hx * 2 + 1)] = y_addr[(iy * 2 + 0) * stride + ix * 2 + 1];
+        y_plane[(hy * 2 + 1) * thumbnail_width + (hx * 2 + 0)] = y_addr[(iy * 2 + 1) * stride + ix * 2 + 0];
+        y_plane[(hy * 2 + 1) * thumbnail_width + (hx * 2 + 1)] = y_addr[(iy * 2 + 1) * stride + ix * 2 + 1];
+        u_plane[hy * thumbnail_width / 2 + hx] = uv_addr[iy * stride + ix * 2 + 0];
+        v_plane[hy * thumbnail_width / 2 + hx] = uv_addr[iy * stride + ix * 2 + 1];
+      }
+    }
+  }
+
+  compressToJpeg(y_plane, u_plane, v_plane);
+}
+
+void JpegEncoder::compressToJpeg(uint8_t *y_plane, uint8_t *u_plane, uint8_t *v_plane) {
+  frame->data[0] = y_plane;
+  frame->data[1] = u_plane;
+  frame->data[2] = v_plane;
+  // Required for MJPEG qscale to take effect (global_quality alone is not enough).
+  frame->quality = FF_QP2LAMBDA * MJPEG_QSCALE;
+  frame->pts = 0;
+
+  int err = avcodec_send_frame(codec_ctx, frame);
+  if (err < 0) {
+    LOGE("thumbnail avcodec_send_frame error %d", err);
+    out_buffer.clear();
+    return;
+  }
+
+  av_packet_unref(pkt);
+  err = avcodec_receive_packet(codec_ctx, pkt);
+  if (err < 0) {
+    LOGE("thumbnail avcodec_receive_packet error %d", err);
+    out_buffer.clear();
+    return;
+  }
+
+  out_buffer.assign(pkt->data, pkt->data + pkt->size);
+  av_packet_unref(pkt);
+}