diff --git a/axis-ptz-controller/axis_ptz_controller.py b/axis-ptz-controller/axis_ptz_controller.py index 41a5b44..808a71e 100644 --- a/axis-ptz-controller/axis_ptz_controller.py +++ b/axis-ptz-controller/axis_ptz_controller.py @@ -653,10 +653,10 @@ def _track_object(self, time_since_last_update: float) -> None: "distance": self.object.distance_to_tripod3d, "focus": _focus, "zoom": _zoom, - "rho_rate_weird_delta": self.object.rho_rate_weird_delta, - "tau_rate_weird_delta": self.object.tau_rate_weird_delta, - "rho_rate_lead_delta": self.object.rho_rate_lead_delta, - "tau_rate_lead_delta": self.object.tau_rate_lead_delta, + "rho_lead_delta": self.object.rho_lead_delta, + "tau_lead_delta": self.object.tau_lead_delta, + "rho_rate_derivative": self.object.rho_rate_derivative, + "tau_rate_derivative": self.object.tau_rate_derivative, "pan_rate_index": self.camera.pan_rate_index, "tilt_rate_index": self.camera.tilt_rate_index, "delta_rho_dot_c": self.delta_rho_dot_c, diff --git a/axis-ptz-controller/object.py b/axis-ptz-controller/object.py index b76b440..1b48f39 100644 --- a/axis-ptz-controller/object.py +++ b/axis-ptz-controller/object.py @@ -90,6 +90,7 @@ def __init__( self.uvw_point_now_relative_to_tripod = np.zeros((3,)) self.uvw_point_lead_relative_to_tripod = np.zeros((3,)) + self.location_update_time = time() def _config_log(self) -> None: """Print to Logging the object configuration""" @@ -262,6 +263,9 @@ def recompute_location(self) -> None: self.rho = self.rho_lead self.tau = self.tau_lead + self.rho_lead_delta = self.rho_lead - self.rho_now + self.tau_lead_delta = self.tau_lead - self.tau_now + camera_yaw, camera_pitch, camera_roll = self.camera.get_yaw_pitch_roll() camera_E_XYZ, camera_N_XYZ, camera_z_XYZ = self.camera.get_e_n_z() # Compute position and velocity in the camera fixed (rst) @@ -306,37 +310,13 @@ def recompute_location(self) -> None: ) / axis_ptz_utilities.norm(self.rst_point_now_relative_to_tripod) ** 2 ) - + time_delta = time() - self.location_update_time + self.location_update_time = time() + self.rho_rate_derivative = (self.rho_rate - math.degrees(-omega[2])) / time_delta + self.tau_rate_derivative = (self.tau_rate - math.degrees(omega[0])) / time_delta self.rho_rate = math.degrees(-omega[2]) self.tau_rate = math.degrees(omega[0]) - omega_lead = ( - 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_lead = math.degrees(-omega_lead[2]) - self.tau_rate_lead = math.degrees(omega_lead[0]) - - omega_weird = ( - 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_now_relative_to_tripod) ** 2 - ) - - self.rho_rate_weird = math.degrees(-omega_weird[2]) - self.tau_rate_weird = math.degrees(omega_weird[0]) - - self.rho_rate_weird_delta = self.rho_rate_weird - self.rho_rate - self.tau_rate_weird_delta = self.tau_rate_weird - self.tau_rate - - self.rho_rate_lead_delta = self.rho_rate_lead - self.rho_rate - self.tau_rate_lead_delta = self.tau_rate_lead - self.tau_rate # Compute object slew rate # omega = (