ALPAKA/SPATZ
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ.git synced 2025-09-29 05:17:33 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Changes to coordinate sysem

Browse Source
This commit is contained in:
bareissvincent
2024-04-06 18:15:14 +02:00
parent 83bdce162a
commit 66557802a3
4 changed files with 65519 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
spatz/sensors/antenna/tx_gain.py
View File

@@ -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))