Merge pull request #60 from pavelmachek/m_4_magn

ThomasFarstrike · web-flow · commit d3e90c5bb40e · 2026-02-27T12:18:59.000+01:00
magn: Start magnetometer support
diff --git a/internal_filesystem/lib/mpos/imu/drivers/base.py b/internal_filesystem/lib/mpos/imu/drivers/base.py
@@ -18,6 +18,10 @@ def read_gyroscope(self):
         """Returns (x, y, z) in deg/s"""
         raise NotImplementedError
 
+    def read_magnetometer(self):
+        """Returns (x, y, z) in uT"""
+        raise NotImplementedError
+
     def read_temperature(self):
         """Returns temperature in &deg;C"""
         raise NotImplementedError
diff --git a/internal_filesystem/lib/mpos/imu/drivers/iio.py b/internal_filesystem/lib/mpos/imu/drivers/iio.py
@@ -12,11 +12,12 @@ class IIODriver(IMUDriverBase):
     """
 
     accel_path: str
+    mag_path: str
 
     def __init__(self):
         super().__init__()
         self.accel_path = self.find_iio_device_with_file("in_accel_x_raw")
-        print("path:", self.accel_path)
+        self.mag_path = self.find_iio_device_with_file("in_magn_x_raw")
 
     def _p(self, name: str):
         return self.accel_path + "/" + name
@@ -138,3 +139,10 @@ def read_gyroscope(self):
             gy - self.gyro_offset[1],
             gz - self.gyro_offset[2],
         )
+
+    def read_magnetometer(self) -> tuple[float, float, float]:
+        gx = self._read_raw_scaled(self.mag_path + "/" + "in_magn_x_raw", self.mag_path + "/" + "in_magn_x_scale")
+        gy = self._read_raw_scaled(self.mag_path + "/" + "in_magn_y_raw", self.mag_path + "/" + "in_magn_y_scale")
+        gz = self._read_raw_scaled(self.mag_path + "/" + "in_magn_z_raw", self.mag_path + "/" + "in_magn_z_scale")        
+
+        return (gx, gy, gz)
diff --git a/internal_filesystem/lib/mpos/imu/manager.py b/internal_filesystem/lib/mpos/imu/manager.py
@@ -3,6 +3,7 @@
 from mpos.imu.constants import (
     TYPE_ACCELEROMETER,
     TYPE_GYROSCOPE,
+    TYPE_MAGNETIC_FIELD,
     TYPE_IMU_TEMPERATURE,
     TYPE_SOC_TEMPERATURE,
     TYPE_TEMPERATURE,
@@ -50,6 +51,15 @@ def init(self, i2c_bus, address=0x6B, mounted_position=FACING_SKY):
     def init_iio(self):
         self._imu_driver = IIODriver()
         self._sensor_list = [
+            Sensor(
+                name="Magnetometer",
+                sensor_type=TYPE_MAGNETIC_FIELD,
+                vendor="Linux IIO",
+                version=1,
+                max_range="?",
+                resolution="?",
+                power_ma=0.2
+            ),
             Sensor(
                 name="Accelerometer",
                 sensor_type=TYPE_ACCELEROMETER,
@@ -156,6 +166,9 @@ def read_sensor_once(self, sensor):
         elif sensor.type == TYPE_GYROSCOPE:
             if self._imu_driver:
                 return self._imu_driver.read_gyroscope()
+        elif sensor.type == TYPE_MAGNETIC_FIELD:
+            if self._imu_driver:
+                return self._imu_driver.read_magnetometer()
         elif sensor.type == TYPE_IMU_TEMPERATURE:
             if self._imu_driver:
                 return self._imu_driver.read_temperature()
