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 new file mode 100644 index 00000000..bc504013 --- /dev/null +++ b/lib/steami_config/examples/calibrate_magnetometer.py @@ -0,0 +1,185 @@ +"""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. + +Instructions and a countdown are displayed on the SSD1327 OLED screen. +Press MENU to start the calibration. +""" + +import gc +from time import sleep_ms + +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 + +# --- 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) + +flash = DaplinkFlash(i2c) +config = SteamiConfig(flash) +config.load() +mag = LIS2MDL(i2c) +config.apply_magnetometer_calibration(mag) + + +# --- Helper functions --- + + +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() + + +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) + + +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 --- + +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)) + +show([ + "COMPAS", + "", + "Tournez la", + "carte dans", + "toutes les", + "directions", + "", + "MENU = demarrer", +]) + +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( + 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)) + +# --- Step 3: Save to config zone --- + +show(["COMPAS", "", "Sauvegarde..."]) + +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") +sleep_ms(500) + +# --- Step 4: Verify --- + +show(["COMPAS", "", "Sauvegarde OK", "", "Verification..."]) + +gc.collect() +config2 = SteamiConfig(flash) +config2.load() + +mag2 = LIS2MDL(i2c) +config2.apply_magnetometer_calibration(mag2) + +print("Verification (5 heading readings after reload):") +result_lines = ["COMPAS", "", "Resultats:"] +for i in range(5): + heading = mag2.heading_flat_only() + line = " {}: cap={:.0f}".format(i + 1, heading) + print(" Reading {}: heading={:.1f} deg".format(i + 1, heading)) + result_lines.append(line) + sleep_ms(500) + +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.") diff --git a/lib/steami_config/steami_config/device.py b/lib/steami_config/steami_config/device.py index f818c989..f6cc32c9 100644 --- a/lib/steami_config/steami_config/device.py +++ b/lib/steami_config/steami_config/device.py @@ -134,3 +134,74 @@ 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.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. 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 + 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..26f464b6 100644 --- a/tests/scenarios/steami_config.yaml +++ b/tests/scenarios/steami_config.yaml @@ -128,6 +128,106 @@ 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.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] + + - 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["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] + - name: "Board revision property" action: script script: | @@ -209,3 +309,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]