mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ.git
synced 2025-06-11 02:26:00 +00:00
102 lines
3.1 KiB
Python
102 lines
3.1 KiB
Python
import numpy as np
|
|
import math
|
|
|
|
from typing import Literal
|
|
from numpy.typing import NDArray
|
|
|
|
from spatz.simulations.csv_source import CSVSource
|
|
|
|
|
|
def T1(angle):
|
|
# return Rotation.from_euler('X', angle, degrees=False).as_matrix()
|
|
return np.array([
|
|
[1, 0, 0],
|
|
[0, math.cos(angle), math.sin(angle)],
|
|
[0, -math.sin(angle), math.cos(angle)],
|
|
])
|
|
|
|
|
|
def T2(angle):
|
|
# return Rotation.from_euler('Y', angle, degrees=False).as_matrix()
|
|
return np.array([
|
|
[math.cos(angle), 0, -math.sin(angle)],
|
|
[0, 1, 0],
|
|
[math.sin(angle), 0, math.cos(angle)]
|
|
])
|
|
|
|
|
|
def T3(angle):
|
|
# return Rotation.from_euler('Z', angle, degrees=False).as_matrix()
|
|
return np.array([
|
|
[math.cos(angle), math.sin(angle), 0],
|
|
[-math.sin(angle), math.cos(angle), 0],
|
|
[0, 0, 1]
|
|
])
|
|
|
|
|
|
class ASTOSSource(CSVSource):
|
|
def __init__(self, path: str, interpolation: Literal['linear'] = 'linear') -> None:
|
|
super().__init__(path, 'time', interpolation)
|
|
|
|
def get_position(self) -> NDArray:
|
|
return self.fetch_values(['x', 'y', 'z'])
|
|
|
|
def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
|
|
vel = self.fetch_values(['vx', 'vy', 'vz'])
|
|
|
|
if frame == 'local':
|
|
return self.global_to_local() @ vel
|
|
|
|
return vel
|
|
|
|
def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
|
|
acc = self.fetch_values(['ax', 'ay', 'az'])
|
|
|
|
if frame == 'local':
|
|
return self.global_to_local() @ acc
|
|
|
|
return acc
|
|
|
|
def get_attitude(self) -> NDArray:
|
|
raise NotImplementedError()
|
|
|
|
def local_to_global(self) -> NDArray:
|
|
return self.global_to_local().T
|
|
|
|
def global_to_local(self) -> NDArray:
|
|
# ASTOS local to body
|
|
rots = self.fetch_values(['pitch_l', 'yaw_l', 'roll_l'])
|
|
pitch_l, yaw_l, roll_l = rots[0], rots[1], rots[2]
|
|
local_to_body = T1(roll_l) @ T2(pitch_l - math.pi/2) @ T1(-yaw_l)
|
|
|
|
# ASTOS global to launch rail.
|
|
init_long = self.fetch_init_value('longitude')
|
|
init_lat = self.fetch_init_value('latitude')
|
|
global_to_launch_rail = T2(-math.pi/2 - init_lat) @ T3(init_long)
|
|
|
|
# ASTOS global to local.
|
|
decl = self.fetch_value('declination')
|
|
long = self.fetch_value('longitude')
|
|
t0 = self.get_start_time()
|
|
omega_E = (2*math.pi) / (24*60*60)
|
|
global_to_local = T2(-decl) @ T3(long + omega_E * t0)
|
|
|
|
# ASTOS local to launch rail inverted
|
|
local_to_launch_rail_inv = global_to_launch_rail @ global_to_local
|
|
|
|
return local_to_body @ local_to_launch_rail_inv
|
|
|
|
def get_angular_velocity(self) -> NDArray:
|
|
return self.fetch_values(['OMEGA_X', 'OMEGA_Y', 'OMEGA_Z'])
|
|
|
|
def get_static_pressure(self) -> float:
|
|
return self.fetch_value('pressure')
|
|
|
|
def get_longitude(self) -> float:
|
|
return self.fetch_value('longitude')
|
|
|
|
def get_latitude(self) -> float:
|
|
return self.fetch_value('latitude')
|
|
|
|
def get_altitude(self) -> float:
|
|
return self.fetch_value('altitude') |