[video] Retrieve FPS from camera thread as a setting. Show console de…

…bug info when needed.
paparazzi · Apr 21, 2022 · d4085fd · d4085fd
1 parent 7a9f1c9
commit d4085fd
Show file tree

Hide file tree

Showing 5 changed files with 51 additions and 0 deletions.
diff --git a/conf/modules/video_thread.xml b/conf/modules/video_thread.xml
@@ -13,6 +13,19 @@
     <define name="VIDEO_THREAD_NICE_LEVEL" value="5" description="Nice level for each separate video thread"/>
   </doc>
 
+  <settings>
+    <dl_settings>
+      <dl_settings name="thread">
+        <dl_setting var="video_thread_debug" min="0" step="1" max="1" shortname="debug" module="computer_vision/video_thread" />
+        <dl_setting var="video_thread_fps_min" min="0" step="1" max="1" shortname="FPS_min" />
+        <dl_setting var="video_thread_fps" min="0" step="1" max="1" shortname="FPS_avg" />
+        <dl_setting var="video_thread_ms_max" min="0" step="1" max="1" shortname="ms_max" />
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
+
+
   <header>
     <file name="video_thread.h"/>
   </header>

diff --git a/sw/airborne/modules/computer_vision/video_thread.c b/sw/airborne/modules/computer_vision/video_thread.c
@@ -70,6 +70,12 @@ PRINT_CONFIG_VAR(VIDEO_THREAD_MAX_CAMERAS)
 
 #define printf_debug    if(VIDEO_THREAD_VERBOSE > 0) printf
 
+uint8_t video_thread_debug = 0;
+float video_thread_fps = 0;
+float video_thread_fps_min = 0;
+float video_thread_ms_max = 0;
+
+
 static struct video_config_t *cameras[VIDEO_THREAD_MAX_CAMERAS] = {NULL};
 
 // Main thread
@@ -151,7 +157,23 @@ static void *video_thread_function(void *data)
     // Free the image
     v4l2_image_free(vid->thread.dev, &img);
 
+    uint32_t time_end = get_sys_time_usec();
+    uint32_t time_dt = time_end - time_begin;
+    RunOnceEvery(100,video_thread_ms_max = 0);
+    if (time_dt/1000.0f > video_thread_ms_max) {
+      video_thread_ms_max = time_dt/1000.0f;
+    }
+    if (video_thread_debug > 0) {
+      fprintf(stdout, "[%s] vision = %i us, vid_fps=%i current fps=%f\n ",print_tag, time_dt, vid->fps, 1000000.f / frame_dt_us);
+    }
+
     // sleep (most of the) remaining time to limit to specified fps
+    float fps_temp = 1000000.f / frame_dt_us;
+    video_thread_fps = 0.95 * video_thread_fps + 0.05 * fps_temp;
+    RunOnceEvery(100,video_thread_fps_min = 90.0f);
+    if (fps_temp < video_thread_fps_min) {
+      video_thread_fps_min = fps_temp;
+    }
     if (vid->fps > 0) {
       uint32_t fps_period_us = 1000000 / vid->fps;
       if (frame_dt_us > fps_period_us + 10000) {

diff --git a/sw/airborne/modules/computer_vision/video_thread.h b/sw/airborne/modules/computer_vision/video_thread.h
@@ -33,6 +33,11 @@
 #include "std.h"
 #include "modules/computer_vision/cv.h"
 
+extern uint8_t video_thread_debug;
+extern float video_thread_fps;
+extern float video_thread_fps_min;
+extern float video_thread_ms_max;
+
 extern void video_thread_init(void);
 extern void video_thread_periodic(void); ///< A dummy for now
 extern void video_thread_start(void);

diff --git a/sw/airborne/modules/computer_vision/video_thread_nps.c b/sw/airborne/modules/computer_vision/video_thread_nps.c
@@ -80,6 +80,11 @@ struct video_config_t bottom_camera = {
   }
 };
 
+uint8_t video_thread_debug = 0;
+float video_thread_fps = 0;
+float video_thread_fps_min = 0;
+float video_thread_ms_max = 0;
+
 // Keep track of added devices.
 struct video_config_t *cameras[VIDEO_THREAD_MAX_CAMERAS] = { NULL };
 

diff --git a/sw/airborne/modules/computer_vision/video_thread_nps.h b/sw/airborne/modules/computer_vision/video_thread_nps.h
@@ -30,6 +30,12 @@
 
 #include "peripherals/video_device.h"
 
+extern uint8_t video_thread_debug;
+extern float video_thread_fps;
+extern float video_thread_fps_min;
+extern float video_thread_ms_max;
+
+
 #ifndef VIDEO_THREAD_MAX_CAMERAS
 #define VIDEO_THREAD_MAX_CAMERAS 4
 #endif