m5/MicroPythonOS
0
0
Fork 0
mirror of https://github.com/m5stack/MicroPythonOS.git synced 2026-05-20 11:51:27 -07:00
Code Issues Packages Projects Releases Wiki Activity

Fix WSEN-ISDS calibration

Browse Source
This commit is contained in:
Thomas Farstrike
2025-12-08 09:56:54 +01:00
parent 8b6883880a
commit 141fc20836
2 changed files with 56 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
internal_filesystem/lib/mpos/hardware/drivers/wsen_isds.py
+8 -8
View File
@@ -299,9 +299,9 @@ class Wsen_Isds:
"""
raw_a_x, raw_a_y, raw_a_z = self._read_raw_accelerations()
a_x = (raw_a_x - self.acc_offset_x) * self.acc_sensitivity
a_y = (raw_a_y - self.acc_offset_y) * self.acc_sensitivity
a_z = (raw_a_z - self.acc_offset_z) * self.acc_sensitivity
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
@@ -316,7 +316,7 @@ class Wsen_Isds:
raw_a_y = self._convert_from_raw(raw[2], raw[3])
raw_a_z = self._convert_from_raw(raw[4], raw[5])
return raw_a_x, raw_a_y, raw_a_z
return raw_a_x * self.acc_sensitivity, raw_a_y * self.acc_sensitivity, raw_a_z * self.acc_sensitivity
def gyro_calibrate(self, samples=None):
"""Calibrate gyroscope by averaging samples while device is stationary.
@@ -350,9 +350,9 @@ class Wsen_Isds:
"""
raw_g_x, raw_g_y, raw_g_z = self._read_raw_angular_velocities()
g_x = (raw_g_x - self.gyro_offset_x) * self.gyro_sensitivity
g_y = (raw_g_y - self.gyro_offset_y) * self.gyro_sensitivity
g_z = (raw_g_z - self.gyro_offset_z) * self.gyro_sensitivity
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
@@ -381,7 +381,7 @@ class Wsen_Isds:
raw_g_y = self._convert_from_raw(raw[2], raw[3])
raw_g_z = self._convert_from_raw(raw[4], raw[5])
return raw_g_x, raw_g_y, raw_g_z
return raw_g_x * self.gyro_sensitivity, raw_g_y * self.gyro_sensitivity, raw_g_z * self.gyro_sensitivity
def read_angular_velocities_accelerations(self):
"""Read both gyroscope and accelerometer in one call.
internal_filesystem/lib/mpos/sensor_manager.py
+48 -38
View File
@@ -680,6 +680,9 @@ class _WsenISDSDriver(_IMUDriver):
gyro_range="500dps",
gyro_data_rate="104Hz"
)
# Software calibration offsets
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)."""
@@ -705,55 +708,62 @@ class _WsenISDSDriver(_IMUDriver):
return self.sensor.temperature
def calibrate_accelerometer(self, samples):
"""Calibrate accelerometer using hardware calibration."""
self.sensor.acc_calibrate(samples)
return_x = (self.sensor.acc_offset_x * self.sensor.acc_sensitivity / 1000.0) * _GRAVITY
return_y = (self.sensor.acc_offset_y * self.sensor.acc_sensitivity / 1000.0) * _GRAVITY
return_z = (self.sensor.acc_offset_z * self.sensor.acc_sensitivity / 1000.0) * _GRAVITY
print(f"normal return_z: {return_z}")
"""Calibrate accelerometer (device must be stationary)."""
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
for _ in range(samples):
ax, ay, az = self.sensor._read_raw_accelerations()
sum_x += (ax / 1000.0) * _GRAVITY
sum_y += (ay / 1000.0) * _GRAVITY
sum_z += (az / 1000.0) * _GRAVITY
time.sleep_ms(10)
print(f"sumz: {sum_z}")
z_offset = 0
if _mounted_position == FACING_EARTH:
return_z *= -1
print(f"sensor is facing earth so returning inverse: {return_z}")
return_z -= _GRAVITY
print(f"returning: {return_x},{return_y},{return_z}")
# Return offsets in m/s² (convert from raw offsets)
return (return_x, return_y, return_z)
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
print(f"offsets: {self.accel_offset}")
return tuple(self.accel_offset)
def calibrate_gyroscope(self, samples):
"""Calibrate gyroscope using hardware calibration."""
self.sensor.gyro_calibrate(samples)
# Return offsets in deg/s (convert from raw offsets)
return (
(self.sensor.gyro_offset_x * self.sensor.gyro_sensitivity) / 1000.0,
(self.sensor.gyro_offset_y * self.sensor.gyro_sensitivity) / 1000.0,
(self.sensor.gyro_offset_z * self.sensor.gyro_sensitivity) / 1000.0
)
"""Calibrate gyroscope (device must be stationary)."""
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
for _ in range(samples):
gx, gy, gz = self.sensor._read_raw_angular_velocities()
sum_x += gx
sum_y += gy
sum_z += gz
time.sleep_ms(10)
# Average offsets (should be 0 when stationary)
self.gyro_offset[0] = sum_x / samples
self.gyro_offset[1] = sum_y / samples
self.gyro_offset[2] = sum_z / samples
return tuple(self.gyro_offset)
def get_calibration(self):
"""Get current calibration (raw offsets from hardware)."""
"""Get current calibration."""
return {
'accel_offsets': [
self.sensor.acc_offset_x,
self.sensor.acc_offset_y,
self.sensor.acc_offset_z
],
'gyro_offsets': [
self.sensor.gyro_offset_x,
self.sensor.gyro_offset_y,
self.sensor.gyro_offset_z
]
'accel_offsets': self.accel_offset,
'gyro_offsets': self.gyro_offset
}
def set_calibration(self, accel_offsets, gyro_offsets):
"""Set calibration from saved values (raw offsets)."""
"""Set calibration from saved values."""
if accel_offsets:
self.sensor.acc_offset_x = accel_offsets[0]
self.sensor.acc_offset_y = accel_offsets[1]
self.sensor.acc_offset_z = accel_offsets[2]
self.accel_offset = list(accel_offsets)
if gyro_offsets:
self.sensor.gyro_offset_x = gyro_offsets[0]
self.sensor.gyro_offset_y = gyro_offsets[1]
self.sensor.gyro_offset_z = gyro_offsets[2]
self.gyro_offset = list(gyro_offsets)
# ============================================================================