mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ.git
synced 2025-06-10 01:55:59 +00:00
81 lines
2.8 KiB
Python
81 lines
2.8 KiB
Python
import numpy as np
|
|
|
|
from typing import List
|
|
from enum import Enum
|
|
from numpy.typing import ArrayLike, NDArray
|
|
|
|
from spatz.sensors import Sensor
|
|
from spatz.transforms import Transform
|
|
from spatz.dataset import Dataset
|
|
from spatz.simulations.data_source import DataSource
|
|
from spatz.logger import Logger
|
|
|
|
|
|
__all__=[
|
|
'Accelerometer'
|
|
]
|
|
|
|
|
|
# Local definition of gravitation
|
|
g = 9.81
|
|
|
|
|
|
class CoordSystem:
|
|
LEFT_HANDED = 0,
|
|
RIGHT_HANDED = 0,
|
|
|
|
|
|
class Accelerometer(Sensor):
|
|
def __init__(
|
|
self, dataset: DataSource,
|
|
logger: Logger,
|
|
coord_system: CoordSystem = CoordSystem.RIGHT_HANDED,
|
|
orientation: NDArray = np.identity(3),
|
|
offset: float = 0,
|
|
transforms: List[Transform] = []):
|
|
"""_summary_
|
|
|
|
Args:
|
|
dataset (Dataset): A dataset object to fetch data from.
|
|
logger (Logger): A logger object to write data to.
|
|
coord_system (CoordSystem, optional): The type of coordinate system to use. Defaults to CoordSystem.RIGHT_HANDED.
|
|
orientation (NDArray, optional): The orientation of the sensor inside the spacecraft. Defaults to np.identity(3).
|
|
offset (float, optional): The offset of the sensor from the origin around which the object rotates. Defaults to 0.
|
|
transforms (List[Transform], optional): A list of transformations applied to the sensor measurements. Defaults to [].
|
|
"""
|
|
super().__init__(dataset, logger, transforms)
|
|
|
|
assert orientation.shape == (3, 3), 'Orientation has to be a 3x3 matrix.'
|
|
|
|
self._offset = np.array([offset, 0, 0])
|
|
self._coord_system = coord_system
|
|
self._orientation = orientation
|
|
|
|
def _get_data(self) -> ArrayLike | float:
|
|
acc = self._dataset.get_acceleration('global')
|
|
|
|
# self._logger.write('global_ax', acc[0], self._get_name())
|
|
# self._logger.write('global_ay', acc[1], self._get_name())
|
|
# self._logger.write('global_az', acc[2], self._get_name())
|
|
|
|
# Convert FL to body
|
|
acc = self._dataset.global_to_local() @ acc
|
|
# acc += mat @ np.array([0, 0, g])
|
|
|
|
# self._logger.write('local_x', acc[0], self._get_name())
|
|
# self._logger.write('local_y', acc[1], self._get_name())
|
|
# self._logger.write('local_z', acc[2], self._get_name())
|
|
|
|
# Flip axes to sensor's perspective. Spatz uses a right-handed coordinate system.
|
|
#if self._coord_system == CoordSystem.LEFT_HANDED:
|
|
# acc[1] *= -1
|
|
|
|
# Rotate the acceleration vector to accomodate the accelerometer's orientation on the rocket.
|
|
acc = self._orientation @ acc
|
|
|
|
# Add the effects of the imu's offset.
|
|
omega = self._dataset.get_angular_velocity()
|
|
acc += (np.cross(omega, self._offset) + np.cross(omega, np.cross(omega, self._offset)))
|
|
|
|
return acc
|
|
|