Major rework using rocketpy

spatz/sensors/imu/accelerometer.py

@@ -1,11 +1,13 @@
 import numpy as np
 
 from typing import List
-from numpy.typing import ArrayLike
+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
 
 
@@ -18,33 +20,61 @@ __all__=[
 g = 9.81
 
 
+class CoordSystem:
+    LEFT_HANDED = 0,
+    RIGHT_HANDED = 0,
+
+
 class Accelerometer(Sensor):
-    def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
+    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(frame='FL')
+        acc = self._dataset.get_acceleration('global')
 
-        self._logger.write('FL_x', acc[0], self._get_name())
-        self._logger.write('FL_y', acc[1], self._get_name())
-        self._logger.write('FL_z', acc[2], self._get_name())
+        # 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
-        mat = self._dataset.launch_rail_to_body()
-        acc = mat @ acc
-        acc += mat @ np.array([0, 0, g])
+        acc = self._dataset.global_to_local() @ acc
+        # acc += mat @ np.array([0, 0, g])
 
-        self._logger.write('B_x', acc[0], self._get_name())
-        self._logger.write('B_y', acc[1], self._get_name())
-        self._logger.write('B_z', acc[2], self._get_name())
+        # 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.
-        acc *= -1
+        # 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_velocities()
+        omega = self._dataset.get_angular_velocity()
         acc += (np.cross(omega, self._offset) + np.cross(omega, np.cross(omega, self._offset)))
 
         return acc