trying the original OMEGA calc

IQTLabs · May 13, 2024 · a9916c0 · a9916c0
1 parent fb49fa3
commit a9916c0
Showing 1 changed file with 11 additions and 2 deletions.
diff --git a/axis-ptz-controller/object.py b/axis-ptz-controller/object.py
@@ -322,12 +322,21 @@ def recompute_location(self) -> None:
  )
 
 
+ # 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
+ # )
+
+ # The original code use the position when the last MSG was received, not the position with leading, giving this a try
  omega = (
  axis_ptz_utilities.cross(
- self.rst_point_lead_relative_to_tripod,
+ self.rst_point_msg_relative_to_tripod,
  self.rst_velocity_msg_relative_to_tripod,
  )
- / axis_ptz_utilities.norm(self.rst_point_lead_relative_to_tripod) ** 2
+ / axis_ptz_utilities.norm(self.rst_point_msg_relative_to_tripod) ** 2
  )
 
  self.rho_rate = math.degrees(-omega[2])