more data to collect

IQTLabs · Apr 28, 2024 · 897bc7a · 897bc7a
1 parent 81ef553
commit 897bc7a
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 3 deletions.
diff --git a/axis-ptz-controller/axis_ptz_controller.py b/axis-ptz-controller/axis_ptz_controller.py
@@ -686,6 +686,8 @@ def _track_object(self, time_since_last_update: float) -> None:
                             "timestamp_c": self.timestamp_c,
                             "rho_o": self.object.rho,
                             "tau_o": self.object.tau,
+                            "rho_camera_o": self.object.camera_pan,
+                            "tau_camera_o": self.object.camera_tilt,
                             "rho_dot_o": self.object.rho_rate,
                             "tau_dot_o": self.object.tau_rate,
                             "rho_c": self.camera.rho,

diff --git a/axis-ptz-controller/axis_ptz_utilities.py b/axis-ptz-controller/axis_ptz_utilities.py
@@ -12,7 +12,7 @@
 import quaternion
 
 # WGS84 parameters
-R_OPLUS = 6371008.7714 #6378137  # [m]
+R_OPLUS = 6378137 #6371008.7714 #6378137  # [m]
 F_INV = 298.257223563
 
 

diff --git a/axis-ptz-controller/object.py b/axis-ptz-controller/object.py
@@ -283,6 +283,24 @@ def recompute_location(self) -> None:
         self.rho = self.rho_lead
         self.tau = self.tau_lead
 
+        # Compute position of aircraft relative to tripod in ENz, then XYZ,
+        # then uvw coordinates
+        r_ENz_a_t = np.array(
+            [
+                math.sin(math.radians(self.rho)) * math.cos(math.radians(self.tau)),
+                math.cos(math.radians(self.rho)) * math.cos(math.radians(self.tau)),
+                math.sin(math.radians(self.tau)),
+            ]
+        )
+        r_XYZ_a_t = np.matmul(self.E_XYZ_to_ENz.transpose(), r_ENz_a_t)
+        r_uvw_a_t = np.matmul(self.E_XYZ_to_uvw, r_XYZ_a_t)
+
+        # Compute pan an tilt
+        self.camera_pan = math.degrees(math.atan2(r_uvw_a_t[0], r_uvw_a_t[1]))  # [deg]
+        self.camera_tilt = math.degrees(
+            math.atan2(r_uvw_a_t[2], axis_ptz_utilities.norm(r_uvw_a_t[0:2]))
+        )  # [deg]
+
         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)
@@ -303,8 +321,8 @@ def recompute_location(self) -> None:
             camera_yaw,
             camera_pitch,
             camera_roll,
-            self.rho_lead,
-            self.tau_lead,
+            self.rho,
+            self.tau,
         )
 
         # self.rst_point_now_relative_to_tripod = np.matmul(