Detumble

.gitignore

@@ -3,6 +3,7 @@ __pycache__/
 .coverage*
 .coverage-reports/
 .DS_Store
+.hypothesis
 .venv
 artifacts/
 coverage-reports/

docs/design-guide.md

@@ -120,7 +120,7 @@ The following table lists possible sensor properties, their corresponding types
 | duty_cycle       | int                  | 16-bit PWM duty cycle                        |
 | eCO2             | float                | equivalent/estimated CO₂ in ppm              |
 | frequency        | int                  | Hertz (Hz)                                   |
-| gyro             | (float, float, float)| x, y, z radians per second                   |
+| angular velocity | (float, float, float)| x, y, z radians per second                   |
 | light            | float                | non-unit-specific light levels               |
 | lux              | float                | SI lux                                       |
 | magnetic         | (float, float, float)| x, y, z micro-Tesla (uT)                     |

mocks/adafruit_lis2mdl/lis2mdl.py

@@ -5,6 +5,8 @@
 need for actual hardware.
 """
 
+from pysquared.sensor_reading.magnetic import Magnetic
+
 
 class LIS2MDL:
     """A mock LIS2MDL magnetometer."""
@@ -17,4 +19,7 @@ def __init__(self, i2c) -> None:
         """
         self.i2c = i2c
 
-    magnetic: tuple[float, float, float] = (0.0, 0.0, 0.0)
+    @property
+    async def async_magnetic(self):
+        """Asynchronously returns a mock magnetic field vector."""
+        return Magnetic(0.0, 0.0, 0.0)

mocks/adafruit_lsm6ds/lsm6dsox.py

@@ -21,5 +21,5 @@ def __init__(self, i2c_bus: I2C, address: int) -> None:
         ...
 
     acceleration: tuple[float, float, float] = (0.0, 0.0, 0.0)
-    gyro: tuple[float, float, float] = (0.0, 0.0, 0.0)
+    angular_velocity: tuple[float, float, float] = (0.0, 0.0, 0.0)
     temperature: float = 0.0

pyproject.toml

@@ -31,6 +31,7 @@ dev = [
     "pre-commit==4.2.0",
     "pyright[nodejs]==1.1.402",
     "pytest==8.4.1",
+    "hypothesis==6.136.7",
 ]
 docs = [
     "mkdocs-material==9.6.14",

pysquared/attitude_control/b_dot_detumble.py

@@ -0,0 +1,134 @@
+"""This file provides functions for detumbling the satellite using the B-dot algorithm.
+
+Coding style for this file foregoes more complex programming constructs in favor readability.
+We assume that non-programmers may read this code to understand and validate detumbling logic.
+
+Units and concepts used in this file:
+- B-dot detumbling algorithm
+    - https://en.wikipedia.org/wiki/Spacecraft_detumbling#Magnetic_control:_B-dot
+- Magnetic field (B-field) strength in micro-Tesla (uT)
+    - https://en.wikipedia.org/wiki/Magnetic_field
+    - https://en.wikipedia.org/wiki/Tesla_(unit)
+- Dipole moment in Ampere-square meter (A⋅m²)
+    - https://en.wikipedia.org/wiki/Magnetic_dipole
+    - https://en.wikipedia.org/wiki/Ampere
+    - https://en.wikipedia.org/wiki/Square_metre
+"""
+
+import math
+
+from ..sensor_reading.magnetic import Magnetic
+
+
+class BDotDetumble:
+    """B-dot detumbling for attitude control.
+
+    Example usage:
+    ```python
+        b_dot_detumble = BDotDetumble(gain=1.0)
+        current_mag_field = Magnetic(value=(0.1, 0.2, 0.3), timestamp=1234567890)
+        previous_mag_field = Magnetic(value=(0.1, 0.2, 0.3), timestamp=1234567880)
+        dipole_moment = b_dot_detumble.dipole_moment(current_mag_field, previous_mag_field)
+        print(dipole_moment)
+    ```
+    """
+
+    def __init__(self, gain: float = 1.0):
+        """Initializes the BDotDetumble class.
+
+        Args:
+            gain: Gain constant for the B-dot detumbling algorithm.
+
+        TODO(nateinaction): Create system for teams to set values that compute gain for them.
+        """
+        self._gain = gain
+
+    @staticmethod
+    def _magnitude(current_mag_field: Magnetic) -> float:
+        """
+        Computes the magnitude of the magnetic field vector.
+
+        Args:
+            current_mag_field: Magnetic object containing the current magnetic field vector.
+
+        Returns:
+            The magnitude of the magnetic field vector.
+        """
+        return math.sqrt(
+            current_mag_field.value[0] ** 2
+            + current_mag_field.value[1] ** 2
+            + current_mag_field.value[2] ** 2
+        )
+
+    @staticmethod
+    def _dB_dt(
+        current_mag_field: Magnetic, previous_mag_field: Magnetic
+    ) -> tuple[float, float, float]:
+        """
+        Computes the time derivative of the magnetic field vector.
+
+        Args:
+            current_mag_field: Magnetic object containing the current magnetic field vector
+            previous_mag_field: Magnetic object containing the previous magnetic field vector
+
+        Returns:
+            dB_dt: tuple of dB/dt (dBx/dt, dBy/dt, dBz/dt) in micro-Tesla per second (uT/s).
+
+        Raises:
+            ValueError: If the time difference between the current and previous magnetic field readings is too small to compute dB/dt.
+        """
+        dt = current_mag_field.timestamp - previous_mag_field.timestamp
+        if dt < 1e-6:
+            raise ValueError(
+                "Timestamp difference between current and previous magnetic field readings is too small to compute dB/dt."
+            )
+
+        dBx_dt = (current_mag_field.value[0] - previous_mag_field.value[0]) / dt
+        dBy_dt = (current_mag_field.value[1] - previous_mag_field.value[1]) / dt
+        dBz_dt = (current_mag_field.value[2] - previous_mag_field.value[2]) / dt
+        return (dBx_dt, dBy_dt, dBz_dt)
+
+    def dipole_moment(
+        self, current_mag_field: Magnetic, previous_mag_field: Magnetic
+    ) -> tuple[float, float, float]:
+        """
+        Computes the required dipole moment to detumble the satellite.
+
+        m = -k * (dB/dt) / |B|
+
+        m is the dipole moment in A⋅m²
+        k is a gain constant
+        dB/dt is the time derivative of the magnetic field reading in micro-Tesla per second (uT/s)
+        |B| is the magnitude of the magnetic field vector in micro-Tesla (uT)
+
+        Args:
+            current_mag_field: Magnetic object containing the current magnetic field vector.
+            previous_mag_field: Magnetic object containing the previous magnetic field vector.
+
+        Returns:
+            The dipole moment in A⋅m² as a tuple (moment_x, moment_y, moment_z).
+
+        Raises:
+            ValueError: If the magnitude of the current magnetic field is too small to compute the dipole moment.
+                or if the time difference between the current and previous magnetic field readings is less than or equal to 0.
+        """
+        magnitude = self._magnitude(current_mag_field)
+        if magnitude < 1e-6:
+            raise ValueError(
+                "Current magnetic field magnitude is too small to compute dipole moment."
+            )
+
+        if current_mag_field.timestamp <= previous_mag_field.timestamp:
+            raise ValueError(
+                "Current magnetic field timestamp must be greater than previous magnetic field timestamp."
+            )
+
+        try:
+            dBx_dt, dBy_dt, dBz_dt = self._dB_dt(current_mag_field, previous_mag_field)
+        except ValueError:
+            raise
+
+        moment_x = -self._gain * dBx_dt / magnitude
+        moment_y = -self._gain * dBy_dt / magnitude
+        moment_z = -self._gain * dBz_dt / magnitude
+        return (moment_x, moment_y, moment_z)

pysquared/attitude_control/detumble_service.py

@@ -0,0 +1,80 @@
+"""Detumble attitude control service using B-dot algorithm with magnetorquers."""
+
+import asyncio
+import time
+
+from ..logger import Logger
+from ..protos.magnetometer import MagnetometerProto
+from ..protos.magnetorquer import MagnetorquerProto
+from ..sensor_reading.magnetic import Magnetic
+from .b_dot_detumble import BDotDetumble
+
+
+class DetumbleService:
+    """Attitude control service implementing B-dot detumble algorithm."""
+
+    previous_mag_field: Magnetic | None = None
+
+    def __init__(
+        self,
+        logger: Logger,
+        magnetometer: MagnetometerProto,
+        magnetorquer: MagnetorquerProto,
+        control_period: float = 1.0,
+    ) -> None:
+        """Initialize the detumble service.
+
+        Args:
+            logger: Logger instance
+            magnetometer: Magnetometer sensor interface
+            magnetorquer: Magnetorquer control interface
+            control_period: Control loop period in seconds
+        """
+        self._logger = logger
+        self._magnetometer = magnetometer
+        self._magnetorquer = magnetorquer
+        self._control_period = control_period
+
+    def execute_control_step(self) -> None:
+        """Execute one step of the detumble control algorithm."""
+        # Get sensor readings
+        try:
+            magnetic_field = self._magnetometer.get_magnetic_field()
+        except Exception:
+            raise
+
+        if self.previous_mag_field is None:
+            self.previous_mag_field = magnetic_field
+            return
+
+        # Calculate required dipole moment using B-dot algorithm
+        try:
+            dipole_moment = BDotDetumble().dipole_moment(
+                current_mag_field=magnetic_field,
+                previous_mag_field=self.previous_mag_field,
+            )
+        except Exception:
+            raise
+        finally:
+            self.previous_mag_field = magnetic_field
+
+        # Apply dipole moment to magnetorquers
+        self._magnetorquer.set_dipole_moment(dipole_moment)
+
+    async def run_detumble_loop(self, max_iterations: int = 1000) -> None:
+        """Run the detumble control loop for a specified number of iterations.
+
+        Args:
+            max_iterations: Maximum number of control iterations
+        """
+        for i in range(max_iterations):
+            start_time = time.monotonic()
+
+            success = self.execute_control_step()
+            if not success:
+                self._logger.warning(f"Control step {i} failed, continuing...")
+
+            # Maintain control period timing
+            elapsed = time.monotonic() - start_time
+            if elapsed < self._control_period:
+                await asyncio.sleep(self._control_period - elapsed)