more data

IQTLabs · May 12, 2024 · fb49fa3 · fb49fa3
1 parent ad66521
commit fb49fa3
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 3 deletions.
diff --git a/axis-ptz-controller/axis_ptz_controller.py b/axis-ptz-controller/axis_ptz_controller.py
@@ -725,6 +725,12 @@ def _track_object(self, time_since_last_update: float) -> None:
                             "tau_dot_c": self.tau_dot_c,
                             "rho_c_gain": self.rho_c_gain,
                             "tau_c_gain": self.tau_c_gain,
+                            "rst_vel_o_0": self.object.rst_velocity_msg_relative_to_tripod[0],
+                            "rst_vel_o_1": self.object.rst_velocity_msg_relative_to_tripod[1],
+                            "rst_vel_o_2": self.object.rst_velocity_msg_relative_to_tripod[2],
+                            "rst_point_o_0": self.object.rst_point_msg_relative_to_tripod[0],
+                            "rst_point_o_1": self.object.rst_point_msg_relative_to_tripod[1],
+                            "rst_point_o_2": self.object.rst_point_msg_relative_to_tripod[2],
                             "distance": self.object.distance_to_tripod3d,
                             "focus": _focus,
                             "zoom": _zoom,

diff --git a/axis-ptz-controller/object.py b/axis-ptz-controller/object.py
@@ -167,8 +167,7 @@ def update_from_msg(self, msg: Dict[str, Any]) -> None:
             self.camera.get_xyz_to_enz_transformation_matrix().transpose(),
             self.enz_velocity_msg_relative_to_tripod,
         )
-        logging.info(f"XYZ object velocity relative to tripod: {self.xyz_velocity_msg_relative_to_tripod} [m/s]")
-        logging.info(f"ENZ object velocity relative to tripod: {self.enz_velocity_msg_relative_to_tripod} [m/s]")
+
         self.no_derivative = True # things get weird when you update the object and then recompute the derivative
         self.object_lock.release()
 
@@ -322,14 +321,15 @@ def recompute_location(self) -> None:
             self.E_XYZ_to_rst, self.xyz_velocity_msg_relative_to_tripod
         )
 
+
         omega = (
             axis_ptz_utilities.cross(
                 self.rst_point_lead_relative_to_tripod,
                 self.rst_velocity_msg_relative_to_tripod,
             )
             / axis_ptz_utilities.norm(self.rst_point_lead_relative_to_tripod) ** 2
         )
-
+        
         self.rho_rate = math.degrees(-omega[2])
         self.tau_rate = math.degrees(omega[0])
         self.no_derivative = False