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 index 009a2e75..4dfcfb4a 100644 --- 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 @@ class CalibrateIMUActivity(Activity): 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 index e5ef79a6..7f6f7be9 100644 --- a/internal_filesystem/lib/mpos/hardware/drivers/wsen_isds.py +++ b/internal_filesystem/lib/mpos/hardware/drivers/wsen_isds.py @@ -133,15 +133,9 @@ class Wsen_Isds: 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 @@ class Wsen_Isds: 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 @@ class Wsen_Isds: 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 index 6efb1f32..ccd8fdba 100644 --- a/internal_filesystem/lib/mpos/sensor_manager.py +++ b/internal_filesystem/lib/mpos/sensor_manager.py @@ -684,24 +684,26 @@ class _WsenISDSDriver(_IMUDriver): 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 @@ class _WsenISDSDriver(_IMUDriver): 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 @@ class _WsenISDSDriver(_IMUDriver): 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)