From 0a41c1b71afe66580b89034eb3edd248297f38d0 Mon Sep 17 00:00:00 2001
From: Luke Berndt <lukekb@gmail.com>
Date: Mon, 25 Mar 2024 11:40:01 -0400
Subject: [PATCH] GO for Control Loop

---
 axis-ptz-controller/axis_ptz_controller.py | 17 ++++++++++++-----
 1 file changed, 12 insertions(+), 5 deletions(-)

diff --git a/axis-ptz-controller/axis_ptz_controller.py b/axis-ptz-controller/axis_ptz_controller.py
index 8c0ae68..ca48c5c 100644
--- a/axis-ptz-controller/axis_ptz_controller.py
+++ b/axis-ptz-controller/axis_ptz_controller.py
@@ -676,6 +676,7 @@ def _track_object(self) -> None:
         # Make sure Object info is not updated while pointing is being computed
         self.object_lock.acquire()
 
+        start_time = time()
         self._compute_object_pointing()
 
         if self.use_camera:
@@ -784,7 +785,8 @@ def _track_object(self) -> None:
                 # Intialize the object id to point the camera at the
                 # object directly once the camera reconnects
                 self.object_id = "NA"
-
+        
+        elapsed_time = time() - start_time
         # Log camera pointing using MQTT
         if self.log_to_mqtt:
             logger_msg = self.generate_payload_json(
@@ -822,6 +824,7 @@ def _track_object(self) -> None:
                             "delta_tau": self._compute_angle_delta(
                                 self.tau_c, self.tau_o
                             ),
+                            "tracking_loop_time": elapsed_time,
                             "object_id": self.object_id,
                         }
                     }
@@ -1018,16 +1021,12 @@ def _object_callback(
         self._reset_stop_timer()
         
         if self.use_camera:
-
-
             # Point the camera at any new object directly
             if self.object_id != object_id:
                 self.object_id = object_id
                 # Compute object pointing
                 self._compute_object_pointing()
                 self._slew_camera()
-            else:
-                self._track_object()
         else:
             logging.debug(f"Controller pan and tilt: {self.rho_c}, {self.tau_c} [deg]")
 
@@ -1383,6 +1382,8 @@ def main(self) -> None:
             capture_job = schedule.every(self.capture_interval).seconds.do(
                 self._capture_image
             )
+        tracking_interval = 0.25
+        update_tracking_time = time()
 
         # Enter the main loop
         while True:
@@ -1396,6 +1397,12 @@ def main(self) -> None:
                 if not self.use_camera:
                     self._update_pointing()
 
+
+                # Track object
+                if ( self.use_camera and time() - update_tracking_time > tracking_interval ):
+                    update_tracking_time = time()
+                    self._track_object()
+
                 # Command zero camera pan and tilt rates, and stop
                 # capturing images if a object message has not been
                 # received in twice the capture interval