diff --git a/lib/ism330dl/README.md b/lib/ism330dl/README.md index 66f2b09..c742b03 100644 --- a/lib/ism330dl/README.md +++ b/lib/ism330dl/README.md @@ -20,6 +20,7 @@ This driver provides a simple API to configure the sensor and read motion data u * raw sensor readings * converted physical units * board orientation reading +* accelerometer bias offset correction * board rotation reading * temperature reading * data-ready status helpers @@ -125,6 +126,40 @@ imu.acceleration_ms2() imu.orientation() ``` +### Accelerometer calibration + +The driver supports accelerometer bias correction using per-axis offsets. + +### Set offsets + +```python +imu.set_accel_offset(ox=0.01, oy=-0.02, oz=0.03) +``` + +### Get offsets +```python +imu.get_accel_offset() +# -> (0.01, -0.02, 0.03) +``` + +--- + +### Notes on persistent calibration (STeaMi) + +On the STeaMi board, accelerometer calibration can be stored using +`steami_config` and automatically applied at startup: + +```python +from steami_config import SteamiConfig +from daplink_bridge import DaplinkBridge + +config = SteamiConfig(DaplinkBridge(i2c)) +config.load() + +imu = ISM330DL(i2c) +config.apply_accelerometer_calibration(imu) +``` + --- ## Gyroscope @@ -251,4 +286,4 @@ The repository provides several example scripts: | `static_orientation.py` | Detect device orientation using the accelerometer | | `motion_orientation.py` | Detect rotation using the gyroscope | ---- \ No newline at end of file +--- diff --git a/lib/ism330dl/ism330dl/device.py b/lib/ism330dl/ism330dl/device.py index 5f4b3f5..b0d4a7c 100644 --- a/lib/ism330dl/ism330dl/device.py +++ b/lib/ism330dl/ism330dl/device.py @@ -51,6 +51,10 @@ def __init__(self, i2c, address=ISM330DL_I2C_DEFAULT_ADDR): self._gyro_scale = GYRO_FS_250DPS self._gyro_odr = GYRO_ODR_104HZ + self._accel_offset_x = 0.0 + self._accel_offset_y = 0.0 + self._accel_offset_z = 0.0 + self._temp_gain = 1.0 self._temp_offset = 0.0 @@ -200,7 +204,27 @@ def temperature_raw(self): def acceleration_g(self): sens = ACCEL_SENSITIVITY_MG[self._accel_scale] raw = self.acceleration_raw() - return tuple((v * sens) / 1000.0 for v in raw) + values = tuple((v * sens) / 1000.0 for v in raw) + + return ( + values[0] - self._accel_offset_x, + values[1] - self._accel_offset_y, + values[2] - self._accel_offset_z, + ) + + def set_accel_offset(self, ox=0.0, oy=0.0, oz=0.0): + """Set accelerometer bias offsets in g.""" + self._accel_offset_x = float(ox) + self._accel_offset_y = float(oy) + self._accel_offset_z = float(oz) + + def get_accel_offset(self): + """Return accelerometer bias offsets in g.""" + return ( + self._accel_offset_x, + self._accel_offset_y, + self._accel_offset_z, + ) def acceleration_ms2(self): g = self.acceleration_g() diff --git a/lib/steami_config/README.md b/lib/steami_config/README.md index c361916..5335b03 100644 --- a/lib/steami_config/README.md +++ b/lib/steami_config/README.md @@ -118,12 +118,39 @@ config.apply_magnetometer_calibration(mag) --- +## Accelerometer Calibration + +Store and restore accelerometer bias offsets for the ISM330DL. + +### Store calibration + +```python +config.set_accelerometer_calibration(ox=0.01, oy=-0.02, oz=0.03) +``` + +### Read calibration + +```python +cal = config.get_accelerometer_calibration() +# -> {"ox": 0.01, "oy": -0.02, "oz": 0.03} or None +``` + +### Apply calibration to a sensor +```python +from ism330dl import ISM330DL + +imu = ISM330DL(i2c) +config.apply_accelerometer_calibration(imu) +``` + +--- + # JSON Format Data is stored as compact JSON to fit within 1 KB: ```json -{"rev":3,"name":"STeaMi-01","tc":{"hts":{"g":1.0,"o":-0.5}},"cm":{"hx":12.3,"hy":-5.1,"hz":0.8,"sx":1.01,"sy":0.98,"sz":1.0}} +{"rev":3,"name":"STeaMi-01","tc":{"hts":{"g":1.0,"o":-0.5}},"cm":{"hx":12.3,"hy":-5.1,"hz":0.8,"sx":1.01,"sy":0.98,"sz":1.0},"ca":{"ox":0.01,"oy":-0.02,"oz":0.03}} ``` | Key | Content | @@ -136,6 +163,8 @@ Data is stored as compact JSON to fit within 1 KB: | `cm` | Magnetometer calibration dict | | `cm.hx/hy/hz` | Hard-iron offsets (X, Y, Z) | | `cm.sx/sy/sz` | Soft-iron scale factors (X, Y, Z) | +| `ca` | Accelerometer calibration dict | +| `ca.ox/oy/oz` | Bias offsets in g (X, Y, Z) | Sensor short keys: `hts` (HTS221), `mag` (LIS2MDL), `ism` (ISM330DL), `hid` (WSEN-HIDS), `pad` (WSEN-PADS). @@ -149,6 +178,7 @@ Sensor short keys: `hts` (HTS221), `mag` (LIS2MDL), `ism` (ISM330DL), | `show_config.py` | Display current board configuration | | `calibrate_temperature.py` | Calibrate all sensors against WSEN-HIDS reference | | `calibrate_magnetometer.py` | Calibrate LIS2MDL with OLED display and persistent storage | +| `calibrate_accelerometer.py` | Calibrate ISM330DL accelerometer bias and persist it | Run with mpremote: diff --git a/lib/steami_config/examples/calibrate_accelerometer.py b/lib/steami_config/examples/calibrate_accelerometer.py new file mode 100644 index 0000000..0a0081f --- /dev/null +++ b/lib/steami_config/examples/calibrate_accelerometer.py @@ -0,0 +1,92 @@ +"""Calibrate ISM330DL accelerometer bias and save to persistent config. + +This example measures accelerometer offsets while the board is lying flat. +The computed offsets (ox, oy, oz) are stored in the config zone and +survive power cycles. + +Instructions: +- Place the board flat and still (screen up) +- Wait for measurement +""" + +from time import sleep_ms + +from daplink_bridge import DaplinkBridge +from ism330dl import ISM330DL +from machine import I2C +from steami_config import SteamiConfig + +# --- Init --- + +i2c = I2C(1) +bridge = DaplinkBridge(i2c) +config = SteamiConfig(bridge) +config.load() + +imu = ISM330DL(i2c) + +print("=== Accelerometer Calibration ===\n") +print("Place the board flat (screen up) and keep it still...") +sleep_ms(2000) + +# --- Step 1: Collect samples --- + +samples = 100 +sx = sy = sz = 0.0 + +for _ in range(samples): + ax, ay, az = imu.acceleration_g() + sx += ax + sy += ay + sz += az + sleep_ms(20) + +ax = sx / samples +ay = sy / samples +az = sz / samples + +# Expected resting orientation: flat, screen up -> (0, 0, -1g) +ox = ax +oy = ay +oz = az + 1.0 +# Flat, screen up → expected (0,0,-1g), so Z offset = measured - (-1g) = az + 1.0 +# because gravity points downward while the sensor Z axis is defined positive downward + +print("\nMeasured average:") +print(" ax = {:.3f} g".format(ax)) +print(" ay = {:.3f} g".format(ay)) +print(" az = {:.3f} g".format(az)) + +print("\nComputed offsets:") +print(" ox = {:.3f} g".format(ox)) +print(" oy = {:.3f} g".format(oy)) +print(" oz = {:.3f} g".format(oz)) + +# --- Step 2: Save to config zone --- + +config.set_accelerometer_calibration(ox=ox, oy=oy, oz=oz) +config.save() + +print("\nCalibration saved to config zone.") + +# --- Step 3: Verify after reload --- + +config2 = SteamiConfig(bridge) +config2.load() + +imu2 = ISM330DL(i2c) +config2.apply_accelerometer_calibration(imu2) + +print("\nApplied offsets after reload:") +ox2, oy2, oz2 = imu2.get_accel_offset() +print(" ox = {:.3f} g".format(ox2)) +print(" oy = {:.3f} g".format(oy2)) +print(" oz = {:.3f} g".format(oz2)) + +print("\nVerification (5 corrected readings):") +for i in range(5): + ax, ay, az = imu2.acceleration_g() + print(" {}: ax={:.3f} ay={:.3f} az={:.3f}".format(i + 1, ax, ay, az)) + sleep_ms(500) + +print("\nDone! Calibration is stored and will be restored at next boot.") diff --git a/lib/steami_config/steami_config/device.py b/lib/steami_config/steami_config/device.py index 6d8e684..398341c 100644 --- a/lib/steami_config/steami_config/device.py +++ b/lib/steami_config/steami_config/device.py @@ -12,7 +12,6 @@ # Reverse map: short key -> sensor name. _KEY_SENSORS = {v: k for k, v in _SENSOR_KEYS.items()} - class SteamiConfig(object): """Persistent configuration stored in the DAPLink F103 config zone. @@ -205,3 +204,53 @@ def apply_magnetometer_calibration(self, lis2mdl_instance): lis2mdl_instance.x_scale = cal["soft_iron_x"] lis2mdl_instance.y_scale = cal["soft_iron_y"] lis2mdl_instance.z_scale = cal["soft_iron_z"] + + # -------------------------------------------------- + # Accelerometer calibration + # -------------------------------------------------- + + def set_accelerometer_calibration(self, ox=0.0, oy=0.0, oz=0.0): + """Store accelerometer bias offsets (in g). + + Args: + ox: X-axis offset + oy: Y-axis offset + oz: Z-axis offset + """ + self._data["ca"] = { + "ox": float(ox), + "oy": float(oy), + "oz": float(oz), + } + + def get_accelerometer_calibration(self): + """Return accelerometer calibration offsets. + + Returns: + dict with ox, oy, oz or None + """ + cal = self._data.get("ca") + if cal is None: + return None + + return { + "ox": cal.get("ox", 0.0), + "oy": cal.get("oy", 0.0), + "oz": cal.get("oz", 0.0), + } + + + def apply_accelerometer_calibration(self, ism330dl_instance): + """Apply stored accelerometer calibration to an ISM330DL instance.""" + if type(ism330dl_instance).__name__.lower() != "ism330dl": + return + + cal = self.get_accelerometer_calibration() + if cal is None: + return + + ism330dl_instance.set_accel_offset( + cal["ox"], + cal["oy"], + cal["oz"], + ) diff --git a/tests/scenarios/ism330dl.yaml b/tests/scenarios/ism330dl.yaml index 21595bc..2c4d39b 100644 --- a/tests/scenarios/ism330dl.yaml +++ b/tests/scenarios/ism330dl.yaml @@ -100,6 +100,72 @@ tests: expect_true: true mode: [hardware] + - name: "Get accel offset returns defaults" + action: script + script: | + ox, oy, oz = dev.get_accel_offset() + result = ox == 0.0 and oy == 0.0 and oz == 0.0 + expect_true: true + mode: [mock] + + - name: "Set accel offset updates stored values" + action: script + script: | + dev.set_accel_offset(0.01, -0.02, 0.03) + ox, oy, oz = dev.get_accel_offset() + result = ox == 0.01 and oy == -0.02 and oz == 0.03 + expect_true: true + mode: [mock] + + - name: "Acceleration g applies accel offsets" + action: script + script: | + dev.set_accel_offset(0.1, -0.2, 0.3) + ax, ay, az = dev.acceleration_g() + result = ( + abs(ax - (-0.1)) < 0.01 + and abs(ay - 0.2) < 0.01 + and abs(az - 0.7) < 0.01 + ) + expect_true: true + mode: [mock] + + - name: "Acceleration ms2 applies accel offsets" + action: script + script: | + dev.set_accel_offset(0.1, 0.0, 0.0) + ax, ay, az = dev.acceleration_ms2() + result = abs(ax - (-0.1 * 9.80665)) < 0.02 + expect_true: true + mode: [mock] + + - name: "Orientation changes with accel offset" + action: script + script: | + dev.set_accel_offset(0.0, 0.0, 2.0) + result = dev.orientation() == "SCREEN_UP" + expect_true: true + mode: [mock] + + - name: "Set and get accel offset on hardware" + action: script + script: | + dev.set_accel_offset(0.01, -0.02, 0.03) + ox, oy, oz = dev.get_accel_offset() + result = ox == 0.01 and oy == -0.02 and oz == 0.03 + expect_true: true + mode: [hardware] + + - name: "Accel offset affects hardware readings" + action: script + script: | + ax1, ay1, az1 = dev.acceleration_g() + dev.set_accel_offset(0.1, 0.0, 0.0) + ax2, ay2, az2 = dev.acceleration_g() + result = abs((ax1 - ax2) - 0.1) < 0.05 + expect_true: true + mode: [hardware] + # ----- Gyroscope ----- - name: "Gyroscope raw returns tuple of 3 ints" diff --git a/tests/scenarios/steami_config.yaml b/tests/scenarios/steami_config.yaml index b3d2941..6021016 100644 --- a/tests/scenarios/steami_config.yaml +++ b/tests/scenarios/steami_config.yaml @@ -48,6 +48,7 @@ tests: expect_true: true mode: [mock] + # -- Mock Temperature Calibration -- - name: "Set and get temperature calibration" action: script script: | @@ -126,6 +127,7 @@ tests: expect_true: true mode: [mock] + # -- Mock Magnetometer Calibration -- - name: "Set and get magnetometer calibration" action: script script: | @@ -243,6 +245,98 @@ tests: result = dev.board_name == "Test-Board" expect_true: true mode: [mock] + + # -- Mock Accelerometer Calibration -- + - name: "Set and get accelerometer calibration" + action: script + script: | + dev._data = {} + dev.set_accelerometer_calibration(ox=0.01, oy=-0.02, oz=0.03) + cal = dev.get_accelerometer_calibration() + result = ( + cal["ox"] == 0.01 + and cal["oy"] == -0.02 + and cal["oz"] == 0.03 + ) + expect_true: true + mode: [mock] + + - name: "Get accelerometer calibration returns None when not set" + action: script + script: | + dev._data = {} + result = dev.get_accelerometer_calibration() is None + expect_true: true + mode: [mock] + + - name: "Apply accelerometer calibration to ISM330DL" + action: script + script: | + dev._data = {"ca": {"ox": 0.01, "oy": -0.02, "oz": 0.03}} + + class ISM330DL(object): + def __init__(self): + self._accel_offset_x = 0.0 + self._accel_offset_y = 0.0 + self._accel_offset_z = 0.0 + + def set_accel_offset(self, ox=0.0, oy=0.0, oz=0.0): + self._accel_offset_x = ox + self._accel_offset_y = oy + self._accel_offset_z = oz + + imu = ISM330DL() + dev.apply_accelerometer_calibration(imu) + result = ( + imu._accel_offset_x == 0.01 + and imu._accel_offset_y == -0.02 + and imu._accel_offset_z == 0.03 + ) + expect_true: true + mode: [mock] + + - name: "Apply accelerometer calibration does nothing when not set" + action: script + script: | + dev._data = {} + + class ISM330DL(object): + def __init__(self): + self._accel_offset_x = 0.0 + self._accel_offset_y = 0.0 + self._accel_offset_z = 0.0 + + def set_accel_offset(self, ox=0.0, oy=0.0, oz=0.0): + self._accel_offset_x = ox + self._accel_offset_y = oy + self._accel_offset_z = oz + + imu = ISM330DL() + dev.apply_accelerometer_calibration(imu) + result = ( + imu._accel_offset_x == 0.0 + and imu._accel_offset_y == 0.0 + and imu._accel_offset_z == 0.0 + ) + expect_true: true + mode: [mock] + + - name: "Accelerometer calibration survives save/load" + action: script + script: | + dev._data = {} + dev.set_accelerometer_calibration(ox=0.01, oy=-0.02, oz=0.03) + dev.save() + dev2 = SteamiConfig(dev._bridge) + dev2.load() + cal = dev2.get_accelerometer_calibration() + result = ( + cal["ox"] == 0.01 + and cal["oy"] == -0.02 + and cal["oz"] == 0.03 + ) + expect_true: true + mode: [mock] # ----- Hardware ----- @@ -286,6 +380,7 @@ tests: expect_true: true mode: [hardware] + # -- Hardware Temperature Calibration -- - name: "Apply calibration to real sensor" action: script script: | @@ -310,6 +405,7 @@ tests: expect_true: true mode: [hardware] + # -- Hardware Magnetometer Calibration -- - name: "Save and load magnetometer calibration on hardware" action: script script: | @@ -381,3 +477,67 @@ tests: ) expect_true: true mode: [hardware] + + # -- Hardware Accelerometer Calibration -- + - name: "Save and load accelerometer calibration on hardware" + action: script + script: | + from time import sleep_ms + dev._bridge.clear_config() + dev._data = {} + dev.set_accelerometer_calibration(ox=0.01, oy=-0.02, oz=0.03) + dev.save() + sleep_ms(200) + dev2 = SteamiConfig(dev._bridge) + dev2.load() + cal = dev2.get_accelerometer_calibration() + result = ( + cal["ox"] == 0.01 + and cal["oy"] == -0.02 + and cal["oz"] == 0.03 + ) + expect_true: true + mode: [hardware] + + - name: "Apply accelerometer calibration to real ISM330DL" + action: script + script: | + from time import sleep_ms + from ism330dl import ISM330DL + imu = ISM330DL(i2c) + dev._bridge.clear_config() + dev._data = {} + dev.set_accelerometer_calibration(ox=0.01, oy=-0.02, oz=0.03) + dev.save() + sleep_ms(200) + dev2 = SteamiConfig(dev._bridge) + dev2.load() + dev2.apply_accelerometer_calibration(imu) + ox, oy, oz = imu.get_accel_offset() + result = ox == 0.01 and oy == -0.02 and oz == 0.03 + expect_true: true + mode: [hardware] + + - name: "Accelerometer calibration coexists with temperature calibration" + action: script + script: | + from time import sleep_ms + dev._bridge.clear_config() + dev._data = {} + dev.set_temperature_calibration("hts221", gain=1.05, offset=-0.3) + dev.set_accelerometer_calibration(ox=0.01, oy=-0.02, oz=0.03) + dev.save() + sleep_ms(200) + dev2 = SteamiConfig(dev._bridge) + dev2.load() + tc = dev2.get_temperature_calibration("hts221") + ac = dev2.get_accelerometer_calibration() + result = ( + tc["gain"] == 1.05 + and tc["offset"] == -0.3 + and ac["ox"] == 0.01 + and ac["oy"] == -0.02 + and ac["oz"] == 0.03 + ) + expect_true: true + mode: [hardware]