LH2 geometry

.gitignore

@@ -5,3 +5,4 @@
 .vscode/ipch
 __pycache__/
 *.pyc
+*geometrical_tests/ootx_data*

geometrical_tests/geometrical_consideration.py

@@ -0,0 +1,89 @@
+import sys
+import numpy as np
+
+sys.path.append("../")
+
+from bmc_decoder.bmc_decoder import SingleWord
+from polynomials.polynomial_finder import PolynomialIdentifier
+from timestamp_computing import TimestampComputing
+from lfsr.lfsr import LFSR
+from lh_geometry import LH2Geometry, Point
+from datetime import datetime
+
+"""
+words = [[0x006BD3, 0x820257], [0x002435, 0x8AA081]] #1600, 0
+words = [[0x007FFF, 0x99DCAE], [0x01107C, 0x9EC059]] #700, 0
+words = [[0x0035B6, 0x972C5B], [0x005ADA, 0x9FF167]] #1600, 800
+words = [[0x00D430, 0x550232], [0x00848A, 0x5DBB7D]] #1600, -800
+00 00 D4 - 30 00 55 02 32 00 00 84    E.@w....  0.U.2...
+8A 00 5D BB 7D
+"""
+clock_frequency = 96e6
+
+words = [[0x0035B6, 0x972C5B], [0x005ADA, 0x9FF167]]  # 1600, 800
+
+now = datetime.now()
+first_word = SingleWord(
+    waveform=[], start_timestamp=words[0][1] / clock_frequency, data=words[0][0]
+)
+second_word = SingleWord(
+    waveform=[], start_timestamp=words[1][1] / clock_frequency, data=words[1][0]
+)
+
+identifier = PolynomialIdentifier(first_word, second_word, first_two_polys=True)
+resulting_polys = identifier.search_polynomial()
+later = datetime.now()
+
+for i in resulting_polys:
+    print(i)
+
+print((later - now).total_seconds())
+if not hasattr(resulting_polys[0], "polynomial"):
+    exit()
+poly = resulting_polys[0].polynomial
+print(hex(poly))
+
+
+lfsr = LFSR(poly)
+
+first_iteration = lfsr.cpt_for(words[0][0])
+second_iteration = lfsr.cpt_for(words[1][0])
+
+print(f"iteration1 {first_iteration} iteration2 {second_iteration}")
+geometry = LH2Geometry()
+
+a_e = geometry.get_azimuth_elevation_from_iteration(first_iteration, second_iteration)
+
+print(a_e[0] * 180 / np.pi)
+print(a_e[1] * 180 / np.pi)
+
+lh2_coords = Point(x=365, y=0, z=323.5)
+sensor_coords = Point(x=1600, y=800, z=20)
+
+needed_azimuth = np.arccos(
+    np.sqrt(
+        ((lh2_coords.x - sensor_coords.x) ** 2)
+        / (
+            (lh2_coords.x - sensor_coords.x) ** 2
+            + (lh2_coords.y - sensor_coords.y) ** 2
+        )
+    )
+)
+needed_elevation = np.arccos(
+    np.sqrt(
+        (
+            (lh2_coords.x - sensor_coords.x) ** 2
+            + ((lh2_coords.y - sensor_coords.y) ** 2)
+        )
+        / (
+            (lh2_coords.x - sensor_coords.x) ** 2
+            + (lh2_coords.y - sensor_coords.y) ** 2
+            + (lh2_coords.z - sensor_coords.z) ** 2
+        )
+    )
+)
+
+print(f"needed_azimuth {needed_azimuth*180/np.pi}")
+print(f"needed_elevation {needed_elevation*180/np.pi}")
+
+print(229.02757038581862 - 90 - 26.68681060186392 + 6.333335383537537)

geometrical_tests/lh_geometry.py

@@ -0,0 +1,86 @@
+import numpy as np
+import sys
+
+sys.path.append("../")
+
+from lfsr.constants import periods, data_frequency
+
+
+class Point:
+    def __init__(self, x=0.0, y=0.0, z=0.0):
+        self.x = x
+        self.y = y
+        self.z = z
+
+
+class LH2Geometry:
+    def __init__(self):
+        self.rotor_frequency = float(48e6 / periods[0])
+        self.tilt_1 = (90 - 63.40388707940443) * np.pi / 180
+        self.tilt_2 = (180 - 146.47547713306804) * np.pi / 180
+
+    def get_theta_motor_from_lfsr_iteration(self, iteration: int) -> float:
+        return float(2 * np.pi * iteration * self.rotor_frequency / data_frequency)
+
+    def compute_azimuth_elevation(self, a1: float, a2: float):
+        Q = Point(x=np.cos(a1), y=np.sin(a1))
+        P = Point(x=np.cos(a2), y=np.sin(a2))
+
+        F = Point(
+            x=np.sin(self.tilt_2) * np.cos(a2 - np.pi / 2),
+            y=np.sin(self.tilt_2) * np.sin(a2 - np.pi / 2),
+            z=np.cos(self.tilt_2),
+        )
+        D = Point(
+            x=np.sin(self.tilt_1) * np.cos(a1 + np.pi / 2),
+            y=np.sin(self.tilt_1) * np.sin(a1 + np.pi / 2),
+            z=np.cos(self.tilt_1),
+        )
+
+        deno_sweep_1 = np.float32(F.x * P.y - P.x * F.y)
+        deno_sweep_2 = np.float32(D.x * Q.y - Q.x * D.y)
+
+        coeff_a = np.float32(P.y * F.z / deno_sweep_1)
+        coeff_b = np.float32(-P.x * F.z / deno_sweep_1)
+
+        coeff_d = np.float32(Q.y * D.z / deno_sweep_2)
+        coeff_e = np.float32(-Q.x * D.z / deno_sweep_2)
+
+        M = Point(
+            x=np.float32(
+                -(coeff_b - coeff_e) / (coeff_e * coeff_a - coeff_b * coeff_d)
+            ),
+            y=np.float32(
+                -(coeff_d - coeff_a) / (coeff_e * coeff_a - coeff_b * coeff_d)
+            ),
+            z=1,
+        )
+
+        negative_quadrants = a1 > a2
+
+        azimuth = np.arccos(np.sqrt((M.x**2) / (M.x**2 + M.y**2)))
+        azimuth = -azimuth if M.y < 0 else azimuth
+        azimuth = -azimuth if negative_quadrants else azimuth
+
+        elevation = np.arccos(
+            np.sqrt((M.x**2 + M.y**2) / (M.x**2 + M.y**2 + M.z**2))
+        )
+        elevation = -elevation if negative_quadrants else elevation
+
+        return (azimuth, elevation)
+
+    def get_azimuth_elevation_from_iteration(
+        self, first_iteration: int, second_iteration: int
+    ):
+        a1 = (
+            self.get_theta_motor_from_lfsr_iteration(first_iteration)
+            - np.pi / 2
+            - 26.68681060186392 * np.pi / 180
+        )
+        a2 = (
+            self.get_theta_motor_from_lfsr_iteration(second_iteration)
+            - np.pi / 2
+            - 26.68681060186392 * np.pi / 180
+            - 122.445 * np.pi / 180
+        )
+        return self.compute_azimuth_elevation(a1, a2)