Skip to content

Commit 141fc20

Browse files
ThomasFarstrikeThomasFarstrike
committed
Fix WSEN-ISDS calibration
1 parent 8b68838 commit 141fc20

File tree

2 files changed

+56
-46
lines changed

2 files changed

+56
-46
lines changed

‎internal_filesystem/lib/mpos/hardware/drivers/wsen_isds.py‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -299,9 +299,9 @@ def read_accelerations(self):
299299
"""
300300
raw_a_x, raw_a_y, raw_a_z = self._read_raw_accelerations()
301301

302-
a_x = (raw_a_x - self.acc_offset_x) * self.acc_sensitivity
303-
a_y = (raw_a_y - self.acc_offset_y) * self.acc_sensitivity
304-
a_z = (raw_a_z - self.acc_offset_z) * self.acc_sensitivity
302+
a_x = (raw_a_x - self.acc_offset_x)
303+
a_y = (raw_a_y - self.acc_offset_y)
304+
a_z = (raw_a_z - self.acc_offset_z)
305305

306306
return a_x, a_y, a_z
307307

@@ -316,7 +316,7 @@ def _read_raw_accelerations(self):
316316
raw_a_y = self._convert_from_raw(raw[2], raw[3])
317317
raw_a_z = self._convert_from_raw(raw[4], raw[5])
318318

319-
return raw_a_x, raw_a_y, raw_a_z
319+
return raw_a_x * self.acc_sensitivity, raw_a_y * self.acc_sensitivity, raw_a_z * self.acc_sensitivity
320320

321321
def gyro_calibrate(self, samples=None):
322322
"""Calibrate gyroscope by averaging samples while device is stationary.
@@ -350,9 +350,9 @@ def read_angular_velocities(self):
350350
"""
351351
raw_g_x, raw_g_y, raw_g_z = self._read_raw_angular_velocities()
352352

353-
g_x = (raw_g_x - self.gyro_offset_x) * self.gyro_sensitivity
354-
g_y = (raw_g_y - self.gyro_offset_y) * self.gyro_sensitivity
355-
g_z = (raw_g_z - self.gyro_offset_z) * self.gyro_sensitivity
353+
g_x = (raw_g_x - self.gyro_offset_x)
354+
g_y = (raw_g_y - self.gyro_offset_y)
355+
g_z = (raw_g_z - self.gyro_offset_z)
356356

357357
return g_x, g_y, g_z
358358

@@ -381,7 +381,7 @@ def _read_raw_angular_velocities(self):
381381
raw_g_y = self._convert_from_raw(raw[2], raw[3])
382382
raw_g_z = self._convert_from_raw(raw[4], raw[5])
383383

384-
return raw_g_x, raw_g_y, raw_g_z
384+
return raw_g_x * self.gyro_sensitivity, raw_g_y * self.gyro_sensitivity, raw_g_z * self.gyro_sensitivity
385385

386386
def read_angular_velocities_accelerations(self):
387387
"""Read both gyroscope and accelerometer in one call.

‎internal_filesystem/lib/mpos/sensor_manager.py‎

Lines changed: 48 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -680,6 +680,9 @@ def __init__(self, i2c_bus, address):
680680
gyro_range="500dps",
681681
gyro_data_rate="104Hz"
682682
)
683+
# Software calibration offsets
684+
self.accel_offset = [0.0, 0.0, 0.0]
685+
self.gyro_offset = [0.0, 0.0, 0.0]
683686

684687
def read_acceleration(self):
685688
"""Read acceleration in m/s² (converts from mg)."""
@@ -705,55 +708,62 @@ def read_temperature(self):
705708
return self.sensor.temperature
706709

707710
def calibrate_accelerometer(self, samples):
708-
"""Calibrate accelerometer using hardware calibration."""
709-
self.sensor.acc_calibrate(samples)
710-
return_x = (self.sensor.acc_offset_x * self.sensor.acc_sensitivity / 1000.0) * _GRAVITY
711-
return_y = (self.sensor.acc_offset_y * self.sensor.acc_sensitivity / 1000.0) * _GRAVITY
712-
return_z = (self.sensor.acc_offset_z * self.sensor.acc_sensitivity / 1000.0) * _GRAVITY
713-
print(f"normal return_z: {return_z}")
711+
"""Calibrate accelerometer (device must be stationary)."""
712+
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
713+
714+
for _ in range(samples):
715+
ax, ay, az = self.sensor._read_raw_accelerations()
716+
sum_x += (ax / 1000.0) * _GRAVITY
717+
sum_y += (ay / 1000.0) * _GRAVITY
718+
sum_z += (az / 1000.0) * _GRAVITY
719+
time.sleep_ms(10)
720+
721+
print(f"sumz: {sum_z}")
722+
z_offset = 0
714723
if _mounted_position == FACING_EARTH:
715-
return_z *= -1
716-
print(f"sensor is facing earth so returning inverse: {return_z}")
717-
return_z -= _GRAVITY
718-
print(f"returning: {return_x},{return_y},{return_z}")
719-
# Return offsets in m/s² (convert from raw offsets)
720-
return (return_x, return_y, return_z)
724+
sum_z *= -1
725+
z_offset = (1000 / samples) + _GRAVITY
726+
print(f"sumz: {sum_z}")
727+
728+
# Average offsets (assuming Z-axis should read +9.8 m/s²)
729+
self.accel_offset[0] = sum_x / samples
730+
self.accel_offset[1] = sum_y / samples
731+
self.accel_offset[2] = (sum_z / samples) - _GRAVITY - z_offset
732+
print(f"offsets: {self.accel_offset}")
733+
734+
return tuple(self.accel_offset)
721735

722736
def calibrate_gyroscope(self, samples):
723-
"""Calibrate gyroscope using hardware calibration."""
724-
self.sensor.gyro_calibrate(samples)
725-
# Return offsets in deg/s (convert from raw offsets)
726-
return (
727-
(self.sensor.gyro_offset_x * self.sensor.gyro_sensitivity) / 1000.0,
728-
(self.sensor.gyro_offset_y * self.sensor.gyro_sensitivity) / 1000.0,
729-
(self.sensor.gyro_offset_z * self.sensor.gyro_sensitivity) / 1000.0
730-
)
737+
"""Calibrate gyroscope (device must be stationary)."""
738+
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
739+
740+
for _ in range(samples):
741+
gx, gy, gz = self.sensor._read_raw_angular_velocities()
742+
sum_x += gx
743+
sum_y += gy
744+
sum_z += gz
745+
time.sleep_ms(10)
746+
747+
# Average offsets (should be 0 when stationary)
748+
self.gyro_offset[0] = sum_x / samples
749+
self.gyro_offset[1] = sum_y / samples
750+
self.gyro_offset[2] = sum_z / samples
751+
752+
return tuple(self.gyro_offset)
731753

732754
def get_calibration(self):
733-
"""Get current calibration (raw offsets from hardware)."""
755+
"""Get current calibration."""
734756
return {
735-
'accel_offsets': [
736-
self.sensor.acc_offset_x,
737-
self.sensor.acc_offset_y,
738-
self.sensor.acc_offset_z
739-
],
740-
'gyro_offsets': [
741-
self.sensor.gyro_offset_x,
742-
self.sensor.gyro_offset_y,
743-
self.sensor.gyro_offset_z
744-
]
757+
'accel_offsets': self.accel_offset,
758+
'gyro_offsets': self.gyro_offset
745759
}
746760

747761
def set_calibration(self, accel_offsets, gyro_offsets):
748-
"""Set calibration from saved values (raw offsets)."""
762+
"""Set calibration from saved values."""
749763
if accel_offsets:
750-
self.sensor.acc_offset_x = accel_offsets[0]
751-
self.sensor.acc_offset_y = accel_offsets[1]
752-
self.sensor.acc_offset_z = accel_offsets[2]
764+
self.accel_offset = list(accel_offsets)
753765
if gyro_offsets:
754-
self.sensor.gyro_offset_x = gyro_offsets[0]
755-
self.sensor.gyro_offset_y = gyro_offsets[1]
756-
self.sensor.gyro_offset_z = gyro_offsets[2]
766+
self.gyro_offset = list(gyro_offsets)
757767

758768

759769
# ============================================================================

0 commit comments

Comments
 (0)