From 3cc6d81e589c8d253075756dc49f105051f16936 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20NEDJAR?= Date: Wed, 25 Mar 2026 12:56:39 +0100 Subject: [PATCH 1/7] feat(steami_config): Add magnetometer calibration storage. --- lib/steami_config/steami_config/device.py | 69 +++++++++++++++++ tests/scenarios/steami_config.yaml | 90 +++++++++++++++++++++++ 2 files changed, 159 insertions(+) diff --git a/lib/steami_config/steami_config/device.py b/lib/steami_config/steami_config/device.py index f818c989..433e5307 100644 --- a/lib/steami_config/steami_config/device.py +++ b/lib/steami_config/steami_config/device.py @@ -134,3 +134,72 @@ def apply_temperature_calibration(self, sensor_instance): return sensor_instance._temp_gain = cal["gain"] sensor_instance._temp_offset = cal["offset"] + + # -------------------------------------------------- + # Magnetometer calibration + # -------------------------------------------------- + + def set_magnetometer_calibration( + self, + hard_iron_x=0.0, + hard_iron_y=0.0, + hard_iron_z=0.0, + soft_iron_x=1.0, + soft_iron_y=1.0, + soft_iron_z=1.0, + ): + """Store magnetometer hard-iron and soft-iron calibration. + + Args: + hard_iron_x: X-axis hard-iron offset. + hard_iron_y: Y-axis hard-iron offset. + hard_iron_z: Z-axis hard-iron offset. + soft_iron_x: X-axis soft-iron scale factor. + soft_iron_y: Y-axis soft-iron scale factor. + soft_iron_z: Z-axis soft-iron scale factor. + """ + self._data["cm"] = { + "hx": hard_iron_x, + "hy": hard_iron_y, + "hz": hard_iron_z, + "sx": soft_iron_x, + "sy": soft_iron_y, + "sz": soft_iron_z, + } + + def get_magnetometer_calibration(self): + """Return magnetometer calibration data. + + Returns: + dict with hard_iron_x/y/z and soft_iron_x/y/z keys, or None. + """ + cm = self._data.get("cm") + if cm is None: + return None + return { + "hard_iron_x": cm["hx"], + "hard_iron_y": cm["hy"], + "hard_iron_z": cm["hz"], + "soft_iron_x": cm["sx"], + "soft_iron_y": cm["sy"], + "soft_iron_z": cm["sz"], + } + + def apply_magnetometer_calibration(self, lis2mdl_instance): + """Apply stored magnetometer calibration to a LIS2MDL instance. + + The instance must have x_off/y_off/z_off and x_scale/y_scale/z_scale + attributes. + + Args: + lis2mdl_instance: a LIS2MDL driver instance. + """ + cal = self.get_magnetometer_calibration() + if cal is None: + return + lis2mdl_instance.x_off = cal["hard_iron_x"] + lis2mdl_instance.y_off = cal["hard_iron_y"] + lis2mdl_instance.z_off = cal["hard_iron_z"] + lis2mdl_instance.x_scale = cal["soft_iron_x"] + lis2mdl_instance.y_scale = cal["soft_iron_y"] + lis2mdl_instance.z_scale = cal["soft_iron_z"] diff --git a/tests/scenarios/steami_config.yaml b/tests/scenarios/steami_config.yaml index f34af5b4..9ef4e103 100644 --- a/tests/scenarios/steami_config.yaml +++ b/tests/scenarios/steami_config.yaml @@ -128,6 +128,96 @@ tests: expect_true: true mode: [mock] + - name: "Set and get magnetometer calibration" + action: script + script: | + dev._data = {} + dev.set_magnetometer_calibration( + hard_iron_x=12.3, hard_iron_y=-5.1, hard_iron_z=0.8, + soft_iron_x=1.01, soft_iron_y=0.98, soft_iron_z=1.0, + ) + cal = dev.get_magnetometer_calibration() + result = ( + cal["hard_iron_x"] == 12.3 + and cal["hard_iron_y"] == -5.1 + and cal["hard_iron_z"] == 0.8 + and cal["soft_iron_x"] == 1.01 + and cal["soft_iron_y"] == 0.98 + and cal["soft_iron_z"] == 1.0 + ) + expect_true: true + mode: [mock] + + - name: "Get magnetometer calibration returns None when not set" + action: script + script: | + dev._data = {} + result = dev.get_magnetometer_calibration() is None + expect_true: true + mode: [mock] + + - name: "Apply magnetometer calibration to LIS2MDL" + action: script + script: | + dev._data = {"cm": {"hx": 10.0, "hy": -3.0, "hz": 1.5, "sx": 1.02, "sy": 0.99, "sz": 1.0}} + + class LIS2MDL(object): + def __init__(self): + self.x_off = 0 + self.y_off = 0 + self.z_off = 0 + self.x_scale = 1 + self.y_scale = 1 + self.z_scale = 1 + + mag = LIS2MDL() + dev.apply_magnetometer_calibration(mag) + result = ( + mag.x_off == 10.0 + and mag.y_off == -3.0 + and mag.z_off == 1.5 + and mag.x_scale == 1.02 + and mag.y_scale == 0.99 + and mag.z_scale == 1.0 + ) + expect_true: true + mode: [mock] + + - name: "Apply magnetometer calibration does nothing when not set" + action: script + script: | + dev._data = {} + + class LIS2MDL(object): + def __init__(self): + self.x_off = 0 + self.y_off = 0 + self.z_off = 0 + self.x_scale = 1 + self.y_scale = 1 + self.z_scale = 1 + + mag = LIS2MDL() + dev.apply_magnetometer_calibration(mag) + result = mag.x_off == 0 and mag.x_scale == 1 + expect_true: true + mode: [mock] + + - name: "Magnetometer calibration survives save/load" + action: script + script: | + dev._data = {} + dev.set_magnetometer_calibration( + hard_iron_x=5.5, hard_iron_y=-2.2, hard_iron_z=0.3, + ) + dev.save() + dev2 = SteamiConfig(dev._flash) + dev2.load() + cal = dev2.get_magnetometer_calibration() + result = cal["hard_iron_x"] == 5.5 and cal["soft_iron_x"] == 1.0 + expect_true: true + mode: [mock] + - name: "Board revision property" action: script script: | From 81e45fd5b16d782d09a4ba5d806552423d14dbfe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20NEDJAR?= Date: Wed, 25 Mar 2026 13:00:06 +0100 Subject: [PATCH 2/7] test(steami_config): Add hardware tests for magnetometer calibration. --- tests/scenarios/steami_config.yaml | 72 ++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/tests/scenarios/steami_config.yaml b/tests/scenarios/steami_config.yaml index 9ef4e103..5fdd0b9a 100644 --- a/tests/scenarios/steami_config.yaml +++ b/tests/scenarios/steami_config.yaml @@ -299,3 +299,75 @@ tests: result = abs(calibrated - ref) < 1.0 expect_true: true mode: [hardware] + + - name: "Save and load magnetometer calibration on hardware" + action: script + script: | + from time import sleep_ms + dev._flash.clear_config() + dev._data = {} + dev.set_magnetometer_calibration( + hard_iron_x=12.3, hard_iron_y=-5.1, hard_iron_z=0.8, + soft_iron_x=1.01, soft_iron_y=0.98, soft_iron_z=1.0, + ) + dev.save() + sleep_ms(200) + dev2 = SteamiConfig(dev._flash) + dev2.load() + cal = dev2.get_magnetometer_calibration() + result = ( + cal["hard_iron_x"] == 12.3 + and cal["hard_iron_y"] == -5.1 + and cal["soft_iron_x"] == 1.01 + ) + expect_true: true + mode: [hardware] + + - name: "Apply magnetometer calibration to real LIS2MDL" + action: script + script: | + from time import sleep_ms + from lis2mdl import LIS2MDL + mag = LIS2MDL(i2c) + dev._flash.clear_config() + dev._data = {} + dev.set_magnetometer_calibration( + hard_iron_x=10.5, hard_iron_y=-3.2, hard_iron_z=0.7, + soft_iron_x=1.02, soft_iron_y=0.99, soft_iron_z=1.0, + ) + dev.save() + sleep_ms(200) + dev2 = SteamiConfig(dev._flash) + dev2.load() + dev2.apply_magnetometer_calibration(mag) + result = ( + mag.x_off == 10.5 + and mag.y_off == -3.2 + and mag.z_off == 0.7 + and mag.x_scale == 1.02 + ) + expect_true: true + mode: [hardware] + + - name: "Magnetometer calibration coexists with temperature calibration" + action: script + script: | + from time import sleep_ms + dev._flash.clear_config() + dev._data = {} + dev.set_temperature_calibration("hts221", gain=1.05, offset=-0.3) + dev.set_magnetometer_calibration(hard_iron_x=8.0, hard_iron_y=-2.0, hard_iron_z=0.5) + dev.save() + sleep_ms(200) + dev2 = SteamiConfig(dev._flash) + dev2.load() + tc = dev2.get_temperature_calibration("hts221") + mc = dev2.get_magnetometer_calibration() + result = ( + tc["gain"] == 1.05 + and tc["offset"] == -0.3 + and mc["hard_iron_x"] == 8.0 + and mc["soft_iron_x"] == 1.0 + ) + expect_true: true + mode: [hardware] From 18aa58eb81e4e21616a2a99b1203ccbb10c4ec99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20NEDJAR?= Date: Wed, 25 Mar 2026 13:19:19 +0100 Subject: [PATCH 3/7] feat(steami_config): Add magnetometer calibration example. --- .../examples/calibrate_magnetometer.py | 81 +++++++++++++++++++ 1 file changed, 81 insertions(+) create mode 100644 lib/steami_config/examples/calibrate_magnetometer.py diff --git a/lib/steami_config/examples/calibrate_magnetometer.py b/lib/steami_config/examples/calibrate_magnetometer.py new file mode 100644 index 00000000..0fada0cb --- /dev/null +++ b/lib/steami_config/examples/calibrate_magnetometer.py @@ -0,0 +1,81 @@ +"""Calibrate the LIS2MDL magnetometer and save to persistent config. + +This example runs a 3D min/max calibration by collecting samples while +the user rotates the board in all directions. The computed hard-iron +offsets and soft-iron scale factors are stored in the config zone and +survive power cycles. + +Usage: + mpremote mount lib/ run lib/steami_config/examples/calibrate_magnetometer.py + +When prompted, slowly rotate the board in all directions (tilt, roll, +yaw) for about 12 seconds. The script then saves the calibration and +verifies it by displaying corrected heading readings. +""" + +import gc +from time import sleep_ms + +from daplink_flash.device import DaplinkFlash +from lis2mdl.device import LIS2MDL +from machine import I2C +from steami_config.device import SteamiConfig + +i2c = I2C(1) +flash = DaplinkFlash(i2c) +config = SteamiConfig(flash) +config.load() + +mag = LIS2MDL(i2c) + +# Show current state +print("=== Magnetometer Calibration ===\n") +print("Current offsets: x={:.1f} y={:.1f} z={:.1f}".format( + mag.x_off, mag.y_off, mag.z_off)) +print("Current scales: x={:.3f} y={:.3f} z={:.3f}\n".format( + mag.x_scale, mag.y_scale, mag.z_scale)) + +# Run 3D calibration +print("Rotate the board slowly in ALL directions for 12 seconds...") +print("(tilt, roll, turn upside down, spin...)\n") +sleep_ms(1000) + +mag.calibrate_minmax_3d(samples=600, delay_ms=20) + +print("Calibration complete!") +print(" Hard-iron offsets: x={:.1f} y={:.1f} z={:.1f}".format( + mag.x_off, mag.y_off, mag.z_off)) +print(" Soft-iron scales: x={:.3f} y={:.3f} z={:.3f}\n".format( + mag.x_scale, mag.y_scale, mag.z_scale)) + +# Save to config zone +config.set_magnetometer_calibration( + hard_iron_x=mag.x_off, + hard_iron_y=mag.y_off, + hard_iron_z=mag.z_off, + soft_iron_x=mag.x_scale, + soft_iron_y=mag.y_scale, + soft_iron_z=mag.z_scale, +) +config.save() +print("Calibration saved to config zone.\n") + +# Free memory before verification +gc.collect() + +# Verify: reload config and apply to a fresh sensor +config2 = SteamiConfig(flash) +config2.load() + +mag2 = LIS2MDL(i2c) +config2.apply_magnetometer_calibration(mag2) + +print("Verification (5 heading readings after reload):") +for i in range(5): + heading = mag2.heading() + norm = mag2.calibrated_field() + print(" Reading {}: heading={:.1f} deg norm=({:.3f}, {:.3f}, {:.3f})".format( + i + 1, heading, norm[0], norm[1], norm[2])) + sleep_ms(500) + +print("\nDone! Calibration is stored and will be restored at next boot.") From 3afaaffef25fb9eea6efc5b28dc057adeeced2fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20NEDJAR?= Date: Wed, 25 Mar 2026 13:21:29 +0100 Subject: [PATCH 4/7] feat(steami_config): Display calibration instructions on OLED screen. --- .../examples/calibrate_magnetometer.py | 82 +++++++++++++++++-- 1 file changed, 73 insertions(+), 9 deletions(-) diff --git a/lib/steami_config/examples/calibrate_magnetometer.py b/lib/steami_config/examples/calibrate_magnetometer.py index 0fada0cb..f0dc73d9 100644 --- a/lib/steami_config/examples/calibrate_magnetometer.py +++ b/lib/steami_config/examples/calibrate_magnetometer.py @@ -5,6 +5,8 @@ offsets and soft-iron scale factors are stored in the config zone and survive power cycles. +Instructions are displayed on the SSD1327 OLED screen when available. + Usage: mpremote mount lib/ run lib/steami_config/examples/calibrate_magnetometer.py @@ -14,18 +16,42 @@ """ import gc +import sys from time import sleep_ms -from daplink_flash.device import DaplinkFlash -from lis2mdl.device import LIS2MDL from machine import I2C -from steami_config.device import SteamiConfig + + +def show_screen(i2c, lines): + """Display text lines on the OLED screen, then free the driver.""" + try: + from ssd1327.device import WS_OLED_128X128_I2C + + oled = WS_OLED_128X128_I2C(i2c) + oled.fill(0) + for i, line in enumerate(lines): + oled.text(line, 0, i * 12, 15) + oled.show() + del oled + except Exception: + pass + sys.modules.pop("ssd1327.device", None) + gc.collect() + i2c = I2C(1) + +# --- Step 1: Load config and magnetometer --- + +from daplink_flash.device import DaplinkFlash # noqa: E402 +from steami_config.device import SteamiConfig # noqa: E402 + flash = DaplinkFlash(i2c) config = SteamiConfig(flash) config.load() +from lis2mdl.device import LIS2MDL # noqa: E402 + mag = LIS2MDL(i2c) # Show current state @@ -35,10 +61,35 @@ print("Current scales: x={:.3f} y={:.3f} z={:.3f}\n".format( mag.x_scale, mag.y_scale, mag.z_scale)) -# Run 3D calibration +# --- Step 2: Display instructions on screen --- + +show_screen(i2c, [ + "=== COMPAS ===", + "", + "Calibration du", + "magnetometre", + "", + "Tournez la carte", + "dans toutes les", + "directions...", + "", + "12 secondes", +]) + print("Rotate the board slowly in ALL directions for 12 seconds...") print("(tilt, roll, turn upside down, spin...)\n") -sleep_ms(1000) +sleep_ms(2000) + +# --- Step 3: Run 3D calibration --- + +show_screen(i2c, [ + "=== COMPAS ===", + "", + "Acquisition...", + "", + "Continuez a", + "tourner la carte", +]) mag.calibrate_minmax_3d(samples=600, delay_ms=20) @@ -48,7 +99,8 @@ print(" Soft-iron scales: x={:.3f} y={:.3f} z={:.3f}\n".format( mag.x_scale, mag.y_scale, mag.z_scale)) -# Save to config zone +# --- Step 4: Save to config zone --- + config.set_magnetometer_calibration( hard_iron_x=mag.x_off, hard_iron_y=mag.y_off, @@ -60,10 +112,18 @@ config.save() print("Calibration saved to config zone.\n") -# Free memory before verification -gc.collect() +show_screen(i2c, [ + "=== COMPAS ===", + "", + "Calibration", + "sauvegardee !", + "", + "Verification...", +]) -# Verify: reload config and apply to a fresh sensor +# --- Step 5: Verify --- + +gc.collect() config2 = SteamiConfig(flash) config2.load() @@ -71,11 +131,15 @@ config2.apply_magnetometer_calibration(mag2) print("Verification (5 heading readings after reload):") +lines = ["=== COMPAS ===", "", "Verification:"] for i in range(5): heading = mag2.heading() norm = mag2.calibrated_field() + line = " {}: cap={:.0f} deg".format(i + 1, heading) print(" Reading {}: heading={:.1f} deg norm=({:.3f}, {:.3f}, {:.3f})".format( i + 1, heading, norm[0], norm[1], norm[2])) + lines.append(line) sleep_ms(500) +show_screen(i2c, [*lines, "", "Termine !"]) print("\nDone! Calibration is stored and will be restored at next boot.") From 9173fa69cf06cc6ed3d5e5f501a69dd7b09f8422 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20NEDJAR?= Date: Wed, 25 Mar 2026 13:24:27 +0100 Subject: [PATCH 5/7] fix(steami_config): Fix heading method name and module paths in mag example. --- lib/steami_config/examples/calibrate_magnetometer.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/lib/steami_config/examples/calibrate_magnetometer.py b/lib/steami_config/examples/calibrate_magnetometer.py index f0dc73d9..7fc19d3a 100644 --- a/lib/steami_config/examples/calibrate_magnetometer.py +++ b/lib/steami_config/examples/calibrate_magnetometer.py @@ -21,6 +21,12 @@ from machine import I2C +# Add driver paths when running via mpremote mount lib/ +for p in ("daplink_flash", "steami_config", "lis2mdl", "ssd1327"): + path = "/remote/" + p + if path not in sys.path: + sys.path.insert(0, path) + def show_screen(i2c, lines): """Display text lines on the OLED screen, then free the driver.""" @@ -133,7 +139,7 @@ def show_screen(i2c, lines): print("Verification (5 heading readings after reload):") lines = ["=== COMPAS ===", "", "Verification:"] for i in range(5): - heading = mag2.heading() + heading = mag2.heading_flat_only() norm = mag2.calibrated_field() line = " {}: cap={:.0f} deg".format(i + 1, heading) print(" Reading {}: heading={:.1f} deg norm=({:.3f}, {:.3f}, {:.3f})".format( @@ -141,5 +147,5 @@ def show_screen(i2c, lines): lines.append(line) sleep_ms(500) -show_screen(i2c, [*lines, "", "Termine !"]) +show_screen(i2c, lines + ["", "Termine !"]) # noqa: RUF005 print("\nDone! Calibration is stored and will be restored at next boot.") From 3918349a8a68ce9566c495aae0164168356a0981 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20NEDJAR?= Date: Wed, 25 Mar 2026 14:31:06 +0100 Subject: [PATCH 6/7] feat(steami_config): Clean up magnetometer calibration example and docs. --- lib/steami_config/README.md | 38 +++- .../examples/calibrate_magnetometer.py | 200 ++++++++++-------- 2 files changed, 154 insertions(+), 84 deletions(-) diff --git a/lib/steami_config/README.md b/lib/steami_config/README.md index bdc60346..abc6a792 100644 --- a/lib/steami_config/README.md +++ b/lib/steami_config/README.md @@ -86,12 +86,44 @@ The sensor class name is used for lookup (`HTS221` -> `"hts221"`). --- +## Magnetometer Calibration + +Store and restore hard-iron and soft-iron calibration for the LIS2MDL. + +### Store calibration + +```python +config.set_magnetometer_calibration( + hard_iron_x=12.3, hard_iron_y=-5.1, hard_iron_z=0.8, + soft_iron_x=1.01, soft_iron_y=0.98, soft_iron_z=1.0, +) +``` + +### Read calibration + +```python +cal = config.get_magnetometer_calibration() +# -> {"hard_iron_x": 12.3, ..., "soft_iron_z": 1.0} or None +``` + +### Apply calibration to a sensor + +```python +from lis2mdl import LIS2MDL + +mag = LIS2MDL(i2c) +config.apply_magnetometer_calibration(mag) +# mag.x_off, y_off, z_off, x_scale, y_scale, z_scale are now set +``` + +--- + # 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},"pad":{"g":1.0,"o":-1.73}}} +{"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}} ``` | Key | Content | @@ -101,6 +133,9 @@ Data is stored as compact JSON to fit within 1 KB: | `tc` | Temperature calibration dict | | `tc..g` | Gain factor | | `tc..o` | Offset in °C | +| `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) | Sensor short keys: `hts` (HTS221), `mag` (LIS2MDL), `ism` (ISM330DL), `hid` (WSEN-HIDS), `pad` (WSEN-PADS). @@ -113,6 +148,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 | Run with mpremote: diff --git a/lib/steami_config/examples/calibrate_magnetometer.py b/lib/steami_config/examples/calibrate_magnetometer.py index 7fc19d3a..bc504013 100644 --- a/lib/steami_config/examples/calibrate_magnetometer.py +++ b/lib/steami_config/examples/calibrate_magnetometer.py @@ -5,99 +5,126 @@ offsets and soft-iron scale factors are stored in the config zone and survive power cycles. -Instructions are displayed on the SSD1327 OLED screen when available. - -Usage: - mpremote mount lib/ run lib/steami_config/examples/calibrate_magnetometer.py - -When prompted, slowly rotate the board in all directions (tilt, roll, -yaw) for about 12 seconds. The script then saves the calibration and -verifies it by displaying corrected heading readings. +Instructions and a countdown are displayed on the SSD1327 OLED screen. +Press MENU to start the calibration. """ import gc -import sys from time import sleep_ms -from machine import I2C +from daplink_flash import DaplinkFlash +from lis2mdl import LIS2MDL +from machine import I2C, SPI, Pin +from ssd1327 import WS_OLED_128X128_SPI +from steami_config import SteamiConfig -# Add driver paths when running via mpremote mount lib/ -for p in ("daplink_flash", "steami_config", "lis2mdl", "ssd1327"): - path = "/remote/" + p - if path not in sys.path: - sys.path.insert(0, path) +# --- Hardware init --- +i2c = I2C(1) +oled = WS_OLED_128X128_SPI( + SPI(1), + Pin("DATA_COMMAND_DISPLAY"), + Pin("RST_DISPLAY"), + Pin("CS_DISPLAY"), +) +btn_menu = Pin("MENU_BUTTON", Pin.IN, Pin.PULL_UP) -def show_screen(i2c, lines): - """Display text lines on the OLED screen, then free the driver.""" - try: - from ssd1327.device import WS_OLED_128X128_I2C +flash = DaplinkFlash(i2c) +config = SteamiConfig(flash) +config.load() +mag = LIS2MDL(i2c) +config.apply_magnetometer_calibration(mag) - oled = WS_OLED_128X128_I2C(i2c) - oled.fill(0) - for i, line in enumerate(lines): - oled.text(line, 0, i * 12, 15) - oled.show() - del oled - except Exception: - pass - sys.modules.pop("ssd1327.device", None) - gc.collect() +# --- Helper functions --- -i2c = I2C(1) -# --- Step 1: Load config and magnetometer --- +def show(lines): + """Display centered text lines on the round OLED screen.""" + oled.fill(0) + th = len(lines) * 12 + ys = max(0, (128 - th) // 2) + for i, line in enumerate(lines): + x = max(0, (128 - len(line) * 8) // 2) + oled.text(line, x, ys + i * 12, 15) + oled.show() -from daplink_flash.device import DaplinkFlash # noqa: E402 -from steami_config.device import SteamiConfig # noqa: E402 -flash = DaplinkFlash(i2c) -config = SteamiConfig(flash) -config.load() +def draw_degree(x, y, col=15): + """Draw a tiny degree symbol (3x3 circle) at pixel position.""" + oled.pixel(x + 1, y, col) + oled.pixel(x, y + 1, col) + oled.pixel(x + 2, y + 1, col) + oled.pixel(x + 1, y + 2, col) -from lis2mdl.device import LIS2MDL # noqa: E402 -mag = LIS2MDL(i2c) +def wait_menu(): + """Wait for MENU button press then release.""" + while btn_menu.value() == 1: + sleep_ms(10) + while btn_menu.value() == 0: + sleep_ms(10) + + +# --- Step 1: Display instructions and wait for MENU --- -# Show current state print("=== Magnetometer Calibration ===\n") print("Current offsets: x={:.1f} y={:.1f} z={:.1f}".format( mag.x_off, mag.y_off, mag.z_off)) print("Current scales: x={:.3f} y={:.3f} z={:.3f}\n".format( mag.x_scale, mag.y_scale, mag.z_scale)) -# --- Step 2: Display instructions on screen --- - -show_screen(i2c, [ - "=== COMPAS ===", - "", - "Calibration du", - "magnetometre", - "", - "Tournez la carte", - "dans toutes les", - "directions...", - "", - "12 secondes", -]) - -print("Rotate the board slowly in ALL directions for 12 seconds...") -print("(tilt, roll, turn upside down, spin...)\n") -sleep_ms(2000) - -# --- Step 3: Run 3D calibration --- - -show_screen(i2c, [ - "=== COMPAS ===", +show([ + "COMPAS", "", - "Acquisition...", + "Tournez la", + "carte dans", + "toutes les", + "directions", "", - "Continuez a", - "tourner la carte", + "MENU = demarrer", ]) -mag.calibrate_minmax_3d(samples=600, delay_ms=20) +print("Press MENU to start calibration...") +wait_menu() +print("Starting calibration...\n") + +# --- Step 2: Acquisition with countdown --- + +samples = 600 +delay = 20 +total_sec = (samples * delay) // 1000 +xmin = ymin = zmin = 1e9 +xmax = ymax = zmax = -1e9 + +for s in range(samples): + x, y, z = mag.magnetic_field() + xmin = min(xmin, x) + xmax = max(xmax, x) + ymin = min(ymin, y) + ymax = max(ymax, y) + zmin = min(zmin, z) + zmax = max(zmax, z) + if s % 50 == 0: + remain = total_sec - (s * delay) // 1000 + show([ + "COMPAS", + "", + "Acquisition...", + "", + "Continuez a", + "tourner", + "", + "{} sec".format(remain), + ]) + sleep_ms(delay) + +mag.x_off = (xmax + xmin) / 2.0 +mag.y_off = (ymax + ymin) / 2.0 +mag.z_off = (zmax + zmin) / 2.0 +mag.x_scale = (xmax - xmin) / 2.0 or 1.0 +mag.y_scale = (ymax - ymin) / 2.0 or 1.0 +mag.z_scale = (zmax - zmin) / 2.0 or 1.0 print("Calibration complete!") print(" Hard-iron offsets: x={:.1f} y={:.1f} z={:.1f}".format( @@ -105,7 +132,9 @@ def show_screen(i2c, lines): print(" Soft-iron scales: x={:.3f} y={:.3f} z={:.3f}\n".format( mag.x_scale, mag.y_scale, mag.z_scale)) -# --- Step 4: Save to config zone --- +# --- Step 3: Save to config zone --- + +show(["COMPAS", "", "Sauvegarde..."]) config.set_magnetometer_calibration( hard_iron_x=mag.x_off, @@ -117,17 +146,11 @@ def show_screen(i2c, lines): ) config.save() print("Calibration saved to config zone.\n") +sleep_ms(500) -show_screen(i2c, [ - "=== COMPAS ===", - "", - "Calibration", - "sauvegardee !", - "", - "Verification...", -]) +# --- Step 4: Verify --- -# --- Step 5: Verify --- +show(["COMPAS", "", "Sauvegarde OK", "", "Verification..."]) gc.collect() config2 = SteamiConfig(flash) @@ -137,15 +160,26 @@ def show_screen(i2c, lines): config2.apply_magnetometer_calibration(mag2) print("Verification (5 heading readings after reload):") -lines = ["=== COMPAS ===", "", "Verification:"] +result_lines = ["COMPAS", "", "Resultats:"] for i in range(5): heading = mag2.heading_flat_only() - norm = mag2.calibrated_field() - line = " {}: cap={:.0f} deg".format(i + 1, heading) - print(" Reading {}: heading={:.1f} deg norm=({:.3f}, {:.3f}, {:.3f})".format( - i + 1, heading, norm[0], norm[1], norm[2])) - lines.append(line) + line = " {}: cap={:.0f}".format(i + 1, heading) + print(" Reading {}: heading={:.1f} deg".format(i + 1, heading)) + result_lines.append(line) sleep_ms(500) -show_screen(i2c, lines + ["", "Termine !"]) # noqa: RUF005 +result_lines.append("") +result_lines.append("Termine !") + +# Draw results with degree symbols +oled.fill(0) +th = len(result_lines) * 12 +ys = max(0, (128 - th) // 2) +for i, line in enumerate(result_lines): + x = max(0, (128 - len(line) * 8) // 2) + oled.text(line, x, ys + i * 12, 15) + if "cap=" in line: + draw_degree(x + len(line) * 8 + 1, ys + i * 12) +oled.show() + print("\nDone! Calibration is stored and will be restored at next boot.") From d711f3c96948649aed595a19659146934fe0991a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20NEDJAR?= Date: Wed, 25 Mar 2026 14:35:02 +0100 Subject: [PATCH 7/7] fix(steami_config): Address PR review on magnetometer calibration. --- lib/steami_config/steami_config/device.py | 16 +++++++++------- tests/scenarios/steami_config.yaml | 14 ++++++++++++-- 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/lib/steami_config/steami_config/device.py b/lib/steami_config/steami_config/device.py index 433e5307..f6cc32c9 100644 --- a/lib/steami_config/steami_config/device.py +++ b/lib/steami_config/steami_config/device.py @@ -177,23 +177,25 @@ def get_magnetometer_calibration(self): if cm is None: return None return { - "hard_iron_x": cm["hx"], - "hard_iron_y": cm["hy"], - "hard_iron_z": cm["hz"], - "soft_iron_x": cm["sx"], - "soft_iron_y": cm["sy"], - "soft_iron_z": cm["sz"], + "hard_iron_x": cm.get("hx", 0.0), + "hard_iron_y": cm.get("hy", 0.0), + "hard_iron_z": cm.get("hz", 0.0), + "soft_iron_x": cm.get("sx", 1.0), + "soft_iron_y": cm.get("sy", 1.0), + "soft_iron_z": cm.get("sz", 1.0), } def apply_magnetometer_calibration(self, lis2mdl_instance): """Apply stored magnetometer calibration to a LIS2MDL instance. The instance must have x_off/y_off/z_off and x_scale/y_scale/z_scale - attributes. + attributes. Only applies to LIS2MDL instances. Args: lis2mdl_instance: a LIS2MDL driver instance. """ + if type(lis2mdl_instance).__name__.lower() != "lis2mdl": + return cal = self.get_magnetometer_calibration() if cal is None: return diff --git a/tests/scenarios/steami_config.yaml b/tests/scenarios/steami_config.yaml index 5fdd0b9a..26f464b6 100644 --- a/tests/scenarios/steami_config.yaml +++ b/tests/scenarios/steami_config.yaml @@ -199,7 +199,10 @@ tests: mag = LIS2MDL() dev.apply_magnetometer_calibration(mag) - result = mag.x_off == 0 and mag.x_scale == 1 + result = ( + mag.x_off == 0 and mag.y_off == 0 and mag.z_off == 0 + and mag.x_scale == 1 and mag.y_scale == 1 and mag.z_scale == 1 + ) expect_true: true mode: [mock] @@ -214,7 +217,14 @@ tests: dev2 = SteamiConfig(dev._flash) dev2.load() cal = dev2.get_magnetometer_calibration() - result = cal["hard_iron_x"] == 5.5 and cal["soft_iron_x"] == 1.0 + result = ( + cal["hard_iron_x"] == 5.5 + and cal["hard_iron_y"] == -2.2 + and cal["hard_iron_z"] == 0.3 + and cal["soft_iron_x"] == 1.0 + and cal["soft_iron_y"] == 1.0 + and cal["soft_iron_z"] == 1.0 + ) expect_true: true mode: [mock]