Major rework using rocketpy

spatz/sensors/imu/__init__.py

@@ -1,3 +1,3 @@
-from spatz.sensors.imu.accelerometer import Accelerometer
+from spatz.sensors.imu.accelerometer import Accelerometer, CoordSystem
 from spatz.sensors.imu.gyroscope import Gyroscope
 from spatz.sensors.imu.imu import IMU

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

spatz/sensors/imu/bmi088.py

@@ -0,0 +1,46 @@
+import numpy as np
+
+from typing import List, AnyStr
+
+from numpy.typing import ArrayLike
+from spatz.dataset import ArrayLike, Dataset
+from spatz.logger import ArrayLike, Logger
+from spatz.sensors import IMU, Accelerometer, Gyroscope, CoordSystem
+from spatz.transforms import Transform, GaussianNoise
+
+
+class BMI088(IMU):
+    def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), offset=0, transforms: List[Transform] = []):
+        acc = BMI088Acc(dataset, logger, orientation, offset, transforms)
+        gyro = BMI088Gyro(dataset, logger, offset, transforms)
+
+        super().__init__(dataset, logger, acc, gyro, transforms)
+
+
+class BMI088Gyro(Gyroscope):
+    def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
+        super().__init__(dataset, logger, offset, transforms)
+
+    def _get_name(self) -> AnyStr:
+        return 'BMI088'
+
+    def _get_data(self) -> ArrayLike:
+        rots = self._dataset.fetch_values(['roll_l', 'pitch_l', 'yaw_l'])
+
+        return rots
+
+    def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
+        return x
+    
+
+class BMI088Acc(Accelerometer):
+    def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), offset: float = 0, transforms: List[Transform] = []):
+        super().__init__(dataset, logger, CoordSystem.RIGHT_HANDED, orientation, offset, transforms)
+
+        self.__noise = GaussianNoise(0, 0.05)
+
+    def _get_name(self) -> AnyStr:
+        return 'BMI088'
+    
+    def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
+        return self.__noise(0, x)

spatz/sensors/imu/gyroscope.py

@@ -4,17 +4,18 @@ from typing import List
 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
 
 
 class Gyroscope(Sensor):
-    def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
+    def __init__(self, dataset: DataSource, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
         super().__init__(dataset, logger, transforms)
 
         self._offset = offset
 
     def _get_data(self) -> ArrayLike | float:
         # Rotation in rad/sec
-        x = self._dataset.get_angular_velocities()
+        x = self._dataset.get_angular_velocity()
 
         return x

spatz/sensors/imu/h3lis100dl.py

@@ -0,0 +1,30 @@
+import numpy as np
+
+from typing import List, AnyStr
+from numpy.core.numeric import identity as identity
+from numpy.typing import ArrayLike, NDArray
+
+from spatz.dataset import Dataset
+from spatz.simulations.data_source import DataSource
+from spatz.logger import Logger
+from spatz.sensors import Accelerometer, CoordSystem
+from spatz.transforms import Transform
+
+
+class H3LIS100DL(Accelerometer):
+    def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), offset: float = 0, transforms: List[Transform] = []):        
+        super().__init__(dataset, logger, CoordSystem.RIGHT_HANDED, orientation, offset, transforms)
+
+    def _get_name(self) -> AnyStr:
+        return "H3LIS100DL"
+    
+    def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
+        # The sensor only measures values between -100g and +100g and in g/LSB.
+        g = 9.81
+        x = np.floor(np.clip(x, -100*g, +100*g) / g)
+
+        return x
+
+
+
+

spatz/sensors/imu/imu.py

@@ -12,7 +12,7 @@ class IMU(CompoundSensor):
                  dataset: Dataset, 
                  logger: Logger, 
                  acc: Accelerometer, 
-                 gyro: Gyroscope,
+                 gyro: Gyroscope,             
                  transforms: List[Transform] = []):
         """_summary_
 

spatz/sensors/imu/wsen_isds.py

@@ -3,8 +3,9 @@ import numpy as np
 from typing import AnyStr, List
 from numpy.typing import ArrayLike
 
-from spatz.sensors import Accelerometer, Gyroscope, IMU
+from spatz.sensors import Accelerometer, Gyroscope, IMU, CoordSystem
 from spatz.transforms import Transform, GaussianNoise
+from spatz.simulations.data_source import DataSource
 from spatz.dataset import Dataset
 from spatz.logger import Logger
 
@@ -14,8 +15,8 @@ class WSEN_ISDS(IMU):
 
 
 class WSEN_ISDS_ACC(Accelerometer):
-    def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
-        super().__init__(dataset, logger, offset, transforms)
+    def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), offset=0, transforms: List[Transform] = []):
+        super().__init__(dataset, logger, CoordSystem.LEFT_HANDED, orientation, offset, transforms)
 
         self.__variance = 0.05
         self.__noise = GaussianNoise(np.zeros(3), np.identity(3) * self.__variance)
@@ -35,11 +36,15 @@ class WSEN_ISDS_ACC(Accelerometer):
         self._logger.write('acc_y_noise', noise[1], self._get_name())
         self._logger.write('acc_z_noise', noise[2], self._get_name())
 
+        # The WSEN-ISDS accelerometer only measures acceleration between -16g and 16g. 
+        g = 9.81
+        y = np.clip(y, -16*g, +16*g)
+
         return y
     
 
 class WSEN_ISDS_GYRO(Gyroscope):
-    def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
+    def __init__(self, dataset: Dataset, logger: Logger, offset=0, transforms: List[Transform] = []):
         super().__init__(dataset, logger, offset, transforms)
 
     def _get_name(self) -> AnyStr:
@@ -51,4 +56,8 @@ class WSEN_ISDS_GYRO(Gyroscope):
 
         # TODO: Noise model.
 
+        self._log('ox', x[0])
+        self._log('oy', x[1])
+        self._log('oz', x[2])
+
         return x