Skip to content

Commit 017a40b

Browse files
nedsebnedseb
authored
feat(steami_config): Add magnetometer calibration storage. (#241)
* feat(steami_config): Add magnetometer calibration storage. * test(steami_config): Add hardware tests for magnetometer calibration. * feat(steami_config): Add magnetometer calibration example. * feat(steami_config): Display calibration instructions on OLED screen. * fix(steami_config): Fix heading method name and module paths in mag example. * feat(steami_config): Clean up magnetometer calibration example and docs. * fix(steami_config): Address PR review on magnetometer calibration.
1 parent 5c605a6 commit 017a40b

4 files changed

Lines changed: 465 additions & 1 deletion

File tree

‎lib/steami_config/README.md‎

Lines changed: 37 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -86,12 +86,44 @@ The sensor class name is used for lookup (`HTS221` -> `"hts221"`).
8686

8787
---
8888

89+
## Magnetometer Calibration
90+
91+
Store and restore hard-iron and soft-iron calibration for the LIS2MDL.
92+
93+
### Store calibration
94+
95+
```python
96+
config.set_magnetometer_calibration(
97+
hard_iron_x=12.3, hard_iron_y=-5.1, hard_iron_z=0.8,
98+
soft_iron_x=1.01, soft_iron_y=0.98, soft_iron_z=1.0,
99+
)
100+
```
101+
102+
### Read calibration
103+
104+
```python
105+
cal = config.get_magnetometer_calibration()
106+
# -> {"hard_iron_x": 12.3, ..., "soft_iron_z": 1.0} or None
107+
```
108+
109+
### Apply calibration to a sensor
110+
111+
```python
112+
from lis2mdl import LIS2MDL
113+
114+
mag = LIS2MDL(i2c)
115+
config.apply_magnetometer_calibration(mag)
116+
# mag.x_off, y_off, z_off, x_scale, y_scale, z_scale are now set
117+
```
118+
119+
---
120+
89121
# JSON Format
90122

91123
Data is stored as compact JSON to fit within 1 KB:
92124

93125
```json
94-
{"rev":3,"name":"STeaMi-01","tc":{"hts":{"g":1.0,"o":-0.5},"pad":{"g":1.0,"o":-1.73}}}
126+
{"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}}
95127
```
96128

97129
| Key | Content |
@@ -101,6 +133,9 @@ Data is stored as compact JSON to fit within 1 KB:
101133
| `tc` | Temperature calibration dict |
102134
| `tc.<key>.g` | Gain factor |
103135
| `tc.<key>.o` | Offset in °C |
136+
| `cm` | Magnetometer calibration dict |
137+
| `cm.hx/hy/hz` | Hard-iron offsets (X, Y, Z) |
138+
| `cm.sx/sy/sz` | Soft-iron scale factors (X, Y, Z) |
104139

105140
Sensor short keys: `hts` (HTS221), `mag` (LIS2MDL), `ism` (ISM330DL),
106141
`hid` (WSEN-HIDS), `pad` (WSEN-PADS).
@@ -113,6 +148,7 @@ Sensor short keys: `hts` (HTS221), `mag` (LIS2MDL), `ism` (ISM330DL),
113148
| ------- | ----------- |
114149
| `show_config.py` | Display current board configuration |
115150
| `calibrate_temperature.py` | Calibrate all sensors against WSEN-HIDS reference |
151+
| `calibrate_magnetometer.py` | Calibrate LIS2MDL with OLED display and persistent storage |
116152

117153
Run with mpremote:
118154

‎lib/steami_config/examples/calibrate_magnetometer.py‎

Lines changed: 185 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,185 @@
1+
"""Calibrate the LIS2MDL magnetometer and save to persistent config.
2+
3+
This example runs a 3D min/max calibration by collecting samples while
4+
the user rotates the board in all directions. The computed hard-iron
5+
offsets and soft-iron scale factors are stored in the config zone and
6+
survive power cycles.
7+
8+
Instructions and a countdown are displayed on the SSD1327 OLED screen.
9+
Press MENU to start the calibration.
10+
"""
11+
12+
import gc
13+
from time import sleep_ms
14+
15+
from daplink_flash import DaplinkFlash
16+
from lis2mdl import LIS2MDL
17+
from machine import I2C, SPI, Pin
18+
from ssd1327 import WS_OLED_128X128_SPI
19+
from steami_config import SteamiConfig
20+
21+
# --- Hardware init ---
22+
23+
i2c = I2C(1)
24+
oled = WS_OLED_128X128_SPI(
25+
SPI(1),
26+
Pin("DATA_COMMAND_DISPLAY"),
27+
Pin("RST_DISPLAY"),
28+
Pin("CS_DISPLAY"),
29+
)
30+
btn_menu = Pin("MENU_BUTTON", Pin.IN, Pin.PULL_UP)
31+
32+
flash = DaplinkFlash(i2c)
33+
config = SteamiConfig(flash)
34+
config.load()
35+
mag = LIS2MDL(i2c)
36+
config.apply_magnetometer_calibration(mag)
37+
38+
39+
# --- Helper functions ---
40+
41+
42+
def show(lines):
43+
"""Display centered text lines on the round OLED screen."""
44+
oled.fill(0)
45+
th = len(lines) * 12
46+
ys = max(0, (128 - th) // 2)
47+
for i, line in enumerate(lines):
48+
x = max(0, (128 - len(line) * 8) // 2)
49+
oled.text(line, x, ys + i * 12, 15)
50+
oled.show()
51+
52+
53+
def draw_degree(x, y, col=15):
54+
"""Draw a tiny degree symbol (3x3 circle) at pixel position."""
55+
oled.pixel(x + 1, y, col)
56+
oled.pixel(x, y + 1, col)
57+
oled.pixel(x + 2, y + 1, col)
58+
oled.pixel(x + 1, y + 2, col)
59+
60+
61+
def wait_menu():
62+
"""Wait for MENU button press then release."""
63+
while btn_menu.value() == 1:
64+
sleep_ms(10)
65+
while btn_menu.value() == 0:
66+
sleep_ms(10)
67+
68+
69+
# --- Step 1: Display instructions and wait for MENU ---
70+
71+
print("=== Magnetometer Calibration ===\n")
72+
print("Current offsets: x={:.1f} y={:.1f} z={:.1f}".format(
73+
mag.x_off, mag.y_off, mag.z_off))
74+
print("Current scales: x={:.3f} y={:.3f} z={:.3f}\n".format(
75+
mag.x_scale, mag.y_scale, mag.z_scale))
76+
77+
show([
78+
"COMPAS",
79+
"",
80+
"Tournez la",
81+
"carte dans",
82+
"toutes les",
83+
"directions",
84+
"",
85+
"MENU = demarrer",
86+
])
87+
88+
print("Press MENU to start calibration...")
89+
wait_menu()
90+
print("Starting calibration...\n")
91+
92+
# --- Step 2: Acquisition with countdown ---
93+
94+
samples = 600
95+
delay = 20
96+
total_sec = (samples * delay) // 1000
97+
xmin = ymin = zmin = 1e9
98+
xmax = ymax = zmax = -1e9
99+
100+
for s in range(samples):
101+
x, y, z = mag.magnetic_field()
102+
xmin = min(xmin, x)
103+
xmax = max(xmax, x)
104+
ymin = min(ymin, y)
105+
ymax = max(ymax, y)
106+
zmin = min(zmin, z)
107+
zmax = max(zmax, z)
108+
if s % 50 == 0:
109+
remain = total_sec - (s * delay) // 1000
110+
show([
111+
"COMPAS",
112+
"",
113+
"Acquisition...",
114+
"",
115+
"Continuez a",
116+
"tourner",
117+
"",
118+
"{} sec".format(remain),
119+
])
120+
sleep_ms(delay)
121+
122+
mag.x_off = (xmax + xmin) / 2.0
123+
mag.y_off = (ymax + ymin) / 2.0
124+
mag.z_off = (zmax + zmin) / 2.0
125+
mag.x_scale = (xmax - xmin) / 2.0 or 1.0
126+
mag.y_scale = (ymax - ymin) / 2.0 or 1.0
127+
mag.z_scale = (zmax - zmin) / 2.0 or 1.0
128+
129+
print("Calibration complete!")
130+
print(" Hard-iron offsets: x={:.1f} y={:.1f} z={:.1f}".format(
131+
mag.x_off, mag.y_off, mag.z_off))
132+
print(" Soft-iron scales: x={:.3f} y={:.3f} z={:.3f}\n".format(
133+
mag.x_scale, mag.y_scale, mag.z_scale))
134+
135+
# --- Step 3: Save to config zone ---
136+
137+
show(["COMPAS", "", "Sauvegarde..."])
138+
139+
config.set_magnetometer_calibration(
140+
hard_iron_x=mag.x_off,
141+
hard_iron_y=mag.y_off,
142+
hard_iron_z=mag.z_off,
143+
soft_iron_x=mag.x_scale,
144+
soft_iron_y=mag.y_scale,
145+
soft_iron_z=mag.z_scale,
146+
)
147+
config.save()
148+
print("Calibration saved to config zone.\n")
149+
sleep_ms(500)
150+
151+
# --- Step 4: Verify ---
152+
153+
show(["COMPAS", "", "Sauvegarde OK", "", "Verification..."])
154+
155+
gc.collect()
156+
config2 = SteamiConfig(flash)
157+
config2.load()
158+
159+
mag2 = LIS2MDL(i2c)
160+
config2.apply_magnetometer_calibration(mag2)
161+
162+
print("Verification (5 heading readings after reload):")
163+
result_lines = ["COMPAS", "", "Resultats:"]
164+
for i in range(5):
165+
heading = mag2.heading_flat_only()
166+
line = " {}: cap={:.0f}".format(i + 1, heading)
167+
print(" Reading {}: heading={:.1f} deg".format(i + 1, heading))
168+
result_lines.append(line)
169+
sleep_ms(500)
170+
171+
result_lines.append("")
172+
result_lines.append("Termine !")
173+
174+
# Draw results with degree symbols
175+
oled.fill(0)
176+
th = len(result_lines) * 12
177+
ys = max(0, (128 - th) // 2)
178+
for i, line in enumerate(result_lines):
179+
x = max(0, (128 - len(line) * 8) // 2)
180+
oled.text(line, x, ys + i * 12, 15)
181+
if "cap=" in line:
182+
draw_degree(x + len(line) * 8 + 1, ys + i * 12)
183+
oled.show()
184+
185+
print("\nDone! Calibration is stored and will be restored at next boot.")

‎lib/steami_config/steami_config/device.py‎

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -134,3 +134,74 @@ def apply_temperature_calibration(self, sensor_instance):
134134
return
135135
sensor_instance._temp_gain = cal["gain"]
136136
sensor_instance._temp_offset = cal["offset"]
137+
138+
# --------------------------------------------------
139+
# Magnetometer calibration
140+
# --------------------------------------------------
141+
142+
def set_magnetometer_calibration(
143+
self,
144+
hard_iron_x=0.0,
145+
hard_iron_y=0.0,
146+
hard_iron_z=0.0,
147+
soft_iron_x=1.0,
148+
soft_iron_y=1.0,
149+
soft_iron_z=1.0,
150+
):
151+
"""Store magnetometer hard-iron and soft-iron calibration.
152+
153+
Args:
154+
hard_iron_x: X-axis hard-iron offset.
155+
hard_iron_y: Y-axis hard-iron offset.
156+
hard_iron_z: Z-axis hard-iron offset.
157+
soft_iron_x: X-axis soft-iron scale factor.
158+
soft_iron_y: Y-axis soft-iron scale factor.
159+
soft_iron_z: Z-axis soft-iron scale factor.
160+
"""
161+
self._data["cm"] = {
162+
"hx": hard_iron_x,
163+
"hy": hard_iron_y,
164+
"hz": hard_iron_z,
165+
"sx": soft_iron_x,
166+
"sy": soft_iron_y,
167+
"sz": soft_iron_z,
168+
}
169+
170+
def get_magnetometer_calibration(self):
171+
"""Return magnetometer calibration data.
172+
173+
Returns:
174+
dict with hard_iron_x/y/z and soft_iron_x/y/z keys, or None.
175+
"""
176+
cm = self._data.get("cm")
177+
if cm is None:
178+
return None
179+
return {
180+
"hard_iron_x": cm.get("hx", 0.0),
181+
"hard_iron_y": cm.get("hy", 0.0),
182+
"hard_iron_z": cm.get("hz", 0.0),
183+
"soft_iron_x": cm.get("sx", 1.0),
184+
"soft_iron_y": cm.get("sy", 1.0),
185+
"soft_iron_z": cm.get("sz", 1.0),
186+
}
187+
188+
def apply_magnetometer_calibration(self, lis2mdl_instance):
189+
"""Apply stored magnetometer calibration to a LIS2MDL instance.
190+
191+
The instance must have x_off/y_off/z_off and x_scale/y_scale/z_scale
192+
attributes. Only applies to LIS2MDL instances.
193+
194+
Args:
195+
lis2mdl_instance: a LIS2MDL driver instance.
196+
"""
197+
if type(lis2mdl_instance).__name__.lower() != "lis2mdl":
198+
return
199+
cal = self.get_magnetometer_calibration()
200+
if cal is None:
201+
return
202+
lis2mdl_instance.x_off = cal["hard_iron_x"]
203+
lis2mdl_instance.y_off = cal["hard_iron_y"]
204+
lis2mdl_instance.z_off = cal["hard_iron_z"]
205+
lis2mdl_instance.x_scale = cal["soft_iron_x"]
206+
lis2mdl_instance.y_scale = cal["soft_iron_y"]
207+
lis2mdl_instance.z_scale = cal["soft_iron_z"]

0 commit comments

Comments
 (0)