From c86112ae77a875fa6494aedb330aa2230be87898 Mon Sep 17 00:00:00 2001
From: Kamil Trzcinski <ayufan@ayufan.eu>
Date: Thu, 7 Apr 2022 13:12:41 +0200
Subject: [PATCH] Not nice dropping of frames (with bad jitter)

---
 cmd/camera_isp.c |  5 +++++
 hw/buffer_lock.c | 41 +++++++++++++++++++++++++++++------------
 hw/buffer_lock.h |  1 +
 3 files changed, 35 insertions(+), 12 deletions(-)

diff --git a/cmd/camera_isp.c b/cmd/camera_isp.c
index bfd741a..0855da2 100644
--- a/cmd/camera_isp.c
+++ b/cmd/camera_isp.c
@@ -54,6 +54,11 @@ int camera_configure_isp(camera_t *camera, float high_div, float low_div)
 
   link_t *links = camera->links;
 
+  if (camera->options.fps > 0) {
+    camera->codec_jpeg->capture_list->fmt_interval_us = 1000 * 1000 / camera->options.fps;
+    printf("buffer_lock.c: frame interval: %d\n", camera->codec_jpeg->capture_list->fmt_interval_us);
+  }
+
   *links++ = (link_t){ camera->camera->capture_list, { camera->isp_srgb->output_list } };
 
   if (camera->isp_yuuv_low) {
diff --git a/hw/buffer_lock.c b/hw/buffer_lock.c
index e2b8b90..42c7917 100644
--- a/hw/buffer_lock.c
+++ b/hw/buffer_lock.c
@@ -1,4 +1,5 @@
 #include "hw/buffer_lock.h"
+#include "hw/buffer_list.h"
 
 bool buffer_lock_is_used(buffer_lock_t *buf_lock)
 {
@@ -38,19 +39,35 @@ bool buffer_lock_needs_buffer(buffer_lock_t *buf_lock)
 
 void buffer_lock_capture(buffer_lock_t *buf_lock, buffer_t *buf)
 {
+  uint64_t now = get_monotonic_time_us(NULL, NULL);
+  uint64_t captured_time_us = buf ? get_time_us(CLOCK_FROM_PARAMS, NULL, &buf->v4l2_buffer.timestamp, 0) : 0;
+
   pthread_mutex_lock(&buf_lock->lock);
-  buffer_consumed(buf_lock->buf, buf_lock->name);
-  buffer_use(buf);
-  buf_lock->buf = buf;
-  buf_lock->counter++;
-  uint64_t last_lock_us = buf_lock->buf_time_us;
-  uint64_t captured_time_us = get_time_us(CLOCK_FROM_PARAMS, NULL, &buf->v4l2_buffer.timestamp, 0);
-  buf_lock->buf_time_us = get_monotonic_time_us(NULL, NULL);
-  E_LOG_DEBUG(buf_lock, "Captured buffer %s (refs=%d), frame=%d, processing_us=%.1f, frame_us=%.1f",
-    dev_name(buf), buf ? buf->mmap_reflinks : 0, buf_lock->counter,
-    (buf_lock->buf_time_us - captured_time_us) / 1000.0f,
-    (buf_lock->buf_time_us - last_lock_us) / 1000.0f);
-  pthread_cond_broadcast(&buf_lock->cond_wait);
+  if (!buf) {
+    buffer_consumed(buf_lock->buf, buf_lock->name);
+    buf_lock->buf = NULL;
+    buf_lock->buf_time_us = now;
+  } else if (now - buf_lock->buf_time_us < buf->buf_list->fmt_interval_us) {
+    buf_lock->dropped++;
+
+    E_LOG_DEBUG(buf_lock, "Dropped buffer %s (refs=%d), frame=%d/%d, frame_us=%.1f",
+      dev_name(buf), buf ? buf->mmap_reflinks : 0,
+      buf_lock->counter, buf_lock->dropped,
+      (now - captured_time_us) / 1000.0f);
+  } else {
+    buffer_consumed(buf_lock->buf, buf_lock->name);
+    buffer_use(buf);
+    buf_lock->buf = buf;
+    buf_lock->counter++;
+    E_LOG_DEBUG(buf_lock, "Captured buffer %s (refs=%d), frame=%d/%d, processing_us=%.1f, frame_us=%.1f",
+      dev_name(buf), buf ? buf->mmap_reflinks : 0,
+      buf_lock->counter, buf_lock->dropped,
+      (now - captured_time_us) / 1000.0f,
+      (now - buf_lock->buf_time_us) / 1000.0f);
+    buf_lock->buf_time_us = now;
+    pthread_cond_broadcast(&buf_lock->cond_wait);
+  }
+
   pthread_mutex_unlock(&buf_lock->lock);
 }
 
diff --git a/hw/buffer_lock.h b/hw/buffer_lock.h
index d62ed46..66cd82b 100644
--- a/hw/buffer_lock.h
+++ b/hw/buffer_lock.h
@@ -12,6 +12,7 @@ typedef struct buffer_lock_s {
   uint64_t buf_time_us;
   int counter;
   int refs;
+  int dropped;
   uint64_t timeout_us;
 } buffer_lock_t;