diff --git a/lib/lis2mdl/README.md b/lib/lis2mdl/README.md new file mode 100644 index 00000000..a1043e41 --- /dev/null +++ b/lib/lis2mdl/README.md @@ -0,0 +1,242 @@ +# LIS2MDL Magnetometer Driver for MicroPython + +A complete and feature-rich driver for the **LIS2MDL 3-axis magnetometer** by STMicroelectronics. +This library allows easy integration of the LIS2MDL sensor with MicroPython or CircuitPython boards using I²C. +It provides low-level register access, automatic calibration, heading computation, and tilt compensation — ideal for **digital compasses**, **robot navigation**, and **orientation tracking**. + +--- + +## 🧭 Features + +* ✅ Full **I²C driver** for LIS2MDL +* ✅ Supports **10 / 20 / 50 / 100 Hz** output data rates +* ✅ **Temperature-compensated** and **low-power** modes +* ✅ Read **raw**, **scaled**, or **calibrated** magnetic field data +* ✅ **2D and 3D calibration** routines (auto min/max method) +* ✅ **Heading computation** (flat or tilt-compensated) +* ✅ **Compass direction labels** (N, NE, E, …) +* ✅ Built-in **digital low-pass filter** and **offset cancellation** control +* ✅ Diagnostic utilities: + + * Register dump + * Calibration quality metrics + * Self-test and temperature readout + +--- + +## ⚙️ Hardware Requirements + +* **LIS2MDL** 3-axis magnetometer (STMicroelectronics) +* **MicroPython/CircuitPython board**, e.g.: + + * ESP32 / ESP8266 + * Raspberry Pi Pico / RP2040 + * STM32 Nucleo or Pyboard +* 3.3V power supply +* I²C interface (SCL, SDA) + +--- + +## 🔌 Wiring Example (ESP32) + +| LIS2MDL Pin | ESP32 Pin | Description | +| ----------- | --------- | ------------ | +| VIN | 3.3V | Power supply | +| GND | GND | Ground | +| SDA | GPIO21 | I²C Data | +| SCL | GPIO22 | I²C Clock | + +Example setup: + +```python +from machine import I2C, Pin +from lis2mdl import LIS2MDL + +i2c = I2C(1) +mag = LIS2MDL(i2c) +``` + +--- + +## 🚀 Quick Start + +### Read magnetic field + +```python +x, y, z = mag.read_magnet_uT() +print("Magnetic field (µT):", x, y, z) +``` + +### Get heading (flat orientation) + +```python +heading = mag.heading_flat_only() +print("Heading: {:.1f}° {}".format(heading, mag.direction_label(heading))) +``` + +### With tilt compensation + +Requires a function returning accelerometer data `(ax, ay, az)` in g: + +```python +def read_accel(): + return (0.0, 0.0, 1.0) # Example static vector + +heading = mag.heading_with_tilt_compensation(read_accel) +``` + +--- + +## ⚖️ Calibration + +Calibration is essential to obtain accurate compass readings. +The driver provides **automated routines** for both 2D (flat) and 3D calibration. + +### 2D Calibration (Flat) + +Rotate the board **horizontally** in all directions: + +```python +mag.calibrate_minmax_2d(samples=300) +``` + +### 3D Calibration (Full) + +Rotate the board **in all orientations**: + +```python +mag.calibrate_minmax_3d(samples=600) +``` + +### Apply and test calibration + +```python +print("Calibration:", mag.read_calibration()) +quality = mag.calibrate_quality() +print("Quality metrics:", quality) +``` + +### Reset calibration + +```python +mag.calibrate_reset() +``` + +--- + +## 🧰 Configuration Examples + +### Output rate and power + +```python +mag.set_odr(50) # 50 Hz +mag.set_low_power(True) # Enable low-power mode +``` + +### Enable digital low-pass filter + +```python +mag.set_low_pass(True) +``` + +### Configure declination and heading offset + +```python +mag.set_declination(1.5) # Magnetic declination (°) +mag.set_heading_offset(-5.0) # Align physical 0° +``` + +### Block data update (BDU) + +```python +mag.set_bdu(True) +``` + +--- + +## 🧮 Heading Filtering + +You can apply a **smoothing filter** on the heading angle to stabilize the readings: + +```python +mag.set_heading_filter(alpha=0.2) # Light smoothing +``` + +* `alpha = 0.0` → no filter +* `alpha = 0.1–0.3` → medium smoothing +* `alpha = 1.0` → very heavy smoothing + +--- + +## 🔍 Diagnostics & Debug + +### Read sensor ID + +```python +print("WHO_AM_I:", hex(mag.read_who_am_i())) +``` + +Expected value: `0x40` + +### Read temperature (approximate) + +```python +print("Temperature (°C):", mag.read_temperature_c()) +``` + +### Check data readiness + +```python +if mag.data_ready(): + print("New magnetic data available!") +``` + +### Dump consecutive registers + +```python +regs = mag.read_registers(0x60, 12) +print("Register dump:", regs) +``` + +--- + +## 🧠 Internal Methods Overview + +| Method | Description | +| ---------------------------------- | ------------------------------ | +| `read_magnet_raw()` | Raw sensor values (int16) | +| `read_magnet_uT()` | Magnetic field in µT | +| `read_magnet_calibrated_norm()` | Calibrated and normalized data | +| `heading_flat_only()` | Flat compass heading | +| `heading_with_tilt_compensation()` | Tilt-corrected heading | +| `read_temperature_c()` | Read relative temperature | +| `power_down()` / `wake()` | Power management | +| `soft_reset()` / `reboot()` | Sensor reset functions | + +--- + +## 🧪 Example: Continuous Compass Loop + +```python +from machine import I2C, Pin +from lis2mdl import LIS2MDL +import time + +i2c = I2C(1, scl=Pin(22), sda=Pin(21)) +mag = LIS2MDL(i2c) +mag.set_declination(2.3) + +while True: + heading = mag.heading_flat_only() + print("Heading: {:.1f}° {}".format(heading, mag.direction_label(heading))) + time.sleep(0.5) +``` + +--- + +## 📚 References + +* [STMicroelectronics LIS2MDL Datasheet](https://www.st.com/resource/en/datasheet/lis2mdl.pdf) +* [MicroPython Documentation](https://docs.micropython.org/en/latest/library/machine.I2C.html) + +--- \ No newline at end of file diff --git a/lib/lis2mdl/examples/magnet_compass.py b/lib/lis2mdl/examples/magnet_compass.py new file mode 100644 index 00000000..fb2d79c4 --- /dev/null +++ b/lib/lis2mdl/examples/magnet_compass.py @@ -0,0 +1,19 @@ +# Example of heading reading with direction label +from time import sleep_ms +from machine import I2C +from lis2mdl.device import LIS2MDL +from lis2mdl.const import * + +i2c = I2C(1) +mag = LIS2MDL(i2c) + +print("Calibrate the magnetometer by moving it in a figure-eight pattern.") +mag.calibrate_minmax_3d() +print("Calibration complete.") + +while True: + angle = mag.heading_flat_only() + + direction = mag.direction_label(angle) + print("Cap:", angle, "°", "-", direction) + sleep_ms(100) diff --git a/lib/lis2mdl/examples/magnet_fieldForce.py b/lib/lis2mdl/examples/magnet_fieldForce.py new file mode 100644 index 00000000..9e8bbabf --- /dev/null +++ b/lib/lis2mdl/examples/magnet_fieldForce.py @@ -0,0 +1,17 @@ +# Example of heading reading with direction label +from time import sleep_ms +from machine import I2C +from lis2mdl.device import LIS2MDL +from lis2mdl.const import * + +i2c = I2C(1) +mag = LIS2MDL(i2c) + +print("Calibrate the magnetometer by moving it in a figure-eight pattern.") +mag.calibrate_minmax_3d() +print("Calibration complete.") + +while True: + field_strength = mag.magnitude_uT() + print("Champ magnétique :", field_strength, "µT") + sleep_ms(100) diff --git a/lib/lis2mdl/examples/magnet_test.py b/lib/lis2mdl/examples/magnet_test.py new file mode 100644 index 00000000..875b2800 --- /dev/null +++ b/lib/lis2mdl/examples/magnet_test.py @@ -0,0 +1,677 @@ +from time import sleep_ms +from machine import I2C +from lis2mdl.device import LIS2MDL +from lis2mdl.const import * +import math + +# Définition des constantes pour remplacer les valeurs magiques +MAGNETIC_FIELD_MIN = 5.0 +MAGNETIC_FIELD_MAX = 200.0 +TEMP_MIN = -100.0 +TEMP_MAX = 150.0 +CENTER_TOLERANCE = 0.2 +ROUND_TOLERANCE = 1.4 +CENTER_TOLERANCE_3D = 0.3 +ROUND_TOLERANCE_3D = 1.6 +ANGLE_DIFF_MIN = 14.0 +ANGLE_DIFF_MAX = 20.0 +ANGLE_DIFF_WRAP_MIN = 340.0 +ANGLE_DIFF_WRAP_MAX = 346.0 +SPAN_MIN = 300.0 +FILTER_DIFF_MAX = 90.0 + + +def _bits(v, hi, lo): + m = (1 << (hi - lo + 1)) - 1 + return (v >> lo) & m + + +def test_sets(dev): + ok = True + + # --- MODE --- + dev.set_mode("continuous") + r = dev.read_reg(LIS2MDL_CFG_REG_A) + exp = 0b00 + print( + "set_mode(continuous): MD=", + _bits(r, 1, 0), + "expected", + exp, + "=>", + "OK" if _bits(r, 1, 0) == exp else "FAIL", + ) + ok &= _bits(r, 1, 0) == exp + + dev.set_mode("single") + r = dev.read_reg(LIS2MDL_CFG_REG_A) + exp = 0b01 + print( + "set_mode(single): MD=", + _bits(r, 1, 0), + "expected", + exp, + "=>", + "OK" if _bits(r, 1, 0) == exp else "FAIL", + ) + ok &= _bits(r, 1, 0) == exp + + dev.set_mode("idle") + r = dev.read_reg(LIS2MDL_CFG_REG_A) + exp = 0b11 + print( + "set_mode(idle): MD=", + _bits(r, 1, 0), + "expected", + exp, + "=>", + "OK" if _bits(r, 1, 0) == exp else "FAIL", + ) + ok &= _bits(r, 1, 0) == exp + + # --- ODR --- + dev.set_odr(50) + r = dev.read_reg(LIS2MDL_CFG_REG_A) + exp = 0b10 + print( + "set_odr(50): ODR=", + _bits(r, 3, 2), + "expected", + exp, + "=>", + "OK" if _bits(r, 3, 2) == exp else "FAIL", + ) + ok &= _bits(r, 3, 2) == exp + + dev.set_odr(100) + r = dev.read_reg(LIS2MDL_CFG_REG_A) + exp = 0b11 + print( + "set_odr(100): ODR=", + _bits(r, 3, 2), + "expected", + exp, + "=>", + "OK" if _bits(r, 3, 2) == exp else "FAIL", + ) + ok &= _bits(r, 3, 2) == exp + + # --- Low power --- + dev.set_low_power(True) + r = dev.read_reg(LIS2MDL_CFG_REG_A) + print( + "set_low_power(True): LP=", + (r >> 4) & 1, + "expected 1 =>", + "OK" if ((r >> 4) & 1) == 1 else "FAIL", + ) + ok &= ((r >> 4) & 1) == 1 + dev.set_low_power(False) + r = dev.read_reg(LIS2MDL_CFG_REG_A) + print( + "set_low_power(False): LP=", + (r >> 4) & 1, + "expected 0 =>", + "OK" if ((r >> 4) & 1) == 0 else "FAIL", + ) + ok &= ((r >> 4) & 1) == 0 + + # --- LPF --- + dev.set_low_pass(True) + r = dev.read_reg(LIS2MDL_CFG_REG_B) + print( + "set_low_pass(True): LPF=", + r & 1, + "expected 1 =>", + "OK" if (r & 1) == 1 else "FAIL", + ) + ok &= (r & 1) == 1 + dev.set_low_pass(False) + r = dev.read_reg(LIS2MDL_CFG_REG_B) + print( + "set_low_pass(False): LPF=", + r & 1, + "expected 0 =>", + "OK" if (r & 1) == 0 else "FAIL", + ) + ok &= (r & 1) == 0 + + # --- Offset cancellation --- + dev.set_offset_cancellation(True, oneshot=False) + r = dev.read_reg(LIS2MDL_CFG_REG_B) + print( + "set_offset_cancellation(True,False): OFF_CANC(bit1)=", + (r >> 1) & 1, + "ONE_SHOT(bit4)=", + (r >> 4) & 1, + "expected 1,0 =>", + "OK" if ((r >> 1) & 1) == 1 and ((r >> 4) & 1) == 0 else "FAIL", + ) + ok &= ((r >> 1) & 1) == 1 and ((r >> 4) & 1) == 0 + + dev.set_offset_cancellation(True, oneshot=True) + r = dev.read_reg(LIS2MDL_CFG_REG_B) + print( + "set_offset_cancellation(True,True): OFF_CANC(bit1)=", + (r >> 1) & 1, + "ONE_SHOT(bit4)=", + (r >> 4) & 1, + "expected 1,1 =>", + "OK" if ((r >> 1) & 1) == 1 and ((r >> 4) & 1) == 1 else "FAIL", + ) + ok &= ((r >> 1) & 1) == 1 and ((r >> 4) & 1) == 1 + + # --- BDU / Endianness / SPI4 --- + dev.set_bdu(True) + r = dev.read_reg(LIS2MDL_CFG_REG_C) + print( + "set_bdu(True): BDU(bit4)=", + (r >> 4) & 1, + "expected 1 =>", + "OK" if ((r >> 4) & 1) == 1 else "FAIL", + ) + ok &= ((r >> 4) & 1) == 1 + + dev.set_endianness(True) + r = dev.read_reg(LIS2MDL_CFG_REG_C) + print( + "set_endianness(True): BLE(bit3)=", + (r >> 3) & 1, + "expected 1 =>", + "OK" if ((r >> 3) & 1) == 1 else "FAIL", + ) + ok &= ((r >> 3) & 1) == 1 + + dev.use_spi_4wire(True) + r = dev.read_reg(LIS2MDL_CFG_REG_C) + print( + "use_spi_4wire(True): 4WSPI(bit2)=", + (r >> 2) & 1, + "expected 1 =>", + "OK" if ((r >> 2) & 1) == 1 else "FAIL", + ) + ok &= ((r >> 2) & 1) == 1 + + # --- Software offsets / declination --- + dev.set_heading_offset(15.0) + dev.set_declination(2.0) + # Instant flat measurement: the angle should increase by ~17° compared to your raw calculation + ang1 = ( + dev.heading_flat_only() + ) # (remember to add offset+declination in your method) + print( + "heading_flat_only with offset+declination: angle≈raw+17° (check visually) =>", + f"{ang1:.2f}°", + ) + + # --- set_calibrate_step / set_hw_offsets --- + # Apply a dummy calibration, then verify the read-back fields + dev.set_calibrate_step(10, -20, 30, 300, 300, 300) + xoff, yoff, zoff, xs, ys, zs = dev.read_calibration() + print( + "set_calibrate_step(...): applied offsets/scales =>", + (xoff, yoff, zoff, xs, ys, zs), + ) + + # If you want to push the correction into the sensor: + dev.set_hw_offsets(0, 0, 0) # e.g., reset to 0 + # You can read the registers to verify (optional) + oxL = dev.read_reg(LIS2MDL_OFFSET_X_REG_L) + oxH = dev.read_reg(LIS2MDL_OFFSET_X_REG_L + 1) + print( + "set_hw_offsets(...): OFFSET_X* =", (oxH << 8) | oxL, "expected written value" + ) + + print("\n=== Overall summary:", "OK" if ok else "Some tests FAIL ===") + + +def _approx_equal(a, b, tol): + return abs(a - b) <= tol + + +def test_reads(dev): + ok = True + print("\n=== TEST READS ===") + + # WHO_AM_I + who = dev.read_who_am_i() + print(f"WHO_AM_I=0x{who:02X} expected 0x40 =>", "OK" if who == 0x40 else "FAIL") + ok &= who == 0x40 + + # STATUS & DATA READY + st1 = dev.read_status() + print(f"Initial STATUS=0x{st1:02X}") + # wait a few ms to let a new frame arrive + sleep_ms(50) + ready = dev.data_ready() + print("data_ready():", ready, "=>", "OK" if isinstance(ready, bool) else "FAIL") + ok &= isinstance(ready, bool) + + # MAG RAW + xr, yr, zr = dev.read_magnet_raw() + print( + f"read_magnet_raw: (X,Y,Z)=({xr},{yr},{zr}) LSB =>", + "OK" if all(isinstance(v, int) for v in (xr, yr, zr)) else "FAIL", + ) + ok &= all(isinstance(v, int) for v in (xr, yr, zr)) + + # MAG µT vs RAW + xu, yu, zu = dev.read_magnet_uT() + print(f"read_magnet_uT: (X,Y,Z)=({xu:.2f},{yu:.2f},{zu:.2f}) µT") + # check consistency of conversion µT ≈ raw*0.15 + ok_conv = ( + _approx_equal(xu, xr * 0.15, 0.5) + and _approx_equal(yu, yr * 0.15, 0.5) + and _approx_equal(zu, zr * 0.15, 0.5) + ) + print("Conversion µT vs RAW*0.15 =>", "OK" if ok_conv else "FAIL") + ok &= ok_conv + + # MAGNITUDE + B = dev.magnitude_uT() + print( + f"magnitude_uT: |B|={B:.1f} µT (Earth ~25-65 µT). =>", + "OK" if MAGNETIC_FIELD_MIN <= B <= MAGNETIC_FIELD_MAX else "FAIL", + ) + ok &= MAGNETIC_FIELD_MIN <= B <= MAGNETIC_FIELD_MAX # wide, since local disturbances are possible + + # CALIBRATION NORM + xc, yc, zc = dev.read_magnet_calibrated_norm() + print(f"read_magnet_calibrated_norm: ({xc:.3f},{yc:.3f},{zc:.3f})") + # expected: magnitudes ~[-2..+2] after simple calibration + ok_cal_rng = abs(xc) < 5 and abs(yc) < 5 and abs(zc) < 5 + print("Calibration norm (|val|<5) =>", "OK" if ok_cal_rng else "WARN") + ok &= ok_cal_rng + + # TEMP + t1 = dev.read_temperature_c() + sleep_ms(50) + t2 = dev.read_temperature_c() + print(f"TempC: t1={t1:.2f}°C, t2={t2:.2f}°C (8 LSB/°C, absolute offset unknown)") + # test: type & broad plausible range + ok_temp = ( + isinstance(t1, float) + and isinstance(t2, float) + and (TEMP_MIN < t1 < TEMP_MAX) + and (TEMP_MIN < t2 < TEMP_MAX) + ) + print("Temp check =>", "OK" if ok_temp else "FAIL") + ok &= ok_temp + + # INT SOURCE (without IT config, should be 0) + ints = dev.read_int_source() + print( + f"INT_SOURCE=0x{ints:02X} (often 0 if no interrupt configured) =>", + "OK" if isinstance(ints, int) else "FAIL", + ) + ok &= isinstance(ints, int) + + # REGISTER DUMP (sanity) + dump = dev.read_registers(LIS2MDL_CFG_REG_A, 8) # A..H ~ 0x60..0x67 + print( + f"Dump 0x60..0x67: {dump} =>", + "OK" if isinstance(dump, (bytes, bytearray)) and len(dump) == 8 else "FAIL", + ) + ok &= isinstance(dump, (bytes, bytearray)) and len(dump) == 8 + + print("\n=== Overall READS result:", "OK ✅" if ok else "Some checks FAIL ❌") + return ok + + +def _fmt_tuple(t): + return "({:.3f},{:.3f})".format(t[0], t[1]) + + +def test_calibrate_2d(dev): + print("\n=== 2D CALIBRATION (flat, 360° rotation) ===") + print("Rotate the board FLAT for ~{} samples...".format(300)) + dev.calibrate_minmax_2d(samples=300, delay_ms=20) + xoff, yoff, _, xs, ys, _ = ( + dev.x_off, + dev.y_off, + dev.z_off, + dev.x_scale, + dev.y_scale, + dev.z_scale, + ) + print("XY Offsets:", xoff, yoff, " XY Scales:", xs, ys) + + # quality + print("Quick check (move a bit while flat during capture)...") + q = dev.calibrate_quality(samples_check=200, delay_ms=10) + print("mean_xy =", _fmt_tuple(q["mean_xy"]), " (expected close to 0,0)") + print( + "anisotropy_xy =", + "{:.2f}".format(q["anisotropy_xy"]), + " (≈1.0 if circle is nicely round)", + ) + print("r_std_xy =", "{:.3f}".format(q["r_std_xy"]), " (smaller = better)") + + ok_center = abs(q["mean_xy"][0]) < CENTER_TOLERANCE and abs(q["mean_xy"][1]) < CENTER_TOLERANCE + ok_round = q["anisotropy_xy"] < ROUND_TOLERANCE # realistic tolerances + print("=> Center close to 0 :", "OK" if ok_center else "WARN") + print("=> Circle ≈ round :", "OK" if ok_round else "WARN") + return ok_center and ok_round + + +def test_calibrate_3d(dev): + print("\n=== 3D CALIBRATION (all orientations) ===") + print("Rotate the board IN ALL DIRECTIONS for ~{} samples...".format(600)) + dev.calibrate_minmax_3d(samples=600, delay_ms=20) + print("Offsets:", dev.x_off, dev.y_off, dev.z_off) + print("Scales :", dev.x_scale, dev.y_scale, dev.z_scale) + + q = dev.calibrate_quality(samples_check=200, delay_ms=10) + print( + "mean_xy =", _fmt_tuple(q["mean_xy"]), " mean_z = {:.3f}".format(q["mean_z"]) + ) + print( + "std_xy = ({:.3f},{:.3f}) std_z = {:.3f}".format( + q["std_xy"][0], q["std_xy"][1], q["std_z"] + ) + ) + print("anisotropy_xy =", "{:.2f}".format(q["anisotropy_xy"])) + print( + "r_mean_xy =", + "{:.3f}".format(q["r_mean_xy"]), + " r_std_xy =", + "{:.3f}".format(q["r_std_xy"]), + ) + + ok_center = abs(q["mean_xy"][0]) < CENTER_TOLERANCE_3D and abs(q["mean_xy"][1]) < CENTER_TOLERANCE_3D + ok_round = q["anisotropy_xy"] < ROUND_TOLERANCE_3D + print("=> Center close to 0 :", "OK" if ok_center else "WARN") + print("=> Circle ≈ round :", "OK" if ok_round else "WARN") + return ok_center and ok_round + + +def test_heading_after_calib(dev, n=200, delay_ms=20): + """ + Verify that the angle moves from 0..360° when rotating flat. + (qualitative test: we look at the span of angles) + """ + print("\n=== HEADING after calibration (qualitative) ===") + angles = [] + for _ in range(n): + ang = dev.heading_flat_only() # make sure you have atan2(y, x) inside + angles.append(ang) + sleep_ms(delay_ms) + minA = min(angles) + maxA = max(angles) + span = (maxA - minA) % 360.0 + print("Angle min={:.1f}°, max={:.1f}°, span~{:.1f}°".format(minA, maxA, span)) + print("=> If you rotated ~one complete turn flat, we expect ~300-360° span.") + + +def run_all_calibration_tests(dev): + ok2d = test_calibrate_2d(dev) + test_heading_after_calib(dev) + ok3d = test_calibrate_3d(dev) + print( + "\n=== Calibration summary:", + "OK ✅" if (ok2d and ok3d) else "Partial ⚠️ (see WARN/notes)", + ) + + +def test_heading_flat_basic(dev, n=10, delay_ms=50): + print("\n=== TEST heading_flat_only (basic reading) ===") + dev.set_heading_filter(0.0) # no filter + angles = [] + for _ in range(n): + a = dev.heading_flat_only() + print(f"angle={a:.2f}° dir={dev.direction_label(a)}") + angles.append(a) + sleep_ms(delay_ms) + ok_types = all(isinstance(a, float) for a in angles) + print("Float types =>", "OK" if ok_types else "FAIL") + return ok_types + + +def test_heading_offset_declination(dev): + print("\n=== TEST offset + declination ===") + dev.set_heading_filter(0.0) + # reference angle without corrections + dev.set_heading_offset(0.0) + dev.set_declination(0.0) + a0 = dev.heading_flat_only() + # apply +15° offset +2° declination + dev.set_heading_offset(15.0) + dev.set_declination(2.0) + a1 = dev.heading_flat_only() + # difference mod 360 + diff = (a1 - a0) % 360.0 + # accept ~17° ±3° (due to noise/quantization/filtering) + ok = (ANGLE_DIFF_MIN <= diff <= ANGLE_DIFF_MAX) or (ANGLE_DIFF_WRAP_MIN <= diff <= ANGLE_DIFF_WRAP_MAX) # wrap + print( + f"angle0={a0:.2f}°, angle1={a1:.2f}°, diff≈{diff:.2f}° =>", + "OK" if ok else "FAIL", + ) + return ok + + +def test_heading_span_turn(dev, duration_ms=6000, step_ms=50): + """ + Rotate the board FLAT in roughly one turn for ~duration. + We check that the angle sweeps ~300..360°. + """ + print("\n=== TEST SPAN (Do one turn on table) ===") + dev.set_heading_filter(0.2) # gentle smoothing + dev.set_heading_offset(0.0) + dev.set_declination(0.0) + angles = [] + t = 0 + while t < duration_ms: + a = dev.heading_flat_only() + angles.append(a) + sleep_ms(step_ms) + t += step_ms + minA = min(angles) + maxA = max(angles) + # span modulo 360 (handles wrap) + span = maxA - minA + if span < 0: + span += 360.0 + print(f"min={minA:.1f}°, max={maxA:.1f}°, span≈{span:.1f}°") + ok = span > SPAN_MIN # we expect almost 360° for a full turn + print("SPAN =>", "OK" if ok else "WARN (do a more complete/slower turn)") + return ok + + +def test_heading_filter_wrap(dev): + """ + Synthetic test of the vector filter around 0/360°. + We inject angles near 360->0 and verify there's no 'jump'. + """ + print("\n=== TEST filter & wrap ===") + dev.set_heading_filter(0.3) + dev._hf_cos = None + dev._hf_sin = None # reset filter + # sequence near 360° then 0° + seq = [350, 355, 0, 5, 10] + outs = [] + # we 'fake' a reading by forcing heading_from_vectors with synthetic vectors + + for ang in seq: + # unit vectors in the XY plane + x = math.cos(math.radians(ang)) + y = math.sin(math.radians(ang)) + out = dev.heading_from_vectors(x, y, 0, calibrated=False) + outs.append(out) + print(f"in={ang:>3}° -> out_filt={out:>6.2f}°") + # Check that the output is monotonically increasing (no jump around ~180°) + ok = all((outs[i] - outs[i - 1]) % 360.0 < FILTER_DIFF_MAX for i in range(1, len(outs))) + print("Wrap-safe filter =>", "OK" if ok else "FAIL") + return ok + + +def run_heading_tests(dev): + all_ok = True + all_ok &= test_heading_flat_basic(dev) + all_ok &= test_heading_offset_declination(dev) + # Run this if you can rotate the board flat: + all_ok &= test_heading_span_turn(dev) + all_ok &= test_heading_filter_wrap(dev) + print("\n=== HEADING summary:", "OK ✅" if all_ok else "Partial ⚠️ (see details)") + +def test_power_modes(dev): + print("\n=== TEST POWER MODES ===") + ok = True + + # Wake in continuous + dev.wake("continuous") + r = dev.read_reg(LIS2MDL_CFG_REG_A) + md = _bits(r, 1, 0) + print( + "wake('continuous') => MD=", + md, + "expected 0b00 =>", + "OK" if md == 0b00 else "FAIL", + ) + ok &= md == 0b00 + + # Wake in single + dev.wake("single") + r = dev.read_reg(LIS2MDL_CFG_REG_A) + md = _bits(r, 1, 0) + print( + "wake('single') => MD=", + md, + "expected 0b01 =>", + "OK" if md == 0b01 else "FAIL", + ) + ok &= md == 0b01 + + # Power down + dev.power_down() + r = dev.read_reg(LIS2MDL_CFG_REG_A) + md = _bits(r, 1, 0) + print( + "power_down() => MD=", + md, + "expected 0b11 =>", + "OK" if md == 0b11 else "FAIL", + ) + ok &= md == 0b11 + print( + "is_idle():", + dev.is_idle(), + "expected True =>", + "OK" if dev.is_idle() else "FAIL", + ) + ok &= dev.is_idle() + + # Back to continuous + dev.wake("continuous") + r = dev.read_reg(LIS2MDL_CFG_REG_A) + md = _bits(r, 1, 0) + print( + "wake('continuous') => MD=", + md, + "expected 0b00 =>", + "OK" if md == 0b00 else "FAIL", + ) + ok &= md == 0b00 + + return ok + + +def test_soft_reset(dev): + print("\n=== TEST SOFT RESET ===") + ok = True + + # Put into a non-default state + dev.set_odr(100) # ODR bits = 11 + dev.set_low_power(True) # LP bit4 = 1 + dev.set_low_pass(True) # CFG_B bit0 = 1 + dev.set_bdu(True) # CFG_C bit4 = 1 + + ra_before = dev.read_reg(LIS2MDL_CFG_REG_A) + rb_before = dev.read_reg(LIS2MDL_CFG_REG_B) + rc_before = dev.read_reg(LIS2MDL_CFG_REG_C) + print( + f"Before reset: CFG_A=0x{ra_before:02X} CFG_B=0x{rb_before:02X} CFG_C=0x{rc_before:02X}" + ) + + # Soft reset + dev.soft_reset(wait_ms=15) + + # Read after reset + ra = dev.read_reg(LIS2MDL_CFG_REG_A) + rb = dev.read_reg(LIS2MDL_CFG_REG_B) + rc = dev.read_reg(LIS2MDL_CFG_REG_C) + print(f"After reset: CFG_A=0x{ra:02X} CFG_B=0x{rb:02X} CFG_C=0x{rc:02X}") + + # Realistic expectations (typical default values): + # - MD (bits1..0) = 11 (idle) + # - ODR (bits3..2) = 00 + # - LP (bit4) = 0 + # - CFG_B.LPF (bit0) = 0 + # - CFG_C.BDU (bit4) = 0 + md_ok = _bits(ra, 1, 0) == 0b11 + odr_ok = _bits(ra, 3, 2) == 0b00 + lp_ok = ((ra >> 4) & 1) == 0 + lpf_ok = (rb & 1) == 0 + bdu_ok = ((rc >> 4) & 1) == 0 + + print("MD=idle (11) =>", "OK" if md_ok else "FAIL") + ok &= md_ok + print("ODR=00 =>", "OK" if odr_ok else "FAIL") + ok &= odr_ok + print("LP=0 =>", "OK" if lp_ok else "FAIL") + ok &= lp_ok + print("LPF=0 =>", "OK" if lpf_ok else "FAIL") + ok &= lpf_ok + print("BDU=0 =>", "OK" if bdu_ok else "FAIL") + ok &= bdu_ok + + # Verify that the SOFT_RST bit has cleared back to 0 (auto-clear) + soft_rst_cleared = ((ra >> 5) & 1) == 0 + print("SOFT_RST auto-clear =>", "OK" if soft_rst_cleared else "FAIL") + ok &= soft_rst_cleared + + return ok + + +def test_reboot(dev): + print("\n=== TEST REBOOT ===") + ok = True + + # Put into a known state + dev.set_odr(20) # ODR=01 + ra_before = dev.read_reg(LIS2MDL_CFG_REG_A) + print(f"Before reboot: CFG_A=0x{ra_before:02X}") + + # Reboot + dev.reboot(wait_ms=15) + ra = dev.read_reg(LIS2MDL_CFG_REG_A) + print(f"After reboot: CFG_A=0x{ra:02X}") + + # The REBOOT bit (bit6) must have cleared back to 0 + reboot_cleared = ((ra >> 6) & 1) == 0 + print("REBOOT auto-clear =>", "OK" if reboot_cleared else "FAIL") + ok &= reboot_cleared + + # WHO_AM_I still correct + who = dev.read_who_am_i() + print(f"WHO_AM_I=0x{who:02X} expected 0x40 =>", "OK" if who == 0x40 else "FAIL") + ok &= who == 0x40 + + return ok + + +def run_power_reset_tests(dev): + all_ok = True + all_ok &= test_power_modes(dev) + all_ok &= test_soft_reset(dev) + all_ok &= test_reboot(dev) + print("\n=== POWER/RESET summary:", "OK ✅" if all_ok else "Partial ⚠️ (see logs)") + + +# ---- Run the tests ---- +i2c = I2C(1) +dev = LIS2MDL(i2c) +test_reads(dev) +test_sets(dev) +run_all_calibration_tests(dev) +run_heading_tests(dev) +run_power_reset_tests(dev) diff --git a/lib/lis2mdl/lis2mdl/__init__.py b/lib/lis2mdl/lis2mdl/__init__.py new file mode 100644 index 00000000..9b853c94 --- /dev/null +++ b/lib/lis2mdl/lis2mdl/__init__.py @@ -0,0 +1,5 @@ +from lis2mdl.device import LIS2MDL + +__all__ = [ + "LIS2MDL", +] diff --git a/lib/lis2mdl/lis2mdl/const.py b/lib/lis2mdl/lis2mdl/const.py new file mode 100644 index 00000000..c8150d01 --- /dev/null +++ b/lib/lis2mdl/lis2mdl/const.py @@ -0,0 +1,37 @@ +from micropython import const + +# LIS2MDL I2C address +LIS2MDL_I2C_ADDR = const(0x1E) + +# Register addresses +LIS2MDL_WHO_AM_I = const(0x4F) # Device identification register +LIS2MDL_CFG_REG_A = const(0x60) # Configuration register A +LIS2MDL_CFG_REG_B = const(0x61) # Configuration register B +LIS2MDL_CFG_REG_C = const(0x62) # Configuration register C +LIS2MDL_STATUS_REG = const(0x67) # Status register + +# Output data registers +LIS2MDL_OUTX_L_REG = const(0x68) # X-axis output low byte +LIS2MDL_OUTX_H_REG = const(0x69) # X-axis output high byte +LIS2MDL_OUTY_L_REG = const(0x6A) # Y-axis output low byte +LIS2MDL_OUTY_H_REG = const(0x6B) # Y-axis output high byte +LIS2MDL_OUTZ_L_REG = const(0x6C) # Z-axis output low byte +LIS2MDL_OUTZ_H_REG = const(0x6D) # Z-axis output high byte + +# Offset registers +LIS2MDL_OFFSET_X_REG_L = const(0x45) # X-axis offset low byte +LIS2MDL_OFFSET_X_REG_H = const(0x46) # X-axis offset high byte +LIS2MDL_OFFSET_Y_REG_L = const(0x47) # Y-axis offset low byte +LIS2MDL_OFFSET_Y_REG_H = const(0x48) # Y-axis offset high byte +LIS2MDL_OFFSET_Z_REG_L = const(0x49) # Z-axis offset low byte +LIS2MDL_OFFSET_Z_REG_H = const(0x4A) # Z-axis offset high byte + +# Temperature output registers +LIS2MDL_TEMP_OUT_L_REG = const(0x6E) # Temperature output low byte +LIS2MDL_TEMP_OUT_H_REG = const(0x6F) # Temperature output high byte + +# Interrupt control and source registers +LIS2MDL_INT_CTRL_REG = const(0x63) # Interrupt control register +LIS2MDL_INT_SOURCE_REG = const(0x64) # Interrupt source register +LIS2MDL_INT_THS_L_REG = const(0x65) # Interrupt threshold low byte +LIS2MDL_INT_THS_H_REG = const(0x66) # Interrupt threshold high byte diff --git a/lib/lis2mdl/lis2mdl/device.py b/lib/lis2mdl/lis2mdl/device.py new file mode 100644 index 00000000..04b5e019 --- /dev/null +++ b/lib/lis2mdl/lis2mdl/device.py @@ -0,0 +1,577 @@ +# device.py +# This driver is for the LIS2MDL magnetometer sensor. It provides methods for initialization, calibration, and reading magnetic field data. + +from machine import I2C +from lis2mdl.const import * +from time import sleep_ms +import math + + +class LIS2MDL(object): + # Default calibration offsets and scales for the magnetometer + x_off = 0 + y_off = 0 + z_off = 0 + + x_scale = 1 + y_scale = 1 + z_scale = 1 + + def __init__( + self, + i2c, + address=LIS2MDL_I2C_ADDR, + odr_hz=10, + temp_comp=True, + low_power=False, + drdy_enable=False, + ): + # Initialize the LIS2MDL sensor with the given I2C interface and settings. + self.i2c = i2c + self.address = address + self.writebuffer = bytearray(1) + self.readbuffer = bytearray(1) + + # Perform a soft reset to ensure the sensor starts in a known state. + self.setReg(0x20, LIS2MDL_CFG_REG_A) # SOFT_RST=1 (not 0x10) + try: + sleep_ms(10) # Small delay for reset to complete + except Exception: + pass + + # Configure the sensor's operating mode, output data rate, and other settings. + odr_bits = {10: 0b00, 20: 0b01, 50: 0b10, 100: 0b11}.get(odr_hz, 0b00) + comp = 1 if temp_comp else 0 + lp = 1 if low_power else 0 + cfg_a = (comp << 7) | (lp << 4) | (odr_bits << 2) | 0b00 + self.setReg(cfg_a, LIS2MDL_CFG_REG_A) # Essential to exit IDLE mode + + # Configure low-pass filter and other optional settings. + self.setReg(0x00, LIS2MDL_CFG_REG_B) # Default: LPF and offset cancellation off + + # Enable block data update and optionally configure the DRDY pin. + cfg_c = 0x10 | (0x01 if drdy_enable else 0x00) + self.setReg(cfg_c, LIS2MDL_CFG_REG_C) + + ## + # --- SET functions --- + ## + + # --- Modes / frequency (CFG_REG_A: 0x60) --- + def set_mode(self, mode: str): + # MD1..MD0: 00=continuous, 01=single, 11=idle + md = {"continuous": 0b00, "single": 0b01, "idle": 0b11}.get(mode, 0b00) + reg = self.read_reg(LIS2MDL_CFG_REG_A) + reg = (reg & ~0b11) | md + self.setReg(reg, LIS2MDL_CFG_REG_A) + + def set_odr(self, hz: int): + # ODR1..0: 00=10Hz, 01=20Hz, 10=50Hz, 11=100Hz + odr_bits = {10: 0b00, 20: 0b01, 50: 0b10, 100: 0b11}.get(hz, 0b00) + reg = self.read_reg(LIS2MDL_CFG_REG_A) + reg = (reg & ~(0b11 << 2)) | (odr_bits << 2) + self.setReg(reg, LIS2MDL_CFG_REG_A) + + def set_low_power(self, enabled: bool): + # LP bit (bit4) : 0=High-Res, 1=Low-Power + reg = self.read_reg(LIS2MDL_CFG_REG_A) + if enabled: + reg |= 1 << 4 + else: + reg &= ~(1 << 4) + self.setReg(reg, LIS2MDL_CFG_REG_A) + + # --- Filters / offset cancellation (CFG_REG_B: 0x61) --- + def set_low_pass(self, enabled: bool): + # LPF (bit0) + reg = self.read_reg(LIS2MDL_CFG_REG_B) + if enabled: + reg |= 1 << 0 + else: + reg &= ~(1 << 0) + self.setReg(reg, LIS2MDL_CFG_REG_B) + + def set_offset_cancellation(self, enabled: bool, oneshot: bool = False): + # OFF_CANC (bit1), OFF_CANC_ONE_SHOT (bit4) + reg = self.read_reg(LIS2MDL_CFG_REG_B) + if enabled: + reg |= 1 << 1 + else: + reg &= ~(1 << 1) + if oneshot: + reg |= 1 << 4 + else: + reg &= ~(1 << 4) + self.setReg(reg, LIS2MDL_CFG_REG_B) + + # --- Interface options / BDU (CFG_REG_C: 0x62) --- + def set_bdu(self, enable=True): + # BDU (bit4) + reg = self.read_reg(LIS2MDL_CFG_REG_C) + if enable: + reg |= 1 << 4 + else: + reg &= ~(1 << 4) + self.setReg(reg, LIS2MDL_CFG_REG_C) + + def set_endianness(self, big_endian: bool): + # BLE (bit3) + reg = self.read_reg(LIS2MDL_CFG_REG_C) + if big_endian: + reg |= 1 << 3 + else: + reg &= ~(1 << 3) + self.setReg(reg, LIS2MDL_CFG_REG_C) + + def use_spi_4wire(self, enable: bool): + # 4WSPI (bit2) + reg = self.read_reg(LIS2MDL_CFG_REG_C) + if enable: + reg |= 1 << 2 + else: + reg &= ~(1 << 2) + self.setReg(reg, LIS2MDL_CFG_REG_C) + + # --- Compass: heading offset & declination (software) --- + _heading_offset_deg = 0.0 # user setting: align your physical 0° + _declination_deg = 0.0 # true north vs magnetic north + + def set_heading_offset(self, deg: float): + self._heading_offset_deg = float(deg) + + def set_declination(self, deg: float): + self._declination_deg = float(deg) + + # (remember to correct your heading_flat_only with atan2(y, x) then + offsets) + + def setReg(self, data, reg): + # Write a byte to a specific register. + self.writebuffer[0] = data + self.i2c.writeto_mem(self.address, reg, self.writebuffer) + + def _write_16(self, reg_l, value): + value &= 0xFFFF + self.setReg(value & 0xFF, reg_l) + self.setReg((value >> 8) & 0xFF, reg_l + 1) + + def set_hw_offsets(self, x: int, y: int, z: int): + # writes to OFFSET_X/Y/Z_REG_L/H + self._write_16(LIS2MDL_OFFSET_X_REG_L, x) + self._write_16(LIS2MDL_OFFSET_Y_REG_L, y) + self._write_16(LIS2MDL_OFFSET_Z_REG_L, z) + + ## + # --- READ functions --- + ## + + def read_magnet_raw(self): + """Reads the raw magnetic field (LSB). Same as read_magnet(), but more explicit.""" + return self.read_magnet() # (x,y,z) int16 LSB + + def read_status(self) -> int: + """Reads STATUS_REG (0x67).""" + return self.read_reg(LIS2MDL_STATUS_REG) + + def data_ready(self) -> bool: + """True if a new XYZ triplet is ready (Zyxda bit).""" + return bool(self.read_status() & (1 << 3)) + + def read_int_source(self) -> int: + """Reads INT_SOURCE_REG (0x64): source of the interrupt.""" + return self.read_reg(LIS2MDL_INT_SOURCE_REG) + + def read_reg(self, reg): + # Read a byte from a specific register. + self.i2c.readfrom_mem_into(self.address, reg, self.readbuffer) + return self.readbuffer[0] + + # --- UNITS, CALIBRATION, MAGNITUDE --- + + _MAG_LSB_TO_uT = 0.15 # 1.5 mG/LSB ≈ 0.15 µT/LSB + + def read_magnet_uT(self): + """Reads the magnetic field in µT, uncalibrated (simple conversion from LSB).""" + x, y, z = self.read_magnet() + return ( + x * self._MAG_LSB_TO_uT, + y * self._MAG_LSB_TO_uT, + z * self._MAG_LSB_TO_uT, + ) + + def read_magnet_calibrated_norm(self): + """Reads the calibrated field (offset/scale per axis), normalized (unitless, ~circle in XY).""" + x, y, z = self.read_magnet() + x = (x - self.x_off) / self.x_scale + y = (y - self.y_off) / self.y_scale + z = (z - self.z_off) / self.z_scale + return (x, y, z) + + def magnitude_uT(self) -> float: + """Total magnetic field strength (µT).""" + x, y, z = self.read_magnet_uT() + return math.sqrt(x * x + y * y + z * z) + + @staticmethod + def _to_int16(v): + # Convert an unsigned 16-bit value to a signed 16-bit value. + return v - 0x10000 if v & 0x8000 else v + + def read_magnet(self): + # Read the raw magnetic field data (X, Y, Z) from the sensor. + buf = self.i2c.readfrom_mem(self.address, LIS2MDL_OUTX_L_REG | 0x80, 6) + x = self._to_int16((buf[1] << 8) | buf[0]) + y = self._to_int16((buf[3] << 8) | buf[2]) + z = self._to_int16((buf[5] << 8) | buf[4]) + return (x, y, z) + + # --- TEMPERATURE --- + + def read_temperature_raw(self) -> int: + """Reads the raw temperature (LSB), 8 LSB/°C, absolute offset not guaranteed.""" + lo = self.read_reg(LIS2MDL_TEMP_OUT_L_REG) + hi = self.read_reg(LIS2MDL_TEMP_OUT_H_REG) + v = (hi << 8) | lo + return v - 0x10000 if (v & 0x8000) else v + + def read_temperature_c(self) -> float: + """Relative temperature in °C (8 LSB/°C). Warning: absolute offset not specified.""" + return self.read_temperature_raw() / 8.0 + + # --- IDENTITY & HARDWARE OFFSETS --- + + def read_who_am_i(self) -> int: + """Reads WHO_AM_I (should be 0x40).""" + return self.read_reg(LIS2MDL_WHO_AM_I) + + def _read_16(self, reg_l) -> int: + """Reads a signed 16-bit value from a _L (LSB) register.""" + lo = self.read_reg(reg_l) + hi = self.read_reg(reg_l + 1) + v = (hi << 8) | lo + return v - 0x10000 if (v & 0x8000) else v + + def read_hw_offsets(self): + """Reads the hardware offsets (OFFSET_* registers).""" + ox = self._read_16(LIS2MDL_OFFSET_X_REG_L) + oy = self._read_16(LIS2MDL_OFFSET_Y_REG_L) + oz = self._read_16(LIS2MDL_OFFSET_Z_REG_L) + return (ox, oy, oz) + + def read_calibration(self): + # Return the current calibration offsets and scales. + return ( + self.x_off, + self.y_off, + self.z_off, + self.x_scale, + self.y_scale, + self.z_scale, + ) + + # --- DIAGNOSTIC / DUMP --- + + def read_registers(self, start_addr: int, length: int) -> bytes: + """Dump of consecutive registers (useful for debugging).""" + return self.i2c.readfrom_mem(self.address, start_addr, length) + + def read_all(self) -> dict: + """Grouped reading useful for debug & logs.""" + raw = self.read_magnet_raw() + uT = self.read_magnet_uT() + cal = self.read_magnet_calibrated_norm() + T = self.read_temperature_c() + st = self.read_status() + return {"raw": raw, "uT": uT, "cal_norm": cal, "tempC": T, "status": st} + + ## + # --- CALIBRATIONS --- + ## + + def set_calibrate_step(self, xoff, yoff, zoff, xscale, yscale, zscale): + # Set the calibration offsets and scales manually. + self.x_off = xoff + self.y_off = yoff + self.z_off = zoff + self.x_scale = xscale + self.y_scale = yscale + self.z_scale = zscale + + def calibrate_minmax_2d(self, samples=300, delay_ms=20): + """ + MIN/MAX calibration while flat (XY only). + Slowly rotate the board FLAT during acquisition. + Updates x_off, y_off, x_scale, y_scale (leaves Z unchanged). + """ + xmin = ymin = 1e9 + xmax = ymax = -1e9 + + for _ in range(samples): + x, y, _ = self.read_magnet() + xmin = min(xmin, x) + xmax = max(xmax, x) + ymin = min(ymin, y) + ymax = max(ymax, y) + sleep_ms(delay_ms) + + self.x_off = (xmax + xmin) / 2.0 + self.y_off = (ymax + ymin) / 2.0 + self.x_scale = (xmax - xmin) / 2.0 or 1.0 + self.y_scale = (ymax - ymin) / 2.0 or 1.0 + # Option: normalize XY to the same average radius for a better 2D compass + avg = (self.x_scale + self.y_scale) / 2.0 + self.x_scale = avg if self.x_scale != 0 else 1.0 + self.y_scale = avg if self.y_scale != 0 else 1.0 + + def calibrate_minmax_3d(self, samples=600, delay_ms=20): + """ + MIN/MAX calibration on 3 axes (rotate the board in ALL directions). + Updates offsets + scales for X, Y, Z. + """ + xmin = ymin = zmin = 1e9 + xmax = ymax = zmax = -1e9 + + for _ in range(samples): + x, y, z = self.read_magnet() + xmin = min(xmin, x) + xmax = max(xmax, x) + ymin = min(ymin, y) + ymax = max(ymax, y) + zmin = min(zmin, z) + zmax = max(zmax, z) + sleep_ms(delay_ms) + + self.x_off = (xmax + xmin) / 2.0 + self.y_off = (ymax + ymin) / 2.0 + self.z_off = (zmax + zmin) / 2.0 + + self.x_scale = (xmax - xmin) / 2.0 or 1.0 + self.y_scale = (ymax - ymin) / 2.0 or 1.0 + self.z_scale = (zmax - zmin) / 2.0 or 1.0 + + def calibrate_apply(self, x, y, z): + """ + Applies the current calibration (offset + scale per axis). + Returns normalized ~unitless values. + """ + xc = (x - self.x_off) / (self.x_scale or 1.0) + yc = (y - self.y_off) / (self.y_scale or 1.0) + zc = (z - self.z_off) / (self.z_scale or 1.0) + return xc, yc, zc + + def calibrate_quality(self, samples_check=200, delay_ms=10): + """ + Evaluates the quality of the current calibration over a short sample. + Returns a dict with useful metrics: center (mean), anisotropy, XY radius dispersion. + (Move the board a bit while flat during the measurement.) + """ + xs = [] + ys = [] + zs = [] + for _ in range(samples_check): + x, y, z = self.read_magnet() + xc, yc, zc = self.calibrate_apply(x, y, z) + xs.append(xc) + ys.append(yc) + zs.append(zc) + sleep_ms(delay_ms) + + # Means (residual center) + mx = sum(xs) / len(xs) + my = sum(ys) / len(ys) + mz = sum(zs) / len(zs) + + # Radius dispersion in the XY plane + + radii = [math.sqrt(x * x + y * y) for x, y in zip(xs, ys)] + r_mean = sum(radii) / len(radii) + r_var = sum((r - r_mean) ** 2 for r in radii) / len(radii) + r_std = math.sqrt(r_var) + + # Simple anisotropy via standard deviations per axis + def _std(arr, mean): + v = sum((a - mean) ** 2 for a in arr) / len(arr) + return math.sqrt(v) + + sx = _std(xs, mx) + sy = _std(ys, my) + sz = _std(zs, mz) + aniso_xy = max(sx, sy) / (min(sx, sy) + 1e-9) + + return { + "mean_xy": (mx, my), + "mean_z": mz, + "std_xy": (sx, sy), + "std_z": sz, + "r_mean_xy": r_mean, + "r_std_xy": r_std, + "anisotropy_xy": aniso_xy, # ideal ≈ 1.0 + } + + def calibrate_reset(self): + """Resets to a 'neutral' calibration (useful before re-calibrating).""" + self.x_off = self.y_off = self.z_off = 0.0 + self.x_scale = self.y_scale = self.z_scale = 1.0 + + def calibrate_step(self): + # Simple alias to calibrate_minmax_3d + return self.calibrate_minmax_3d() + + ## + # --- Heading functions --- + ## + + # angle filter via vector averaging (robust around 0/360) + _hf_alpha = 0.0 + _hf_cos = None + _hf_sin = None + + def set_heading_filter(self, alpha: float): + """ + alpha=0 -> no filtering. 0.1..0.3 = light/medium smoothing. + Filter by averaging cos/sin to avoid artifacts at 0/360°. + """ + self._hf_alpha = max(0.0, min(1.0, alpha)) + self._hf_cos = None + self._hf_sin = None + + @staticmethod + def _normalize_deg(a): + a = a % 360.0 + return a if a >= 0 else a + 360.0 + + def _apply_heading_offsets(self, angle_deg): + angle_deg = angle_deg + self._heading_offset_deg + self._declination_deg + return self._normalize_deg(angle_deg) + + def _filter_heading(self, angle_deg): + """Filters the angle via vector averaging; returns filtered angle (or raw if alpha=0).""" + if self._hf_alpha <= 0.0: + return angle_deg + + c = math.cos(math.radians(angle_deg)) + s = math.sin(math.radians(angle_deg)) + if self._hf_cos is None or self._hf_sin is None: + self._hf_cos, self._hf_sin = c, s + else: + a = self._hf_alpha + self._hf_cos = (1.0 - a) * self._hf_cos + a * c + self._hf_sin = (1.0 - a) * self._hf_sin + a * s + # light normalization to avoid amplitude drift + NORMALIZATION_THRESHOLD = 1e-6 + norm = math.sqrt(self._hf_cos * self._hf_cos + self._hf_sin * self._hf_sin) + if norm > NORMALIZATION_THRESHOLD: + self._hf_cos /= norm + self._hf_sin /= norm + ang = math.degrees(math.atan2(self._hf_sin, self._hf_cos)) + return self._normalize_deg(ang) + + def heading_from_vectors(self, x, y, z, calibrated=True): + """ + Computes the angle (0..360°) from a triplet. + - calibrated=True: applies offset/scale per axis (recommended) + - flat only (uses XY) + """ + + if calibrated: + x = (x - self.x_off) / (self.x_scale or 1.0) + y = (y - self.y_off) / (self.y_scale or 1.0) + # IMPORTANT: atan2(Y, X) + ang = math.degrees(math.atan2(x, y)) # atan2(Y, X) for compass heading (Y is forward, X is right) + ang = self._apply_heading_offsets(ang) + return self._filter_heading(ang) + + def heading_flat_only(self): + """ + Reads the sensor and returns the angle (0..360°) assuming the board is FLAT. + Uses XY (no tilt compensation). + """ + x, y, z = self.read_magnet() + return self.heading_from_vectors(x, y, z, calibrated=True) + + def heading_with_tilt_compensation(self, read_accel): + """ + Tilt-compensated compass (if an accelerometer is available). + read_accel() must return (ax, ay, az) ~g. + """ + + x, y, z = self.read_magnet() + # 3D calibration + x = (x - self.x_off) / (self.x_scale or 1.0) + y = (y - self.y_off) / (self.y_scale or 1.0) + z = (z - self.z_off) / (self.z_scale or 1.0) + + ax, ay, az = read_accel() + # roll / pitch from accelerometer + roll = math.atan2(ay, az) + pitch = math.atan2(-ax, math.sqrt(ay * ay + az * az)) + # straighten the magnetic vector + Xh = x * math.cos(pitch) + z * math.sin(pitch) + Yh = ( + x * math.sin(roll) * math.sin(pitch) + + y * math.cos(roll) + - z * math.sin(roll) * math.cos(pitch) + ) + ang = math.degrees(math.atan2(Yh, Xh)) # atan2(Yh, Xh) + ang = self._apply_heading_offsets(ang) + return self._filter_heading(ang) + + def direction_label(self, angle=None): + """Returns N/NE/E/... ; if angle=None, reads heading_flat_only().""" + if angle is None: + angle = self.heading_flat_only() + dirs = ["N", "NE", "E", "SE", "S", "SW", "W", "NW", "N"] + idx = int((angle) // 45) + return dirs[idx] + + ## + # --- Power/reset functions --- + ## + + def get_mode(self) -> str: + """Returns the current mode from MD1..0 (CFG_REG_A).""" + r = self.read_reg(LIS2MDL_CFG_REG_A) + md = r & 0b11 + return {0b00: "continuous", 0b01: "single", 0b11: "idle"}.get(md, "idle") + + def power_down(self): + """Switches to IDLE mode (low power).""" + r = self.read_reg(LIS2MDL_CFG_REG_A) + r = (r & ~0b11) | 0b11 # MD1..0 = 11 + self.setReg(r, LIS2MDL_CFG_REG_A) + + def wake(self, mode: str = "continuous"): + """Wakes the sensor: 'continuous' (default) or 'single'.""" + md = {"continuous": 0b00, "single": 0b01}.get(mode, 0b00) + r = self.read_reg(LIS2MDL_CFG_REG_A) + r = (r & ~0b11) | md + self.setReg(r, LIS2MDL_CFG_REG_A) + + def soft_reset(self, wait_ms: int = 10): + """ + SOFT_RST (bit5) in CFG_REG_A. + The bit auto-clears; after reset, the sensor returns to default values (idle mode expected). + """ + r = self.read_reg(LIS2MDL_CFG_REG_A) + r |= 1 << 5 # SOFT_RST + self.setReg(r, LIS2MDL_CFG_REG_A) + try: + sleep_ms(wait_ms) + except: + pass + + def reboot(self, wait_ms: int = 10): + """ + REBOOT (bit6) in CFG_REG_A: reload internal registers. + The bit auto-clears. + """ + r = self.read_reg(LIS2MDL_CFG_REG_A) + r |= 1 << 6 # REBOOT + self.setReg(r, LIS2MDL_CFG_REG_A) + try: + sleep_ms(wait_ms) + except Exception: + pass + + def is_idle(self) -> bool: + """True if the sensor is in IDLE mode (MD1..0 == 11).""" + return (self.read_reg(LIS2MDL_CFG_REG_A) & 0b11) == 0b11 diff --git a/lib/lis2mdl/manifest.py b/lib/lis2mdl/manifest.py new file mode 100644 index 00000000..6dd49526 --- /dev/null +++ b/lib/lis2mdl/manifest.py @@ -0,0 +1,6 @@ +metadata( + description="Driver of LIS2MDL magnetometer sensor.", + version="0.0.1", +) + +package("lis2mdl") diff --git a/tests/runner/executor.py b/tests/runner/executor.py index 42f1ea9a..83eca79c 100644 --- a/tests/runner/executor.py +++ b/tests/runner/executor.py @@ -24,6 +24,14 @@ def load_driver_mock(driver_name, fake_i2c): sys.modules["machine"] = fake_machine sys.modules["micropython"] = micropython_stub + # Patch time module to add MicroPython-specific functions + import time + + if not hasattr(time, "sleep_ms"): + time.sleep_ms = lambda ms: time.sleep(ms / 1000) + if not hasattr(time, "sleep_us"): + time.sleep_us = lambda us: time.sleep(us / 1000000) + # Add driver lib path to sys.path root = Path(__file__).parent.parent.parent driver_lib = root / "lib" / driver_name diff --git a/tests/scenarios/lis2mdl.yaml b/tests/scenarios/lis2mdl.yaml new file mode 100644 index 00000000..7bcedf51 --- /dev/null +++ b/tests/scenarios/lis2mdl.yaml @@ -0,0 +1,99 @@ +driver: lis2mdl +driver_class: LIS2MDL +i2c_address: 0x1E + +# I2C config for hardware tests (STeaMi board - STM32WB55) +i2c: + id: 1 + +# Register values for mock tests +# These simulate a real LIS2MDL magnetometer +mock_registers: + # WHO_AM_I (expected 0x40) + 0x4F: 0x40 + # CFG_REG_A (continuous mode, 10Hz, temp comp on) + 0x60: 0x80 + # CFG_REG_B (default) + 0x61: 0x00 + # CFG_REG_C (BDU enabled) + 0x62: 0x10 + # STATUS_REG (new data ready on all axes) + 0x67: 0x0F + # OUTX_L..OUTZ_H (simulated magnetic field ~300, -150, 450 LSB) + # Multi-byte read at 0xE8 (0x68 | 0x80), 6 bytes block + 0xE8: [0x2C, 0x01, 0x6A, 0xFF, 0xC2, 0x01] + # TEMP_OUT (simulated ~25°C: 25*8 = 200 = 0x00C8) + 0x6E: 0xC8 # TEMP_OUT_L + 0x6F: 0x00 # TEMP_OUT_H + # Offset registers (all zero) + 0x45: 0x00 + 0x46: 0x00 + 0x47: 0x00 + 0x48: 0x00 + 0x49: 0x00 + 0x4A: 0x00 + +tests: + - name: "Verify WHO_AM_I register" + action: read_register + register: 0x4F + expect: 0x40 + mode: [mock, hardware] + + - name: "Read WHO_AM_I via method" + action: call + method: read_who_am_i + expect: 0x40 + mode: [mock, hardware] + + - name: "Read status register" + action: call + method: read_status + expect_not_none: true + mode: [mock, hardware] + + - name: "Read magnetic field returns tuple" + action: call + method: read_magnet + expect_not_none: true + mode: [mock] + + - name: "Read magnetic field in uT returns tuple" + action: call + method: read_magnet_uT + expect_not_none: true + mode: [mock] + + - name: "Read temperature returns float" + action: call + method: read_temperature_c + expect_not_none: true + mode: [mock] + + - name: "Magnitude in plausible range" + action: call + method: magnitude_uT + expect_range: [10.0, 120.0] + mode: [hardware] + + - name: "Temperature in plausible range" + action: call + method: read_temperature_c + expect_range: [-10.0, 60.0] + mode: [hardware] + + - name: "Magnetic field values feel correct" + action: manual + display: + - method: magnitude_uT + label: "Magnitude" + unit: "µT" + - method: read_temperature_c + label: "Temperature" + unit: "°C" + - method: heading_flat_only + label: "Heading" + unit: "°" + prompt: "Ces valeurs sont-elles cohérentes (magnitude ~25-65µT, cap 0-360°) ?" + expect_true: true + mode: [hardware]