Restructure IMU devices from Enum class to dict, populated during import using entry points

README.md

@@ -66,6 +66,25 @@ def main() -> None:
 if __name__ == "__main__":
     main()
 ```
+### Adding a new IMU/Changing the config of an existing IMU
+To use an IMU that is not defined in the built-in list of the module (defined IMUs are BNO055, BNO08X, and LSM6DSOX+LIS3MDL), follow these steps to register it to the program:
+1. Add the Adafruit library and driver for the IMU to your project dependencies.
+2. Define IMU Config / change the config using `replace` (See wiki page for more detail.) Store the new configs in `your_project.imu_configs.py`.
+3. Register the model in `pyproject.toml`:
+```
+[project.entry-points."imu_module.devices"]
+NEW_IMU = "your_project.imu_configs:NEW_IMU"
+```
+
+where `NEW_IMU` is the IMUConfig name and `your_project.imu_configs` is the file where the IMUConfig is defined.
+
+or override an existing built-in device:
+```
+[project.entry-points."imu_module.device_overrides"]
+BNO055 = "your_project.imu_configs:BNO055_CUSTOM"
+```
+
+4. Save edits. With UV, changes in the `pyproject.toml` are synchronized automatically upon `uv run`. With pip, poetry and conda, a manual module reinstallation may be necessary.
 
 ## Program Usage
 To run the main pipeline for all connected sensors with optional flag `-r` to record data:
@@ -98,11 +117,13 @@ make calibrate
 │       ├── __init__.py
 │       ├── __main__.py
 │       ├── base_classes.py
+│       ├── builtin_devices.py
 │       ├── definitions.py
 │       ├── devices.py
 │       ├── factory.py
 │       ├── i2c_bus.py
 │       ├── orientation_filters.py
+│       ├── registry.py
 │       ├── sensor_manager.py
 │       ├── utils.py
 │       └── wrapper.py

pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "imu_python"
-version = "0.1.0"
+version = "0.1.1"
 description = "IMU sensor codes in Python for the exosuit"
 readme = "README.md"
 authors = [
@@ -44,6 +44,12 @@ no_hw = [
   # For now this can be empty.
 ]
 
+[project.entry-points."imu_module.devices"]
+# register IMU configs
+
+[project.entry-points."imu_module.device_overrides"]
+# register IMU config overrides
+
 [project.urls]
 homepage = "https://github.com/TUM-Aries-Lab/imu-module"
 

src/imu_python/base_classes.py

@@ -125,6 +125,27 @@ def to_euler(self, seq: str) -> VectorXYZ:
         euler = rot.as_euler(seq=seq, degrees=False)
         return VectorXYZ.from_tuple(euler)
 
+    def rotate(self, rotation_matrix: NDArray) -> None:
+        """Rotate the quaternion using a 3x3 rotation matrix.
+
+        :param rotation_matrix: A 3x3 rotation matrix.
+        :return: None
+        """
+        if rotation_matrix.shape != (3, 3):
+            msg = f"Expected 3x3 rotation matrix, got {rotation_matrix.shape}"
+            logger.error(msg)
+            raise ValueError(msg)
+        matrix_form = Rot.from_quat(
+            quat=[self.x, self.y, self.z, self.w], scalar_first=False
+        )
+        rotation = Rot.from_matrix(rotation_matrix)
+        rotated = rotation * matrix_form
+        new_quat = Rot.as_quat(rotated, scalar_first=False)
+        self.x = new_quat[0]
+        self.y = new_quat[1]
+        self.z = new_quat[2]
+        self.w = new_quat[3]
+
 
 @dataclass(frozen=True)
 class IMUDeviceData:
@@ -168,6 +189,7 @@ class IMUConfig:
         accel_range_g: Accelerometer range in g.
         gyro_range_dps: Gyroscope range in degrees per second.
         filter_config: Configuration for the sensor fusion filter.
+        scalar_first: Flag for on-board fusion quaternion ordering (True if scalar is the first element, False if last).
 
     """
 
@@ -176,6 +198,7 @@ class IMUConfig:
     accel_range_g: float
     gyro_range_dps: float
     filter_config: FilterConfig = field(default_factory=FilterConfig)
+    scalar_first: bool = True
 
 
 @dataclass
@@ -285,7 +308,8 @@ def magnetic(self) -> tuple[float, float, float] | None:
         logger.debug("Magnetic data requested")
         if not self._is_connected:
             raise OSError(I2C_ERROR, "remote I/O error")
-        return None
+        x, y, z = np.random.normal(loc=0, scale=0.1, size=(3,))
+        return x, y, z
 
     def disconnect(self) -> None:
         """Simulate a hardware disconnection for testing purposes."""
@@ -301,9 +325,11 @@ class IMUSensorTypes(StrEnum):
         accel: Accelerometer sensor type.
         gyro: Gyroscope sensor type.
         mag: Magnetometer sensor type.
+        quat: Quaternion (on-board fusion).
 
     """
 
     accel = "acceleration"
     gyro = "gyro"
     mag = "magnetic"
+    quat = "quaternion"

src/imu_python/builtin_devices.py

@@ -0,0 +1,205 @@
+"""IMUConfig for known supported IMUs.
+
+Current configs: BNO055, BNO08x, LSM6DSOX+LIS3MDL
+"""
+
+from imu_python.base_classes import (
+    IMUConfig,
+    IMUParamNames,
+    IMUSensorTypes,
+    PreConfigStep,
+    PreConfigStepType,
+    SensorConfig,
+)
+from imu_python.definitions import MOCK_NAME, FilterConfig, IMUDeviceID
+
+BNO055 = IMUConfig(
+    devices={
+        IMUDeviceID.IMU0: SensorConfig(
+            name="BNO055",
+            addresses=[0x28, 0x29],
+            library="adafruit_bno055",  # module import path
+            module_class="BNO055_I2C",  # driver class inside the module
+            param_names=IMUParamNames(i2c="i2c", address="address"),
+            pre_config=[
+                # Switch to CONFIG mode
+                PreConfigStep(
+                    name="mode",
+                    args=("CONFIG_MODE",),
+                    step_type=PreConfigStepType.SET,
+                ),
+                # Wait for sensor to switch modes
+                PreConfigStep(
+                    name="time.sleep",
+                    args=(0.025,),
+                    step_type=PreConfigStepType.CALL,
+                ),
+                # Set sensor ranges and bandwidths
+                PreConfigStep(
+                    name="accel_range",
+                    args=("ACCEL_4G",),
+                    step_type=PreConfigStepType.SET,
+                ),
+                PreConfigStep(
+                    name="gyro_range",
+                    args=("GYRO_2000_DPS",),
+                    step_type=PreConfigStepType.SET,
+                ),
+                PreConfigStep(
+                    name="accel_bandwidth",
+                    args=("ACCEL_125HZ",),
+                    step_type=PreConfigStepType.SET,
+                ),
+                PreConfigStep(
+                    name="gyro_bandwidth",
+                    args=("GYRO_116HZ",),
+                    step_type=PreConfigStepType.SET,
+                ),
+                # Wait for settings to take effect
+                PreConfigStep(
+                    name="time.sleep",
+                    args=(0.025,),
+                    step_type=PreConfigStepType.CALL,
+                ),
+                # Switch to AMG mode where accel, gyro and mag are on
+                PreConfigStep(
+                    name="mode",
+                    args=("AMG_MODE",),
+                    step_type=PreConfigStepType.SET,
+                ),
+            ],
+        ),
+    },
+    roles={
+        IMUSensorTypes.accel: IMUDeviceID.IMU0,
+        IMUSensorTypes.gyro: IMUDeviceID.IMU0,
+        IMUSensorTypes.mag: IMUDeviceID.IMU0,
+    },
+    accel_range_g=4.0,
+    gyro_range_dps=2000.0,
+    filter_config=FilterConfig(freq_hz=100.0, gain=0.002250),
+    # Note: Gyro range setting does not actually work on the BNO055
+)
+
+LSM6DSOX_LIS3MDL = IMUConfig(
+    devices={
+        IMUDeviceID.IMU0: SensorConfig(
+            name="LSM6DSOX",
+            addresses=[0x6A, 0x6B],
+            library="adafruit_lsm6ds.lsm6dsox",
+            module_class="LSM6DSOX",
+            param_names=IMUParamNames(i2c="i2c_bus", address="address"),
+            constants_module="adafruit_lsm6ds",
+            pre_config=[
+                PreConfigStep(
+                    name="accelerometer_range",
+                    args=("AccelRange.RANGE_4G",),
+                    step_type=PreConfigStepType.SET,
+                ),
+                PreConfigStep(
+                    name="gyro_range",
+                    args=("GyroRange.RANGE_500_DPS",),
+                    step_type=PreConfigStepType.SET,
+                ),
+                PreConfigStep(
+                    name="accelerometer_data_rate",
+                    args=("Rate.RATE_416_HZ",),
+                    step_type=PreConfigStepType.SET,
+                ),
+                PreConfigStep(
+                    name="gyro_data_rate",
+                    args=("Rate.RATE_416_HZ",),
+                    step_type=PreConfigStepType.SET,
+                ),
+            ],
+        ),
+        IMUDeviceID.IMU1: SensorConfig(
+            name="LIS3MDL",
+            addresses=[0x1C, 0x1E],
+            library="adafruit_lis3mdl",
+            module_class="LIS3MDL",
+            param_names=IMUParamNames(i2c="i2c_bus", address="address"),
+            pre_config=[
+                PreConfigStep(
+                    name="range",
+                    args=("Range.RANGE_4_GAUSS",),
+                    step_type=PreConfigStepType.SET,
+                ),
+                PreConfigStep(
+                    name="data_rate",
+                    args=("Rate.RATE_40_HZ",),
+                    step_type=PreConfigStepType.SET,
+                ),
+            ],
+        ),
+    },
+    roles={
+        IMUSensorTypes.accel: IMUDeviceID.IMU0,
+        IMUSensorTypes.gyro: IMUDeviceID.IMU0,
+        IMUSensorTypes.mag: IMUDeviceID.IMU1,
+    },
+    accel_range_g=4.0,
+    gyro_range_dps=500.0,
+    filter_config=FilterConfig(freq_hz=104.0, gain=0.000573),
+)
+
+BNO08X = IMUConfig(
+    devices={
+        IMUDeviceID.IMU0: SensorConfig(
+            name="BNO08x",
+            addresses=[0x4A, 0x4B],
+            library="adafruit_bno08x.i2c",
+            module_class="BNO08X_I2C",
+            param_names=IMUParamNames(i2c="i2c_bus", address="address"),
+            constants_module="adafruit_bno08x",
+            pre_config=[
+                PreConfigStep(
+                    name="enable_feature",
+                    args=("BNO_REPORT_ACCELEROMETER", 10000),
+                    step_type=PreConfigStepType.CALL,
+                ),
+                PreConfigStep(
+                    name="enable_feature",
+                    args=("BNO_REPORT_GYROSCOPE", 10000),
+                    step_type=PreConfigStepType.CALL,
+                ),
+                PreConfigStep(
+                    name="enable_feature",
+                    args=("BNO_REPORT_MAGNETOMETER",),
+                    step_type=PreConfigStepType.CALL,
+                ),
+                PreConfigStep(
+                    name="enable_feature",
+                    args=("BNO_REPORT_ROTATION_VECTOR", 10000),
+                    step_type=PreConfigStepType.CALL,
+                ),
+            ],
+        ),
+    },
+    roles={
+        IMUSensorTypes.accel: IMUDeviceID.IMU0,
+        IMUSensorTypes.gyro: IMUDeviceID.IMU0,
+        IMUSensorTypes.mag: IMUDeviceID.IMU0,
+        IMUSensorTypes.quat: IMUDeviceID.IMU0,
+    },
+    accel_range_g=8.0,  # default 8g, not settable in driver
+    gyro_range_dps=2000.0,  # default 2000dps, not settable in driver
+)
+
+MOCK = IMUConfig(
+    devices={
+        IMUDeviceID.IMU0: SensorConfig(
+            name=MOCK_NAME,
+            addresses=[0x00, 0x01],  # fake I2C addresses for testing
+            library="imu_python.base_classes",  # module path (corrected)
+            module_class="AdafruitIMU",  # driver class
+            param_names=IMUParamNames(i2c="i2c", address="address"),
+        ),
+    },
+    roles={
+        IMUSensorTypes.accel: IMUDeviceID.IMU0,
+        IMUSensorTypes.gyro: IMUDeviceID.IMU0,
+    },
+    accel_range_g=8.0,
+    gyro_range_dps=2000.0,
+)

src/imu_python/definitions.py

@@ -86,6 +86,8 @@ def __iter__(self):
 
 I2C_ERROR = EREMOTEIO
 
+MOCK_NAME = "MOCK"
+
 
 @dataclass
 class IMUUpdateTime: