SensorManager: cleanup calibration

ThomasFarstrike · ThomasFarstrike · commit dadf4e8f4fb6 · 2025-12-08T11:04:00.000+01:00
diff --git a/internal_filesystem/builtin/apps/com.micropythonos.settings/assets/calibrate_imu.py b/internal_filesystem/builtin/apps/com.micropythonos.settings/assets/calibrate_imu.py
@@ -62,7 +62,7 @@ def onCreate(self):
         self.status_label.set_text("Initializing...")
         self.status_label.set_style_text_font(lv.font_montserrat_12, 0)
         self.status_label.set_long_mode(lv.label.LONG_MODE.WRAP)
-        self.status_label.set_width(lv.pct(90))
+        self.status_label.set_width(lv.pct(100))
 
         # Detail label (for additional info)
         self.detail_label = lv.label(screen)
diff --git a/internal_filesystem/lib/mpos/hardware/drivers/wsen_isds.py b/internal_filesystem/lib/mpos/hardware/drivers/wsen_isds.py
@@ -133,15 +133,9 @@ def __init__(self, i2c, address=0x6B, acc_range="2g", acc_data_rate="1.6Hz",
         self.i2c = i2c
         self.address = address
 
-        self.acc_offset_x = 0
-        self.acc_offset_y = 0
-        self.acc_offset_z = 0
         self.acc_range = 0
         self.acc_sensitivity = 0
 
-        self.gyro_offset_x = 0
-        self.gyro_offset_y = 0
-        self.gyro_offset_z = 0
         self.gyro_range = 0
         self.gyro_sensitivity = 0
 
@@ -261,20 +255,6 @@ def _acc_calc_sensitivity(self):
         else:
             print("Invalid range value:", self.acc_range)
 
-    def read_accelerations(self):
-        """Read calibrated accelerometer data.
-
-        Returns:
-            Tuple (x, y, z) in mg (milligrams)
-        """
-        raw_a_x, raw_a_y, raw_a_z = self._read_raw_accelerations()
-
-        a_x = (raw_a_x - self.acc_offset_x)
-        a_y = (raw_a_y - self.acc_offset_y)
-        a_z = (raw_a_z - self.acc_offset_z)
-
-        return a_x, a_y, a_z
-
     def _read_raw_accelerations(self):
         """Read raw accelerometer data."""
         if not self._acc_data_ready():
@@ -289,20 +269,6 @@ def _read_raw_accelerations(self):
         return raw_a_x * self.acc_sensitivity, raw_a_y * self.acc_sensitivity, raw_a_z * self.acc_sensitivity
 
 
-    def read_angular_velocities(self):
-        """Read calibrated gyroscope data.
-
-        Returns:
-            Tuple (x, y, z) in mdps (milli-degrees per second)
-        """
-        raw_g_x, raw_g_y, raw_g_z = self._read_raw_angular_velocities()
-
-        g_x = (raw_g_x - self.gyro_offset_x)
-        g_y = (raw_g_y - self.gyro_offset_y)
-        g_z = (raw_g_z - self.gyro_offset_z)
-
-        return g_x, g_y, g_z
-
     @property
     def temperature(self) -> float:
         temp_raw = self._read_raw_temperature()
diff --git a/internal_filesystem/lib/mpos/sensor_manager.py b/internal_filesystem/lib/mpos/sensor_manager.py
@@ -684,24 +684,26 @@ def __init__(self, i2c_bus, address):
         self.accel_offset = [0.0, 0.0, 0.0]
         self.gyro_offset = [0.0, 0.0, 0.0]
 
+
     def read_acceleration(self):
         """Read acceleration in m/s² (converts from mg)."""
-        ax, ay, az = self.sensor.read_accelerations()
-        # Convert mg to m/s²: mg → g → m/s²
+        ax, ay, az = self.sensor._read_raw_accelerations()
+        # Convert G to m/s² and apply calibration
         return (
-            (ax / 1000.0) * _GRAVITY,
-            (ay / 1000.0) * _GRAVITY,
-            (az / 1000.0) * _GRAVITY
+            ((ax / 1000) * _GRAVITY) - self.accel_offset[0],
+            ((ay / 1000) * _GRAVITY) - self.accel_offset[1],
+            ((az / 1000) * _GRAVITY) - self.accel_offset[2]
         )
 
+
     def read_gyroscope(self):
         """Read gyroscope in deg/s (converts from mdps)."""
-        gx, gy, gz = self.sensor.read_angular_velocities()
-        # Convert mdps to deg/s
+        gx, gy, gz = self.sensor._read_raw_angular_velocities()
+        # Convert mdps to deg/s and apply calibration
         return (
-            gx / 1000.0,
-            gy / 1000.0,
-            gz / 1000.0
+            gx / 1000.0 - self.gyro_offset[0],
+            gy / 1000.0 - self.gyro_offset[1],
+            gz / 1000.0 - self.gyro_offset[2]
         )
 
     def read_temperature(self):
@@ -722,13 +724,12 @@ def calibrate_accelerometer(self, samples):
         z_offset = 0
         if _mounted_position == FACING_EARTH:
             sum_z *= -1
-            z_offset = (1000 / samples) + _GRAVITY
         print(f"sumz: {sum_z}")
 
         # Average offsets (assuming Z-axis should read +9.8 m/s²)
         self.accel_offset[0] = sum_x / samples
         self.accel_offset[1] = sum_y / samples
-        self.accel_offset[2] = (sum_z / samples) - _GRAVITY - z_offset
+        self.accel_offset[2] = (sum_z / samples) - _GRAVITY
         print(f"offsets: {self.accel_offset}")
 
         return tuple(self.accel_offset)
@@ -739,9 +740,9 @@ def calibrate_gyroscope(self, samples):
 
         for _ in range(samples):
             gx, gy, gz = self.sensor._read_raw_angular_velocities()
-            sum_x += gx
-            sum_y += gy
-            sum_z += gz
+            sum_x += gx / 1000.0
+            sum_y += gy / 1000.0
+            sum_z += gz / 1000.0
             time.sleep_ms(10)
 
         # Average offsets (should be 0 when stationary)
