make derivative part of gain control

axis-ptz-controller/axis_ptz_controller.py

@@ -585,16 +585,16 @@ def _track_object(self, time_since_last_update: float) -> None:
  )
 
  # Compute slew rate differences
- self.rho_c_gain = self.pan_gain * self.delta_rho 
- self.tau_c_gain = self.tilt_gain * self.delta_tau
+ self.rho_c_gain = self.pan_gain * self.delta_rho * abs(self.object.rho_derivative)
+ self.tau_c_gain = self.tilt_gain * self.delta_tau * abs(self.object.tau_derivative)
 
  # Compute position and velocity in the camera fixed (rst)
  # coordinate system of the object relative to the tripod at
  # time zero after pointing the camera at the object
 
  # Update camera pan and tilt rate
- self.rho_dot_c = self.object.rho_rate + self.rho_c_gain
- self.tau_dot_c = self.object.tau_rate + self.tau_c_gain
+ self.rho_dot_c = self.object.rho_rate + self.rho_c_gain #- (self.object.rho_derivative ** 2)
+ self.tau_dot_c = self.object.tau_rate + self.tau_c_gain #- (self.object.tau_derivative ** 2)
 
  # Get, or compute and set focus, command camera pan and tilt
  # rates, and begin capturing images, if needed

axis-ptz-controller/camera.py

@@ -386,6 +386,7 @@ def slew_camera(self, rho_target: float, tau_target: float) -> None:
  )
  / (self.tilt_rate_max),
  )
+ duration = duration + 0.5
  logging.info(f"Sleeping: {duration} [s]")
  sleep(duration)
 

axis-ptz-controller/object.py

@@ -64,6 +64,8 @@ def __init__(
 
  # Distance between the object and the tripod at time one
  self.distance_to_tripod3d = 0.0 # [m]
+ self.rho_derivative = 0.0 # [deg/s]
+ self.tau_derivative = 0.0 # [deg/s]
 
  # Position and velocity in the geocentric (XYZ) coordinate
  # system of the object relative to the tripod at time zero
@@ -91,6 +93,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()
+ self.uninitialized = True
 
  def _config_log(self) -> None:
  """Print to Logging the object configuration"""
@@ -262,9 +265,15 @@ def recompute_location(self) -> None:
 
  time_delta = time() - self.location_update_time
  self.location_update_time = time()
- self.rho_derivative = (self.rho - self.rho_lead) / time_delta
- self.tau_derivative = (self.tau - self.tau_lead) / time_delta
 
+ if not self.uninitialized:
+ self.rho_derivative = (self.rho - self.rho_lead) / time_delta
+ self.tau_derivative = (self.tau - self.tau_lead) / time_delta
+
+ if self.rho_derivative > 100:
+ self.rho_derivative = 100
+ if self.tau_derivative > 100:
+ self.tau_derivative = 100
  self.rho = self.rho_lead
  self.tau = self.tau_lead
 
@@ -315,7 +324,7 @@ def recompute_location(self) -> None:
 
  self.rho_rate = math.degrees(-omega[2])
  self.tau_rate = math.degrees(omega[0])
-
+ self.uninitialized = False
 
  # Compute object slew rate
  # omega = (