[video] runtime adjustable fps

and use CLOCK_MONTONIC instead of gettimeofday
paparazzi · Sep 15, 2015 · 61306d7 · 61306d7
1 parent 23cac90
commit 61306d7
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 10 deletions.
diff --git a/conf/modules/video_thread.xml b/conf/modules/video_thread.xml
@@ -24,6 +24,7 @@
         <dl_setting var="video_thread.take_shot" min="0" step="1" max="1" shortname="take_shot" module="computer_vision/video_thread" handler="take_shot">
           <strip_button name="Shoot" icon="digital-camera.png" value="1" group="video"/>
         </dl_setting>
+        <dl_setting var="video_thread.fps" min="1" step="1" max="25" shortname="fps" module="computer_vision/video_thread" param="VIDEO_THREAD_FPS"/>
       </dl_settings>
     </dl_settings>
   </settings>

diff --git a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h
@@ -57,4 +57,17 @@ static inline void sys_time_usleep(uint32_t us)
   usleep(us);
 }
 
+/** elapsed time in microsecs between two timespecs */
+static inline unsigned int sys_time_elapsed_us(struct timespec *prev, struct timespec *now)
+{
+  time_t d_sec = now->tv_sec - prev->tv_sec;
+  long d_nsec = now->tv_nsec - prev->tv_nsec;
+  /* wrap if negative nanoseconds */
+  if (d_nsec < 0) {
+    d_sec -= 1;
+    d_nsec += 1000000000L;
+  }
+  return d_sec * 1000000 + d_nsec / 1000;
+}
+
 #endif /* SYS_TIME_ARCH_H */
diff --git a/sw/airborne/modules/computer_vision/video_thread.c b/sw/airborne/modules/computer_vision/video_thread.c
@@ -41,6 +41,8 @@
 #include "lib/encoding/jpeg.h"
 #include "peripherals/video_device.h"
 
+#include "mcu_periph/sys_time.h"
+
 // include board for bottom_camera and front_camera on ARDrone2 and Bebop
 #include BOARD_CONFIG
 
@@ -168,20 +170,24 @@ static void *video_thread_function(void *data)
   set_nice_level(10);
 
   // Initialize timing
-  uint32_t microsleep = (uint32_t)(1000000. / (float)video_thread.fps);
-  struct timeval last_time;
-  gettimeofday(&last_time, NULL);
+  struct timespec time_now;
+  struct timespec time_prev;
+  clock_gettime(CLOCK_MONOTONIC, &time_prev);
 
   // Start streaming
   video_thread.is_running = TRUE;
   while (video_thread.is_running) {
-    // compute usleep to have a more stable frame rate
-    struct timeval vision_thread_sleep_time;
-    gettimeofday(&vision_thread_sleep_time, NULL);
-    int dt = (int)(vision_thread_sleep_time.tv_sec - last_time.tv_sec) * 1000000 +
-             (int)(vision_thread_sleep_time.tv_usec - last_time.tv_usec);
-    if (dt < microsleep) { usleep(microsleep - dt); }
-    last_time = vision_thread_sleep_time;
+
+    // get time in us since last run
+    clock_gettime(CLOCK_MONOTONIC, &time_now);
+    unsigned int dt_us = sys_time_elapsed_us(&time_prev, &time_now);
+    time_prev = time_now;
+
+    // sleep remaining time to limit to specified fps
+    uint32_t fps_period_us = (uint32_t)(1000000. / (float)video_thread.fps);
+    if (dt_us < fps_period_us) {
+      usleep(fps_period_us - dt_us);
+    }
 
     // Wait for a new frame (blocking)
     struct image_t img;