SPATZ migration + proper directory structure

spatz/sensors/__init__.py

@@ -0,0 +1,6 @@
+from sensor import Sensor
+from compound import CompoundSensor
+
+from imu import Accelerometer
+from imu import Gyroscope
+from imu import IMU

spatz/sensors/compound.py

@@ -0,0 +1,22 @@
+import numpy as np
+
+from typing import List
+from numpy.typing import ArrayLike
+
+from sensor import Sensor
+from spatz.dataset import Dataset, List
+from spatz.logger import Logger
+from spatz.transforms import Transform
+
+
+class CompoundSensor(Sensor):
+    def __init__(self, dataset: Dataset, logger: Logger, sensors: List[Sensor], transforms: List[Transform] = []):
+        super().__init__(dataset, logger, transforms)
+
+        self.__sensors = sensors
+
+    def _get_data(self) -> ArrayLike:
+        x = np.stack([sensor() for sensor in self.__sensors])
+        x = self._sensor_specific_effects(x)
+
+        return x

spatz/sensors/imu/__init__.py

@@ -0,0 +1,2 @@
+from accelerometer import Accelerometer
+from gyroscope import Gyroscope

spatz/sensors/imu/accelerometer.py

@@ -0,0 +1,50 @@
+import numpy as np
+
+from typing import List
+from numpy.typing import ArrayLike
+
+from spatz.sensors import Sensor
+from spatz.transforms import Transform
+from spatz.dataset import Dataset
+from spatz.logger import Logger
+
+
+__all__=[
+    'Accelerometer'
+]
+
+
+# Local definition of gravitation
+g = 9.81
+
+
+class Accelerometer(Sensor):
+    def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
+        super().__init__(dataset, logger, transforms)
+
+        self._offset = np.array([offset, 0, 0])
+
+    def _get_data(self) -> ArrayLike | float:
+        acc = self._dataset.get_acceleration(frame='FL')
+        acc += np.array([0, 0, g])
+
+        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())
+
+        # Convert FL to body
+        acc = self._dataset.launch_rail_to_body() @ acc
+
+        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())
+
+        # Flip axes to sensor's perspective.
+        acc *= -1
+
+        # Add the effects of the imu's offset.
+        omega = self._dataset.get_angular_velocities()
+        acc += (np.cross(omega, self._offset) + np.cross(omega, np.cross(omega, self._offset)))
+
+        return acc
+    

spatz/sensors/imu/gyroscope.py

@@ -0,0 +1,20 @@
+from numpy.typing import ArrayLike
+from typing import List
+
+from spatz.sensors import Sensor
+from spatz.transforms import Transform
+from spatz.dataset import Dataset
+from spatz.logger import Logger
+
+
+class Gyroscope(Sensor):
+    def __init__(self, dataset: Dataset, 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_rotation_rates()
+
+        return x

spatz/sensors/imu/imu.py

@@ -0,0 +1,25 @@
+from typing import List
+from spatz.dataset import Dataset, List
+from spatz.logger import Logger
+from spatz.sensors import CompoundSensor, Accelerometer, Gyroscope
+from spatz.sensors.sensor import Sensor
+from spatz.transforms import Transform
+
+
+class IMU(CompoundSensor):
+    def __init__(self, 
+                 dataset: Dataset, 
+                 logger: Logger, 
+                 acc: Accelerometer, 
+                 gyro: Gyroscope,
+                 transforms: List[Transform] = []):
+        """_summary_
+
+        Args:
+            dataset (Dataset): _description_
+            logger (Logger): _description_
+            acc (Accelerometer): _description_
+            gyro (Gyroscope): _description_
+            transforms (List[Transform], optional): _description_. Defaults to [].
+        """
+        super().__init__(dataset, logger, [acc, gyro], transforms)

spatz/sensors/imu/wsen_isds.py

@@ -0,0 +1,54 @@
+import numpy as np
+
+from typing import AnyStr, List
+from numpy.typing import ArrayLike
+
+from spatz.sensors import Accelerometer, Gyroscope, IMU
+from spatz.transforms import Transform, GaussianNoise
+from spatz.dataset import Dataset
+from spatz.logger import Logger
+
+
+class WSEN_ISDS(IMU):
+    pass
+
+
+class WSEN_ISDS_ACC(Accelerometer):
+    def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
+        super().__init__(dataset, logger, offset, transforms)
+
+        self.__variance = 0.05
+        self.__noise = GaussianNoise(np.zeros(3), np.identity(3) * self.__variance)
+
+    def _get_name(self) -> AnyStr:
+        return 'WSEN_ISDS'
+
+    def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
+        t = self._dataset.get_time()
+
+        # Apply noise to the true values.
+        y = self.__noise(t, x)
+        noise = y - x
+
+        # Log the chosen noise values.
+        self._logger.write('acc_x_noise', noise[0], self._get_name())
+        self._logger.write('acc_y_noise', noise[1], self._get_name())
+        self._logger.write('acc_z_noise', noise[2], self._get_name())
+
+        return y
+    
+
+class WSEN_ISDS_GYRO(Gyroscope):
+    def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
+        super().__init__(dataset, logger, offset, transforms)
+
+    def _get_name(self) -> AnyStr:
+        return 'WSEN_ISDS'
+
+    def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
+        # Convert to degrees per second.
+        x = (x / np.pi) * 180
+
+        # TODO: Noise model.
+
+        return x

spatz/sensors/pressure/__init__.py

@@ -0,0 +1,2 @@
+from pressure import PressureSensor
+from ms5611_01ba03 import MS5611_01BA03

spatz/sensors/pressure/ms5611_01ba03.py

@@ -0,0 +1,33 @@
+from typing import List, AnyStr
+from numpy.typing import ArrayLike
+
+from pressure import PressureSensor
+
+from spatz.dataset import Dataset, Phase
+from spatz.logger import Logger
+from spatz.transforms import GaussianNoise, Transform
+
+
+class MS5611_01BA03(PressureSensor):
+    def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = []):
+        super().__init__(dataset, logger, transforms)
+
+        # Noise model obtained by a test flight using this sensor.
+        self.__pad_noise = GaussianNoise(0, 0.03)
+        self.__flight_noise = GaussianNoise(0, 1.5)
+
+    def _get_name(self) -> AnyStr:
+        return 'MS5611_01BA03'
+
+    def _sensor_specific_effects(self, x: ArrayLike | float) -> ArrayLike | float:
+        t = self._dataset.get_time()
+
+        # Transform from Pa to hPa
+        x /= 1e2
+
+        noisy = self.__pad_noise(t, x) if self._dataset.get_phase() == Phase.ONPAD else self.__flight_noise(t, x)
+
+        # Log the noise added to the pressure measurements.
+        self._logger.write('noise', noisy - x, domain=self._get_name())
+
+        return noisy

spatz/sensors/pressure/pressure.py

@@ -0,0 +1,46 @@
+import math
+import numpy as np
+
+from typing import List
+
+from spatz.sensors import Sensor
+from spatz.logger import Logger
+from spatz.dataset import Dataset
+from spatz.transforms import Transform
+
+
+class PressureSensor(Sensor):
+    def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = [], ts_effects=True):
+        """
+        Args:
+            dataset (Dataset): A dataset instance.
+            transforms (List[Transform], optional): Transforms to apply to the sensor outputs. Defaults to [].
+            ts_effects (bool, optional): If True, models transsonic effects. Defaults to True.
+        """
+        super(PressureSensor, self).__init__(dataset, logger, transforms)
+
+        self._ts_effects = ts_effects
+
+    def _get_data(self) -> float:
+        x = self._dataset.get_pressure()
+
+        if self._ts_effects:
+            # Pre-defined constants.
+            _p = 3e6
+            sigma = 40
+
+            # How far away from transsonic speed (mach 1) are we?
+            vvec = self._dataset.get_velocity()
+            dv = np.abs(np.linalg.norm(vvec) - self._dataset.get_speed_of_sound())
+
+            # Model transsonic effects by a peak at mach 1 which decays the further we are away from it.
+            ts_eff = _p * math.exp(-0.5* (dv / sigma)**2 ) / (sigma * math.sqrt(2*math.pi))
+
+            # Log the values for the transsonic effect.
+            self._logger.write('ts_effects', ts_eff, domain=self._get_name())
+            self._logger.write('mach_no', self._dataset.get_mach_number(), domain='mach')
+            self._logger.write('speedofsound', self._dataset.get_speed_of_sound(), domain='mach')
+
+            x = x + ts_eff
+
+        return x

spatz/sensors/sensor.py

@@ -0,0 +1,64 @@
+import math
+import numpy as np
+
+from abc import abstractmethod
+from typing import List, AnyStr
+from numpy.typing import ArrayLike
+
+from spatz.transforms import *
+from spatz.logger import *
+from spatz.dataset import *
+
+
+class Sensor:
+    def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = []):
+        self._dataset = dataset
+        self._logger = logger
+        self._transforms = transforms
+
+    def set_dataset(self, dataset: Dataset):
+        self._dataset = dataset
+
+    def set_logger(self, logger: Logger):
+        self._logger = logger
+
+    def _log(self, name: AnyStr, value: Any):
+        self._logger.write(name, value, self._get_name())
+
+    @abstractmethod
+    def _get_name(self) -> AnyStr:
+        raise NotImplementedError()
+
+    @abstractmethod
+    def _sensor_specific_effects(self, x: ArrayLike | float) -> ArrayLike | float:
+        raise NotImplementedError()
+
+    @abstractmethod
+    def _get_data(self) -> ArrayLike | float:
+        raise NotImplementedError()
+
+    def __call__(self) -> ArrayLike | float:
+        out = self._get_data()
+        out = self._sensor_specific_effects(out)
+
+        for transform in self._transforms:
+            out = transform(out)
+
+        # Log the outputs of the sensor.
+        if np.isscalar(out):
+            self._log('out', out)
+        else:
+            for i in range(len(out)):
+                self._log(f'out_{i}', out[i])
+
+        return out
+    
+
+class CompoundSensor(Sensor):
+    def __init__(self, sensors: List[Sensor]):
+        super(CompoundSensor, self).__init__(None)
+
+        self.__sensors = sensors
+
+    def _get_data(self) -> ArrayLike:
+        return np.stack([sensor() for sensor in self.__sensors])

spatz/sensors/temperature/__init__.py

@@ -0,0 +1 @@
+from temperature import TemperatureSensor

spatz/sensors/temperature/temperature.py

@@ -0,0 +1,13 @@
+from typing import List
+
+from spatz.sensors import Sensor
+from spatz.dataset import Dataset
+from spatz.transforms import Transform
+
+
+class TemperatureSensor(Sensor):
+    def __init__(self, dataset: Dataset, transforms: List[Transform] = []):
+        super(TemperatureSensor, self).__init__(dataset, transforms)
+
+    def _get_data(self) -> float:
+        return self._dataset.get_temperature()