Changes to coordinate sysem

spatz/sensors/antenna/tx_gain.py

@@ -69,13 +69,17 @@ class GainPattern():
             return angles,gain
 
     def get_theta_cut(self, theta:float) -> ArrayLike: #Return farfield cut with theta = const (looking from the top)
-            assert 0<= theta < 360
+            assert 0<= theta < 180
             sub_df_left = self._df.loc[self._df["Theta"] == theta]
-            angles = sub_df_left["Phi"]
-            gain = sub_df_left[GAIN_NAME]
-            sub_df_right = self._df.loc[self._df["Theta"] == ((theta + 180) % 360)]
-            angles = sub_df_right["Phi"]
-            gain = sub_df_right[GAIN_NAME]
+            angles_l = sub_df_left["Phi"]
+            gain_l = sub_df_left[GAIN_NAME]
+
+            sub_df_right = self._df.loc[self._df["Theta"] == (360-theta)]
+            angles_r = sub_df_right["Phi"]+180
+            gain_r = sub_df_right[GAIN_NAME]
+
+            angles = pd.concat([angles_l,angles_r])
+            gain = pd.concat([gain_l,gain_r])
 
             return angles,gain
 
@@ -126,28 +130,55 @@ class AntennaTxGain(Sensor):
         self._pattern = GainPattern(gain_pattern_path,1)
 
     def _get_data(self) -> ArrayLike | float:
+        magic_matrix = np.array([
+             [0,1,0],
+             [1,0,0],
+             [0,0,-1]
+        ]) 
+        
         # Get current position of rocket in FL Frame (Launcher Frame).
         pos_fl = self._dataset.fetch_values(['x', 'y', 'z']) #X,Y,Z is in FL (Launcher frame) -> Z is up, X is east
-
-        # Transform X,Y,Z to B Frame (Body Frame)
-        pos_b = np.array(pos_fl) @ self._dataset.launch_rail_to_body()
+        
+        gs_offset_fl = np.array([-1810,-1500,100]) #Radar hill is approx 1.81km west. 1.5km south, 100higher
+        rocket_to_gs_fl = pos_fl-gs_offset_fl
+        rocket_to_gs_fl_n = rocket_to_gs_fl/np.linalg.norm(rocket_to_gs_fl)
 
         # Rocket in body frame is simply [1,0,0]^T by definition
-        rocket_b = np.array([1,0,0]).T 
+        rocket_b = np.array([1,0,0])
+        rocket_fl = magic_matrix @ np.linalg.inv(self._dataset.launch_rail_to_body()) @ rocket_b
+        rocket_fl_n = rocket_fl / np.linalg.norm(rocket_fl)
+
+        
 
         # Angle between rocket and pos returns elevation angle (Phi). Assume a rotation of 0° for now to get theta
-        theta = np.rad2deg(np.arccos(np.clip(np.dot(pos_b/np.linalg.norm(pos_b), rocket_b),-1.0,1.0))) #Clip trick from: https://stackoverflow.com/questions/2827393/angles-between-two-n-dimensional-vectors-in-python
-        #return phi
-        phi = 0
+        theta = 180-np.rad2deg(np.arccos(np.clip(np.dot(rocket_to_gs_fl_n,rocket_fl_n),-1.0,1.0))) #Clip trick from: https://stackoverflow.com/questions/2827393/angles-between-two-n-dimensional-vectors-in-python
         
+        self._log("rocket_x",rocket_fl_n[0])
+        self._log("rocket_y",rocket_fl_n[1])
+        self._log("rocket_z",rocket_fl_n[2])
+        self._log("pos_x",rocket_to_gs_fl_n[0])
+        self._log("pos_y",rocket_to_gs_fl_n[1])
+        self._log("pos_z",rocket_to_gs_fl_n[2])
+        self._log("theta",theta)
+        #return phi
+        
+        #Get Theta cut for this angle
+        #angles, gains = self._pattern.get_theta_cut(np.round(theta))
+
+        #min_gain = np.min(gains)
+        #min_ix = np.argmin(gains)
+        #min_angle = angles[min_ix]
+        #self._log("works_case_angle",min_angle)  
+
+
+        min_gain = self._pattern.get_gain(45,theta)
         
         # Fetch gain in this direction
-        return self._pattern.get_gain(phi,theta)
+        return min_gain
 
     def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
         return x
     
-
     def _get_name(self) -> AnyStr:
         return 'antenna/tx_gain'
         
@@ -157,4 +188,5 @@ if  __name__ == '__main__':
     print(pattern.get_gain(0,12))
     print(pattern.get_gain(0,16))
     print(pattern.get_gain(6,12))
-    print(pattern.get_gain(0,10))
+    print(pattern.get_gain(0,10))
+    print(pattern.get_theta_cut(90))