camtrack: onboard_controller: add timeout and restart

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

Author: srmainwaring

MAVProxy/modules/mavproxy_camtrack/onboard_controller.py

@@ -214,10 +214,15 @@ def run(self):
         # Connect to video stream
         video_stream = VideoStream(self._rtsp_url)
 
-        # TODO: add retry limit and timeout
+        # Timeout if video not available
+        video_last_update_time = time.time()
+        video_timeout_period = 5.0
         print("Waiting for video stream")
         while not video_stream.frame_available():
             print(".", end="")
+            if (time.time() - video_last_update_time) > video_timeout_period:
+                print("\nVideo stream not avaialble - restarting")
+                return
             time.sleep(0.1)
         print("\nVideo stream available")
 
@@ -246,8 +251,8 @@ def run(self):
         fps = 50
         update_rate = fps
         update_period = 1.0 / update_rate
+
         frame_count = 0
-        av_update_time = 0.0
         while True:
             start_time = time.time()
 
@@ -317,7 +322,6 @@ def __compare_rect(rect1, rect2):
             elapsed_time = time.time() - start_time
             sleep_time = max(0.0, update_period - elapsed_time)
             time.sleep(sleep_time)
-            av_update_time += elapsed_time
 
 
 class VideoStream:
@@ -1038,6 +1042,8 @@ def set_normalised_roi(self, nroi):
 
 
 if __name__ == "__main__":
+    import os
+    import sys
     from optparse import OptionParser
 
     parser = OptionParser("onboard_controller.py [options]")
@@ -1062,4 +1068,10 @@ def set_normalised_roi(self, nroi):
     controller = OnboardController(
         opts.master, opts.sysid, opts.cmpid, opts.rtsp_server
     )
-    controller.run()
+
+    while True:
+        try:
+            controller.run()
+        except KeyboardInterrupt:
+            EXIT_CODE_CTRL_C = 130
+            sys.exit(EXIT_CODE_CTRL_C)