From b5782f1c260511d696fde79d5b32eeb9f58cc26c Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Tue, 21 Oct 2025 09:09:02 +0200 Subject: [PATCH 01/15] lis2mdl: initialise LIS2MDL driver. --- lib/lis2mdl/README.md | 1 + lib/lis2mdl/exemple/magnet.py | 0 lib/lis2mdl/lis2mdl/_init_.py | 0 lib/lis2mdl/lis2mdl/const.py | 0 lib/lis2mdl/lis2mdl/device.py | 0 lib/lis2mdl/manifest.py | 0 6 files changed, 1 insertion(+) create mode 100644 lib/lis2mdl/README.md create mode 100644 lib/lis2mdl/exemple/magnet.py create mode 100644 lib/lis2mdl/lis2mdl/_init_.py create mode 100644 lib/lis2mdl/lis2mdl/const.py create mode 100644 lib/lis2mdl/lis2mdl/device.py create mode 100644 lib/lis2mdl/manifest.py diff --git a/lib/lis2mdl/README.md b/lib/lis2mdl/README.md new file mode 100644 index 00000000..759a9a1f --- /dev/null +++ b/lib/lis2mdl/README.md @@ -0,0 +1 @@ +MAgnetometer \ No newline at end of file diff --git a/lib/lis2mdl/exemple/magnet.py b/lib/lis2mdl/exemple/magnet.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/lis2mdl/lis2mdl/_init_.py b/lib/lis2mdl/lis2mdl/_init_.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/lis2mdl/lis2mdl/const.py b/lib/lis2mdl/lis2mdl/const.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/lis2mdl/lis2mdl/device.py b/lib/lis2mdl/lis2mdl/device.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/lis2mdl/manifest.py b/lib/lis2mdl/manifest.py new file mode 100644 index 00000000..e69de29b From c4ed7301ef1e8a39eee948e7924dab1b11bc11b8 Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Tue, 21 Oct 2025 14:07:29 +0200 Subject: [PATCH 02/15] lis2mdl: add first measurement values. --- lib/lis2mdl/exemple/magnet.py | 17 +++++++++++ lib/lis2mdl/lis2mdl/__init__.py | 5 ++++ lib/lis2mdl/lis2mdl/_init_.py | 0 lib/lis2mdl/lis2mdl/const.py | 27 +++++++++++++++++ lib/lis2mdl/lis2mdl/device.py | 51 +++++++++++++++++++++++++++++++++ 5 files changed, 100 insertions(+) create mode 100644 lib/lis2mdl/lis2mdl/__init__.py delete mode 100644 lib/lis2mdl/lis2mdl/_init_.py diff --git a/lib/lis2mdl/exemple/magnet.py b/lib/lis2mdl/exemple/magnet.py index e69de29b..eb947d0b 100644 --- a/lib/lis2mdl/exemple/magnet.py +++ b/lib/lis2mdl/exemple/magnet.py @@ -0,0 +1,17 @@ +from time import sleep_ms +from machine import I2C +from lis2mdl.device import LIS2MDL + +# Initialize the LIS2MDL magnetometer +i2c = I2C(1) +magnetometer = LIS2MDL(i2c) + +print("Starting magnetometer readings...") + +while True: + # Read magnetic field data + magnet = magnetometer.read_magnet() + print(f"Magnetic field (X, Y, Z): {magnet}") + + # Wait for 1 second + sleep_ms(1000) \ No newline at end of file diff --git a/lib/lis2mdl/lis2mdl/__init__.py b/lib/lis2mdl/lis2mdl/__init__.py new file mode 100644 index 00000000..fe52efde --- /dev/null +++ b/lib/lis2mdl/lis2mdl/__init__.py @@ -0,0 +1,5 @@ +from lis2mdl.device import LIS2MDL + +__all__ = [ + "LIS2MDL", +] \ No newline at end of file diff --git a/lib/lis2mdl/lis2mdl/_init_.py b/lib/lis2mdl/lis2mdl/_init_.py deleted file mode 100644 index e69de29b..00000000 diff --git a/lib/lis2mdl/lis2mdl/const.py b/lib/lis2mdl/lis2mdl/const.py index e69de29b..8264e369 100644 --- a/lib/lis2mdl/lis2mdl/const.py +++ b/lib/lis2mdl/lis2mdl/const.py @@ -0,0 +1,27 @@ +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 \ No newline at end of file diff --git a/lib/lis2mdl/lis2mdl/device.py b/lib/lis2mdl/lis2mdl/device.py index e69de29b..65aa9986 100644 --- a/lib/lis2mdl/lis2mdl/device.py +++ b/lib/lis2mdl/lis2mdl/device.py @@ -0,0 +1,51 @@ +from machine import I2C +from lis2mdl.const import * + +class LIS2MDL(object): + def __init__(self, i2c, address=LIS2MDL_I2C_ADDR): + self.i2c = i2c + self.address = address + + self.writebuffer = bytearray(1) + self.readbuffer = bytearray(1) + + # Initialize the device + self.reset() + self.configure() + + def reset(self): + # Reset the device using CFG_REG_A + self.setReg(0x10, LIS2MDL_CFG_REG_A) + + def configure(self): + # Configure the device (example: enable continuous mode) + self.setReg(0x00, LIS2MDL_CFG_REG_B) + self.setReg(0x01, LIS2MDL_CFG_REG_C) + + def setReg(self, data, reg): + self.writebuffer[0] = data + self.i2c.writeto_mem(self.address, reg, self.writebuffer) + + def getReg(self, reg): + self.i2c.readfrom_mem_into(self.address, reg, self.readbuffer) + return self.readbuffer[0] + + def get2Reg(self, reg): + lowerByte = self.getReg(reg) + higherByte = self.getReg(reg + 1) + return (higherByte << 8) + lowerByte + + def read_magnet(self): + # Read magnetic field data for X, Y, Z axes + x = self.get2Reg(LIS2MDL_OUTX_L_REG) + y = self.get2Reg(LIS2MDL_OUTY_L_REG) + z = self.get2Reg(LIS2MDL_OUTZ_L_REG) + return (x, y, z) + + def whoAmI(self): + # Read the WHO_AM_I register + return self.getReg(LIS2MDL_WHO_AM_I) + + def status(self): + # Read the STATUS register + return self.getReg(LIS2MDL_STATUS_REG) \ No newline at end of file From e147cfc7769549d56bc94a3dd86fadcbcdeee038 Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Wed, 22 Oct 2025 14:36:28 +0200 Subject: [PATCH 03/15] lis2mdl: fix device configuration and first number. Still doesnt point to north but can see rotation --- lib/lis2mdl/exemple/magnet.py | 61 ++++++++++++++++++++++++++++++----- lib/lis2mdl/lis2mdl/device.py | 58 +++++++++++++++++++-------------- 2 files changed, 87 insertions(+), 32 deletions(-) diff --git a/lib/lis2mdl/exemple/magnet.py b/lib/lis2mdl/exemple/magnet.py index eb947d0b..2cd30f11 100644 --- a/lib/lis2mdl/exemple/magnet.py +++ b/lib/lis2mdl/exemple/magnet.py @@ -1,17 +1,62 @@ from time import sleep_ms from machine import I2C from lis2mdl.device import LIS2MDL +import math -# Initialize the LIS2MDL magnetometer i2c = I2C(1) -magnetometer = LIS2MDL(i2c) +mag = LIS2MDL(i2c) -print("Starting magnetometer readings...") +print("Quick calibration: rotate the board FLAT over 360°...") +xmin = ymin = 1e9 +xmax = ymax = -1e9 +# capture ~3 seconds of data for min/max +for _ in range(150): + x, y, z = mag.read_magnet() + xmin = min(xmin, x); xmax = max(xmax, x) + ymin = min(ymin, y); ymax = max(ymax, y) + sleep_ms(20) + +# offsets (hard-iron) and scales (simple 2D soft-iron) +x_off = (xmax + xmin) / 2.0 +y_off = (ymax + ymin) / 2.0 +x_scale = (xmax - xmin) / 2.0 +y_scale = (ymax - ymin) / 2.0 +# normalize the scale to have a circle (not essential but better) +scale = (x_scale + y_scale) / 2.0 + +print("Offsets:", x_off, y_off, " Scales:", x_scale, y_scale) + +print("\nContinuous reading (compass):") while True: - # Read magnetic field data - magnet = magnetometer.read_magnet() - print(f"Magnetic field (X, Y, Z): {magnet}") + x, y, z = mag.read_magnet() + + # recentering + normalization + x_c = (x - x_off) / scale + y_c = (y - y_off) / scale + + # heading (adjust the sign according to your reference if needed) + angle = math.degrees(math.atan2(y_c, x_c)) + if angle < 0: + angle += 360 + + direction = "" + if angle >= 337.5 or angle < 22.5: + direction = "N" + elif angle >= 22.5 and angle < 67.5: + direction = "NE" + elif angle >= 67.5 and angle < 112.5: + direction = "E" + elif angle >= 112.5 and angle < 157.5: + direction = "SE" + elif angle >= 157.5 and angle < 202.5: + direction = "S" + elif angle >= 202.5 and angle < 247.5: + direction = "SW" + elif angle >= 247.5 and angle < 292.5: + direction = "W" + elif angle >= 292.5 and angle < 337.5: + direction = "NW" - # Wait for 1 second - sleep_ms(1000) \ No newline at end of file + print("{} | {:.2f},{:.2f},{:.2f} | angle={:.2f}°".format(direction, x, y, z, angle)) + sleep_ms(100) diff --git a/lib/lis2mdl/lis2mdl/device.py b/lib/lis2mdl/lis2mdl/device.py index 65aa9986..0db2587d 100644 --- a/lib/lis2mdl/lis2mdl/device.py +++ b/lib/lis2mdl/lis2mdl/device.py @@ -1,27 +1,37 @@ +# device.py from machine import I2C from lis2mdl.const import * class LIS2MDL(object): - def __init__(self, i2c, address=LIS2MDL_I2C_ADDR): + def __init__(self, i2c, address=LIS2MDL_I2C_ADDR, odr_hz=10, temp_comp=True, low_power=False, drdy_pin=False): self.i2c = i2c self.address = address - self.writebuffer = bytearray(1) self.readbuffer = bytearray(1) - # Initialize the device - self.reset() - self.configure() - - def reset(self): - # Reset the device using CFG_REG_A - self.setReg(0x10, LIS2MDL_CFG_REG_A) - - def configure(self): - # Configure the device (example: enable continuous mode) + # Soft reset property + self.setReg(0x20, LIS2MDL_CFG_REG_A) # SOFT_RST=1 (not 0x10) + # small delay (if needed on the MicroPython side) + try: + import time; time.sleep_ms(10) + except: + pass + + # CFG_REG_A: COMP_TEMP, LP, ODR, MD=00 (continuous) + 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 + + # CFG_REG_B: default 0x00 (LPF/off_canc off); you can enable LPF if desired self.setReg(0x00, LIS2MDL_CFG_REG_B) - self.setReg(0x01, LIS2MDL_CFG_REG_C) + # CFG_REG_C: BDU=1 (+ DRDY_on_PIN if requested) + cfg_c = 0x10 | (0x01 if drdy_pin else 0x00) + self.setReg(cfg_c, LIS2MDL_CFG_REG_C) + + # --- identical low-level --- def setReg(self, data, reg): self.writebuffer[0] = data self.i2c.writeto_mem(self.address, reg, self.writebuffer) @@ -30,22 +40,22 @@ def getReg(self, reg): self.i2c.readfrom_mem_into(self.address, reg, self.readbuffer) return self.readbuffer[0] - def get2Reg(self, reg): - lowerByte = self.getReg(reg) - higherByte = self.getReg(reg + 1) - return (higherByte << 8) + lowerByte + # signed helper + @staticmethod + def _to_int16(v): + return v - 0x10000 if v & 0x8000 else v + # burst read + signed def read_magnet(self): - # Read magnetic field data for X, Y, Z axes - x = self.get2Reg(LIS2MDL_OUTX_L_REG) - y = self.get2Reg(LIS2MDL_OUTY_L_REG) - z = self.get2Reg(LIS2MDL_OUTZ_L_REG) + # auto-increment: reg | 0x80 (cf. datasheet I2C op) + 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) def whoAmI(self): - # Read the WHO_AM_I register return self.getReg(LIS2MDL_WHO_AM_I) def status(self): - # Read the STATUS register - return self.getReg(LIS2MDL_STATUS_REG) \ No newline at end of file + return self.getReg(LIS2MDL_STATUS_REG) From 63072e986ba1c73e2561820747fce78f6b55c75c Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Wed, 22 Oct 2025 14:43:53 +0200 Subject: [PATCH 04/15] lis2mdl: fix exemple point to north. --- lib/lis2mdl/exemple/magnet.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/lis2mdl/exemple/magnet.py b/lib/lis2mdl/exemple/magnet.py index 2cd30f11..65549a24 100644 --- a/lib/lis2mdl/exemple/magnet.py +++ b/lib/lis2mdl/exemple/magnet.py @@ -36,7 +36,7 @@ y_c = (y - y_off) / scale # heading (adjust the sign according to your reference if needed) - angle = math.degrees(math.atan2(y_c, x_c)) + angle = math.degrees(math.atan2(x_c,y_c)) if angle < 0: angle += 360 From 1bbab38fa62a214a0f5a93da841750cb9fbcb6da Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Mon, 27 Oct 2025 10:51:53 +0100 Subject: [PATCH 05/15] lis2mdl: add driver function and example. --- lib/lis2mdl/README.md | 69 +++++++++++++++++++- lib/lis2mdl/exemple/magnet.py | 61 +++-------------- lib/lis2mdl/lis2mdl/device.py | 119 ++++++++++++++++++++++++++++++---- 3 files changed, 184 insertions(+), 65 deletions(-) diff --git a/lib/lis2mdl/README.md b/lib/lis2mdl/README.md index 759a9a1f..bbd2ec13 100644 --- a/lib/lis2mdl/README.md +++ b/lib/lis2mdl/README.md @@ -1 +1,68 @@ -MAgnetometer \ No newline at end of file +# LIS2MDL Magnetometer Driver + +This is a MicroPython driver for the LIS2MDL 3-axis magnetometer. The LIS2MDL is a high-performance magnetic sensor designed for applications such as electronic compasses, motion tracking, and orientation detection. This driver provides methods for initialization, calibration, and reading magnetic field data. + +## Features +- Soft reset and initialization of the LIS2MDL sensor. +- Configurable output data rate (ODR) and low-power mode. +- 3D calibration for offsets and scales. +- Heading calculation with and without tilt compensation. +- Direction labeling (e.g., N, NE, E, etc.). + +## Requirements +- MicroPython-compatible board with I2C support. +- LIS2MDL sensor module. + +## Installation +1. Copy the `lis2mdl` folder to your MicroPython device. +2. Import the driver in your script: + ```python + from lis2mdl.device import LIS2MDL + ``` + +## Usage +### Basic Example +```python +from time import sleep_ms +from machine import I2C +from lis2mdl.device import LIS2MDL + +# Initialize I2C and LIS2MDL +i2c = I2C(1) +mag = LIS2MDL(i2c) + +# Perform calibration +mag.calibrate_step() +calibration_values = mag.get_calibration() +print("Calibration values:", calibration_values) + +# Continuous heading reading +print("Continuous heading readings:") +while True: + angle = mag.heading_flat_only() + direction = mag.direction_label(angle) + print(f"{direction} | angle={angle:.2f}°") + sleep_ms(100) +``` + +### Tilt Compensation Example +To use tilt compensation, you need an accelerometer to provide roll and pitch data. Pass a function that reads accelerometer data to the `heading_with_tilt_compensation` method. + +```python +def read_accel(): + # Replace with your accelerometer reading logic + return ax, ay, az + +angle = mag.heading_with_tilt_compensation(read_accel) +print(f"Tilt-compensated angle: {angle:.2f}°") +``` + +## Calibration +The `calibrate_step` method performs a quick 3D calibration. Rotate the sensor flat over 360° to capture the minimum and maximum magnetic field values. The offsets and scales are calculated automatically. + +## Datasheet +For detailed information about the LIS2MDL sensor, refer to the [datasheet](https://www.st.com/resource/en/datasheet/lis2mdl.pdf). + +## Notes +- The `heading_with_tilt_compensation` method has not been tested due to the lack of an accelerometer during development. +- Ensure the I2C pins are correctly connected to the LIS2MDL module. \ No newline at end of file diff --git a/lib/lis2mdl/exemple/magnet.py b/lib/lis2mdl/exemple/magnet.py index 65549a24..af58a488 100644 --- a/lib/lis2mdl/exemple/magnet.py +++ b/lib/lis2mdl/exemple/magnet.py @@ -1,62 +1,19 @@ from time import sleep_ms from machine import I2C from lis2mdl.device import LIS2MDL -import math i2c = I2C(1) mag = LIS2MDL(i2c) -print("Quick calibration: rotate the board FLAT over 360°...") -xmin = ymin = 1e9 -xmax = ymax = -1e9 +mag.calibrate_step() +calibration_values = mag.get_calibration() +print("x_off, y_off, z_off, x_scale, y_scale, z_scale :") +print(calibration_values) -# capture ~3 seconds of data for min/max -for _ in range(150): - x, y, z = mag.read_magnet() - xmin = min(xmin, x); xmax = max(xmax, x) - ymin = min(ymin, y); ymax = max(ymax, y) - sleep_ms(20) - -# offsets (hard-iron) and scales (simple 2D soft-iron) -x_off = (xmax + xmin) / 2.0 -y_off = (ymax + ymin) / 2.0 -x_scale = (xmax - xmin) / 2.0 -y_scale = (ymax - ymin) / 2.0 -# normalize the scale to have a circle (not essential but better) -scale = (x_scale + y_scale) / 2.0 - -print("Offsets:", x_off, y_off, " Scales:", x_scale, y_scale) - -print("\nContinuous reading (compass):") +print("Lecture continue (boussole):") while True: - x, y, z = mag.read_magnet() - - # recentering + normalization - x_c = (x - x_off) / scale - y_c = (y - y_off) / scale - - # heading (adjust the sign according to your reference if needed) - angle = math.degrees(math.atan2(x_c,y_c)) - if angle < 0: - angle += 360 + angle = mag.heading_flat_only() + dir8 = mag.direction_label(angle) - direction = "" - if angle >= 337.5 or angle < 22.5: - direction = "N" - elif angle >= 22.5 and angle < 67.5: - direction = "NE" - elif angle >= 67.5 and angle < 112.5: - direction = "E" - elif angle >= 112.5 and angle < 157.5: - direction = "SE" - elif angle >= 157.5 and angle < 202.5: - direction = "S" - elif angle >= 202.5 and angle < 247.5: - direction = "SW" - elif angle >= 247.5 and angle < 292.5: - direction = "W" - elif angle >= 292.5 and angle < 337.5: - direction = "NW" - - print("{} | {:.2f},{:.2f},{:.2f} | angle={:.2f}°".format(direction, x, y, z, angle)) - sleep_ms(100) + print(f"{dir8} | angle={angle:.2f}°") + sleep_ms(100) \ No newline at end of file diff --git a/lib/lis2mdl/lis2mdl/device.py b/lib/lis2mdl/lis2mdl/device.py index 0db2587d..7f6fd032 100644 --- a/lib/lis2mdl/lis2mdl/device.py +++ b/lib/lis2mdl/lis2mdl/device.py @@ -1,53 +1,67 @@ # 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 * +import time +import math class LIS2MDL(object): + # Default calibration offsets and scales for the magnetometer + x_off = -132.0 + y_off = -521.5 + z_off = -891.0 + + x_scale = 273.0 + y_scale = 313.5 + z_scale = 295.0 + def __init__(self, i2c, address=LIS2MDL_I2C_ADDR, odr_hz=10, temp_comp=True, low_power=False, drdy_pin=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) - # Soft reset property + # 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) - # small delay (if needed on the MicroPython side) try: - import time; time.sleep_ms(10) + import time; time.sleep_ms(10) # Small delay for reset to complete except: pass - # CFG_REG_A: COMP_TEMP, LP, ODR, MD=00 (continuous) + # 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 + self.setReg(cfg_a, LIS2MDL_CFG_REG_A) # Essential to exit IDLE mode - # CFG_REG_B: default 0x00 (LPF/off_canc off); you can enable LPF if desired - self.setReg(0x00, LIS2MDL_CFG_REG_B) + # Configure low-pass filter and other optional settings. + self.setReg(0x00, LIS2MDL_CFG_REG_B) # Default: LPF and offset cancellation off - # CFG_REG_C: BDU=1 (+ DRDY_on_PIN if requested) + # Enable block data update and optionally configure the DRDY pin. cfg_c = 0x10 | (0x01 if drdy_pin else 0x00) self.setReg(cfg_c, LIS2MDL_CFG_REG_C) - # --- identical low-level --- + # --- Low-level I2C communication methods --- 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 getReg(self, reg): + # Read a byte from a specific register. self.i2c.readfrom_mem_into(self.address, reg, self.readbuffer) return self.readbuffer[0] - # signed helper @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 - # burst read + signed def read_magnet(self): - # auto-increment: reg | 0x80 (cf. datasheet I2C op) + # 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]) @@ -55,7 +69,88 @@ def read_magnet(self): return (x, y, z) def whoAmI(self): + # Return the WHO_AM_I register value to verify the sensor's identity. return self.getReg(LIS2MDL_WHO_AM_I) def status(self): + # Return the STATUS_REG value to check the sensor's status. return self.getReg(LIS2MDL_STATUS_REG) + + 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_step(self): + # Perform a quick 3D calibration by rotating the board flat over 360°. + print("Quick 3D calibration: rotate the board FLAT over 360°...") + + xmin = ymin = zmin = 1e9 + xmax = ymax = zmax = -1e9 + + for _ in range(150): + 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) + time.sleep_ms(20) + + # Calculate offsets and scales based on the min/max values. + 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 + self.y_scale = (ymax - ymin) / 2.0 + self.z_scale = (zmax - zmin) / 2.0 + + def get_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) + + def heading_flat_only(self): + # Calculate the heading (angle) assuming the sensor is flat. + x, y, z = self.read_magnet() + x = (x - self.x_off) / self.x_scale + y = (y - self.y_off) / self.y_scale + angle = math.degrees(math.atan2(x, y)) + if angle < 0: + angle += 360 + return angle + + def heading_with_tilt_compensation(self, read_accel): + # Calculate the heading with tilt compensation using accelerometer data. + # Note: This method has not been tested as no accelerometer was available during development. + + # Read magnetometer and accelerometer data. + x, y, z = self.read_magnet() + ax, ay, az = read_accel() + + # Apply 3D calibration (offsets and scales for each axis). + x = (x - self.x_off) / self.x_scale + y = (y - self.y_off) / self.y_scale + z = (z - self.z_off) / self.z_scale + + # Calculate roll and pitch from accelerometer data. + roll = math.atan2(ay, az) + pitch = math.atan2(-ax, math.sqrt(ay*ay + az*az)) + + # Adjust the magnetic vector to account for tilt. + 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) + + # Calculate the heading (0 to 360°). + angle = math.degrees(math.atan2(Xh, Yh)) + if angle < 0: + angle += 360 + return angle + + def direction_label(self, angle): + # Return a compass direction label (e.g., N, NE, E, etc.) based on the angle. + dirs = ["N","NE","E","SE","S","SW","W","NW","N"] + idx = int((angle + 22.5)//45) + return dirs[idx] \ No newline at end of file From d853c4b8b9614008bed3ddaf7d81bfd432423a08 Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Tue, 28 Oct 2025 16:04:08 +0100 Subject: [PATCH 06/15] lis2mdl: add LIS2MDL driver. --- lib/lis2mdl/README.md | 302 +++++++++++++++--- lib/lis2mdl/exemple/magnet.py | 19 -- lib/lis2mdl/exemple/magnet_test.py | 443 ++++++++++++++++++++++++++ lib/lis2mdl/lis2mdl/const.py | 12 +- lib/lis2mdl/lis2mdl/device.py | 487 +++++++++++++++++++++++++---- 5 files changed, 1142 insertions(+), 121 deletions(-) delete mode 100644 lib/lis2mdl/exemple/magnet.py create mode 100644 lib/lis2mdl/exemple/magnet_test.py diff --git a/lib/lis2mdl/README.md b/lib/lis2mdl/README.md index bbd2ec13..768be723 100644 --- a/lib/lis2mdl/README.md +++ b/lib/lis2mdl/README.md @@ -1,68 +1,274 @@ -# LIS2MDL Magnetometer Driver +# LIS2MDL Magnetometer Driver for MicroPython -This is a MicroPython driver for the LIS2MDL 3-axis magnetometer. The LIS2MDL is a high-performance magnetic sensor designed for applications such as electronic compasses, motion tracking, and orientation detection. This driver provides methods for initialization, calibration, and reading magnetic field data. +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 -- Soft reset and initialization of the LIS2MDL sensor. -- Configurable output data rate (ODR) and low-power mode. -- 3D calibration for offsets and scales. -- Heading calculation with and without tilt compensation. -- Direction labeling (e.g., N, NE, E, etc.). +--- -## Requirements -- MicroPython-compatible board with I2C support. -- LIS2MDL sensor module. +## 🧭 Features -## Installation -1. Copy the `lis2mdl` folder to your MicroPython device. -2. Import the driver in your script: - ```python - from lis2mdl.device import LIS2MDL - ``` +* ✅ 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: -## Usage -### Basic Example ```python -from time import sleep_ms -from machine import I2C -from lis2mdl.device import LIS2MDL +from machine import I2C, Pin +from device import LIS2MDL -# Initialize I2C and LIS2MDL -i2c = I2C(1) +i2c = I2C(1, scl=Pin(22), sda=Pin(21)) mag = LIS2MDL(i2c) +``` -# Perform calibration -mag.calibrate_step() -calibration_values = mag.get_calibration() -print("Calibration values:", calibration_values) +--- -# Continuous heading reading -print("Continuous heading readings:") -while True: - angle = mag.heading_flat_only() - direction = mag.direction_label(angle) - print(f"{direction} | angle={angle:.2f}°") - sleep_ms(100) +## 🧩 Installation + +### 1. Copy files to your board + +Upload both files using Thonny, mpremote, or ampy: + +``` +device.py +lis2mdl/const.py ``` -### Tilt Compensation Example -To use tilt compensation, you need an accelerometer to provide roll and pitch data. Pass a function that reads accelerometer data to the `heading_with_tilt_compensation` method. +### 2. Import and initialize + +```python +from machine import I2C, Pin +from device import LIS2MDL + +i2c = I2C(1, scl=Pin(22), sda=Pin(21)) +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(): - # Replace with your accelerometer reading logic - return ax, ay, az + 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) -angle = mag.heading_with_tilt_compensation(read_accel) -print(f"Tilt-compensated angle: {angle:.2f}°") +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() ``` -## Calibration -The `calibrate_step` method performs a quick 3D calibration. Rotate the sensor flat over 360° to capture the minimum and maximum magnetic field values. The offsets and scales are calculated automatically. +--- + +## 🧰 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 device 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) +``` + +--- + +## 🧾 Notes + +* The LIS2MDL outputs magnetic data in **LSB**, with a conversion factor of **0.15 µT/LSB**. +* Temperature readings are **relative only** (not absolute). +* Calibration should be repeated if the sensor environment changes (metal nearby, relocation, etc.). +* Tilt compensation requires an external **accelerometer** or IMU (e.g., LIS3DH, MPU6050, BNO055). + +--- + +## 📚 References -## Datasheet -For detailed information about the LIS2MDL sensor, refer to the [datasheet](https://www.st.com/resource/en/datasheet/lis2mdl.pdf). +* [STMicroelectronics LIS2MDL Datasheet](https://www.st.com/resource/en/datasheet/lis2mdl.pdf) +* [MicroPython Documentation](https://docs.micropython.org/en/latest/library/machine.I2C.html) -## Notes -- The `heading_with_tilt_compensation` method has not been tested due to the lack of an accelerometer during development. -- Ensure the I2C pins are correctly connected to the LIS2MDL module. \ No newline at end of file +--- \ No newline at end of file diff --git a/lib/lis2mdl/exemple/magnet.py b/lib/lis2mdl/exemple/magnet.py deleted file mode 100644 index af58a488..00000000 --- a/lib/lis2mdl/exemple/magnet.py +++ /dev/null @@ -1,19 +0,0 @@ -from time import sleep_ms -from machine import I2C -from lis2mdl.device import LIS2MDL - -i2c = I2C(1) -mag = LIS2MDL(i2c) - -mag.calibrate_step() -calibration_values = mag.get_calibration() -print("x_off, y_off, z_off, x_scale, y_scale, z_scale :") -print(calibration_values) - -print("Lecture continue (boussole):") -while True: - angle = mag.heading_flat_only() - dir8 = mag.direction_label(angle) - - print(f"{dir8} | angle={angle:.2f}°") - sleep_ms(100) \ No newline at end of file diff --git a/lib/lis2mdl/exemple/magnet_test.py b/lib/lis2mdl/exemple/magnet_test.py new file mode 100644 index 00000000..6f7f3329 --- /dev/null +++ b/lib/lis2mdl/exemple/magnet_test.py @@ -0,0 +1,443 @@ +from time import sleep_ms +from machine import I2C +from lis2mdl.device import LIS2MDL +from lis2mdl.const import * + +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 5.0 <= B <= 200.0 else "FAIL") + ok &= (5.0 <= B <= 200.0) # 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 (-100.0 < t1 < 150.0) and (-100.0 < t2 < 150.0)) + 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]) < 0.2 and abs(q["mean_xy"][1]) < 0.2 + ok_round = q["anisotropy_xy"] < 1.4 # 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]) < 0.3 and abs(q["mean_xy"][1]) < 0.3 + ok_round = q["anisotropy_xy"] < 1.6 + 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 = (14.0 <= diff <= 20.0) or (340.0 <= diff <= 346.0) # 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 > 300.0 # 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 + import math + 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 < 90.0 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 _bits(v, hi, lo): + m = (1 << (hi-lo+1)) - 1 + return (v >> lo) & m + +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/const.py b/lib/lis2mdl/lis2mdl/const.py index 8264e369..63912f2a 100644 --- a/lib/lis2mdl/lis2mdl/const.py +++ b/lib/lis2mdl/lis2mdl/const.py @@ -24,4 +24,14 @@ 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 \ No newline at end of file +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_CRTL_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 index 7f6fd032..957d6acf 100644 --- a/lib/lis2mdl/lis2mdl/device.py +++ b/lib/lis2mdl/lis2mdl/device.py @@ -44,17 +44,148 @@ def __init__(self, i2c, address=LIS2MDL_I2C_ADDR, odr_hz=10, temp_comp=True, low cfg_c = 0x10 | (0x01 if drdy_pin else 0x00) self.setReg(cfg_c, LIS2MDL_CFG_REG_C) - # --- Low-level I2C communication methods --- + + + ## + # --- 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 + _declination_deg = 0.0 + + 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 getReg(self, reg): + 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. @@ -68,13 +199,64 @@ def read_magnet(self): z = self._to_int16((buf[5] << 8) | buf[4]) return (x, y, z) - def whoAmI(self): - # Return the WHO_AM_I register value to verify the sensor's identity. - return self.getReg(LIS2MDL_WHO_AM_I) + # --- 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 status(self): - # Return the STATUS_REG value to check the sensor's status. - return self.getReg(LIS2MDL_STATUS_REG) + 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. @@ -85,72 +267,271 @@ def set_calibrate_step(self, xoff, yoff, zoff, xscale, yscale, zscale): self.y_scale = yscale self.z_scale = zscale - def calibrate_step(self): - # Perform a quick 3D calibration by rotating the board flat over 360°. - print("Quick 3D calibration: rotate the board FLAT over 360°...") - + 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, z = self.read_magnet() + xmin = min(xmin, x); xmax = max(xmax, x) + ymin = min(ymin, y); ymax = max(ymax, y) + time.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(150): + 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) - time.sleep_ms(20) - - # Calculate offsets and scales based on the min/max values. + time.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 - self.y_scale = (ymax - ymin) / 2.0 - self.z_scale = (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) + time.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 + import math as _m + radii = [_m.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 = _m.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 _m.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 --- + ## + + _heading_offset_deg = 0.0 # user setting: align your physical 0° + _declination_deg = 0.0 # true north vs magnetic north + # angle filter via vector averaging (robust around 0/360) + _hf_alpha = 0.0 + _hf_cos = None + _hf_sin = None + + def set_heading_offset(self, deg: float): + self._heading_offset_deg = float(deg) + + def set_declination(self, deg: float): + self._declination_deg = float(deg) + + 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 + import math + 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 + norm = math.sqrt(self._hf_cos * self._hf_cos + self._hf_sin * self._hf_sin) + if norm > 1e-6: + 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) + """ + import math + 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(y, x)) + ang = self._apply_heading_offsets(ang) + return self._filter_heading(ang) - def get_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) - def heading_flat_only(self): - # Calculate the heading (angle) assuming the sensor is flat. + """ + 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() - x = (x - self.x_off) / self.x_scale - y = (y - self.y_off) / self.y_scale - angle = math.degrees(math.atan2(x, y)) - if angle < 0: - angle += 360 - return angle - - def heading_with_tilt_compensation(self, read_accel): - # Calculate the heading with tilt compensation using accelerometer data. - # Note: This method has not been tested as no accelerometer was available during development. + return self.heading_from_vectors(x, y, z, calibrated=True) - # Read magnetometer and accelerometer data. + def heading_with_tilt_compensation(self, read_accel): + """ + Tilt-compensated compass (if an accelerometer is available). + read_accel() must return (ax, ay, az) ~g. + """ + import math x, y, z = self.read_magnet() - ax, ay, az = read_accel() + # 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) - # Apply 3D calibration (offsets and scales for each axis). - x = (x - self.x_off) / self.x_scale - y = (y - self.y_off) / self.y_scale - z = (z - self.z_off) / self.z_scale - - # Calculate roll and pitch from accelerometer data. + ax, ay, az = read_accel() + # roll / pitch from accelerometer roll = math.atan2(ay, az) pitch = math.atan2(-ax, math.sqrt(ay*ay + az*az)) - - # Adjust the magnetic vector to account for tilt. + # 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) - # Calculate the heading (0 to 360°). - angle = math.degrees(math.atan2(Xh, Yh)) - if angle < 0: - angle += 360 - return angle - - def direction_label(self, angle): - # Return a compass direction label (e.g., N, NE, E, etc.) based on the angle. + 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 + 22.5)//45) - return dirs[idx] \ No newline at end of file + 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: + import time; time.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: + import time; time.sleep_ms(wait_ms) + except: + 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 \ No newline at end of file From 5bc3789fcebb149fdc8c4b73fd68a3393104668e Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Thu, 30 Oct 2025 15:00:21 +0100 Subject: [PATCH 07/15] lis2mdl: add LIS2MDL exemple. --- lib/lis2mdl/exemple/magnet_compass.py | 14 ++++++++++++++ lib/lis2mdl/exemple/magnet_fieldForce.py | 13 +++++++++++++ 2 files changed, 27 insertions(+) create mode 100644 lib/lis2mdl/exemple/magnet_compass.py create mode 100644 lib/lis2mdl/exemple/magnet_fieldForce.py diff --git a/lib/lis2mdl/exemple/magnet_compass.py b/lib/lis2mdl/exemple/magnet_compass.py new file mode 100644 index 00000000..12d1519c --- /dev/null +++ b/lib/lis2mdl/exemple/magnet_compass.py @@ -0,0 +1,14 @@ +# exemple 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) + +while True: + angle = mag.heading_flat_only() + direction = mag.direction_label(angle) + print("Cap:", angle, "°", "-", direction) + sleep_ms(100) diff --git a/lib/lis2mdl/exemple/magnet_fieldForce.py b/lib/lis2mdl/exemple/magnet_fieldForce.py new file mode 100644 index 00000000..b8efbc89 --- /dev/null +++ b/lib/lis2mdl/exemple/magnet_fieldForce.py @@ -0,0 +1,13 @@ +# exemple 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) + +while True: + field_strength = mag.magnitude_uT() + print("Champ magnétique :", field_strength, "µT") + sleep_ms(100) From c80aebef9b0844caba94ef66e5120babab889083 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20NEDJAR?= Date: Mon, 3 Nov 2025 10:37:54 +0100 Subject: [PATCH 08/15] lis2mdl: correct simple linter errors. --- lib/lis2mdl/exemple/magnet_test.py | 398 ++++++++++++++++++++++------- lib/lis2mdl/lis2mdl/__init__.py | 2 +- lib/lis2mdl/lis2mdl/const.py | 6 +- lib/lis2mdl/lis2mdl/device.py | 221 +++++++++------- 4 files changed, 449 insertions(+), 178 deletions(-) diff --git a/lib/lis2mdl/exemple/magnet_test.py b/lib/lis2mdl/exemple/magnet_test.py index 6f7f3329..d9faec7b 100644 --- a/lib/lis2mdl/exemple/magnet_test.py +++ b/lib/lis2mdl/exemple/magnet_test.py @@ -2,11 +2,14 @@ from machine import I2C from lis2mdl.device import LIS2MDL from lis2mdl.const import * +import math + def _bits(v, hi, lo): - m = (1 << (hi-lo+1)) - 1 + m = (1 << (hi - lo + 1)) - 1 return (v >> lo) & m + def test_sets(dev): ok = True @@ -14,93 +17,202 @@ def test_sets(dev): 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) + 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) + 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) + 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) + 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) + 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) + 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) + 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) + 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) + 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) + 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) + 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) + 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) + 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) + 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}°") + 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)) + 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 + 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") + 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 ===") @@ -108,7 +220,7 @@ def test_reads(dev): # 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) + ok &= who == 0x40 # STATUS & DATA READY st1 = dev.read_status() @@ -121,29 +233,37 @@ def test_reads(dev): # 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)) + 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)) + 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 5.0 <= B <= 200.0 else "FAIL") - ok &= (5.0 <= B <= 200.0) # wide, since local disturbances are possible + print( + f"magnitude_uT: |B|={B:.1f} µT (Earth ~25–65 µT). =>", + "OK" if 5.0 <= B <= 200.0 else "FAIL", + ) + ok &= 5.0 <= B <= 200.0 # 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) + 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 @@ -153,46 +273,71 @@ def test_reads(dev): 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 (-100.0 < t1 < 150.0) and (-100.0 < t2 < 150.0)) + ok_temp = ( + isinstance(t1, float) + and isinstance(t2, float) + and (-100.0 < t1 < 150.0) + and (-100.0 < t2 < 150.0) + ) 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") + 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( + 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 + 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( + "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]) < 0.2 and abs(q["mean_xy"][1]) < 0.2 - ok_round = q["anisotropy_xy"] < 1.4 # realistic tolerances + ok_round = q["anisotropy_xy"] < 1.4 # realistic tolerances print("=> Center close to 0 :", "OK" if ok_center else "WARN") - print("=> Circle ≈ round :", "OK" if ok_round 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)) @@ -201,17 +346,29 @@ def test_calibrate_3d(dev): 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( + "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"])) + 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]) < 0.3 and abs(q["mean_xy"][1]) < 0.3 - ok_round = q["anisotropy_xy"] < 1.6 + ok_round = q["anisotropy_xy"] < 1.6 print("=> Center close to 0 :", "OK" if ok_center else "WARN") - print("=> Circle ≈ round :", "OK" if ok_round 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. @@ -220,18 +377,25 @@ def test_heading_after_calib(dev, n=200, delay_ms=20): print("\n=== HEADING after calibration (qualitative) ===") angles = [] for _ in range(n): - ang = dev.heading_flat_only() # make sure you have atan2(y, x) inside + 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 + 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)") + 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) ===") @@ -246,6 +410,7 @@ def test_heading_flat_basic(dev, n=10, delay_ms=50): 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) @@ -261,9 +426,13 @@ def test_heading_offset_declination(dev): diff = (a1 - a0) % 360.0 # accept ~17° ±3° (due to noise/quantization/filtering) ok = (14.0 <= diff <= 20.0) or (340.0 <= diff <= 346.0) # wrap - print(f"angle0={a0:.2f}°, angle1={a1:.2f}°, diff≈{diff:.2f}° =>", "OK" if ok else "FAIL") + 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. @@ -271,7 +440,8 @@ def test_heading_span_turn(dev, duration_ms=6000, step_ms=50): """ 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) + dev.set_heading_offset(0.0) + dev.set_declination(0.0) angles = [] t = 0 while t < duration_ms: @@ -279,15 +449,18 @@ def test_heading_span_turn(dev, duration_ms=6000, step_ms=50): angles.append(a) sleep_ms(step_ms) t += step_ms - minA = min(angles); maxA = max(angles) + minA = min(angles) + maxA = max(angles) # span modulo 360 (handles wrap) span = maxA - minA - if span < 0: span += 360.0 + if span < 0: + span += 360.0 print(f"min={minA:.1f}°, max={maxA:.1f}°, span≈{span:.1f}°") ok = span > 300.0 # 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°. @@ -295,12 +468,13 @@ def test_heading_filter_wrap(dev): """ print("\n=== TEST filter & wrap ===") dev.set_heading_filter(0.3) - dev._hf_cos = None; dev._hf_sin = None # reset filter + 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 - import math + for ang in seq: # unit vectors in the XY plane x = math.cos(math.radians(ang)) @@ -309,10 +483,11 @@ def test_heading_filter_wrap(dev): 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 < 90.0 for i in range(1, len(outs))) + ok = all((outs[i] - outs[i - 1]) % 360.0 < 90.0 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) @@ -322,10 +497,12 @@ def run_heading_tests(dev): all_ok &= test_heading_filter_wrap(dev) print("\n=== HEADING summary:", "OK ✅" if all_ok else "Partial ⚠️ (see details)") + def _bits(v, hi, lo): - m = (1 << (hi-lo+1)) - 1 + m = (1 << (hi - lo + 1)) - 1 return (v >> lo) & m + def test_power_modes(dev): print("\n=== TEST POWER MODES ===") ok = True @@ -333,41 +510,77 @@ def test_power_modes(dev): # 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) + 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) + 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() + 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) + 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 + 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}") + 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) @@ -384,30 +597,37 @@ def test_soft_reset(dev): # - 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 + 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 + 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 + 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}") @@ -418,14 +638,17 @@ def test_reboot(dev): # 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 + 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) + 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) @@ -435,7 +658,8 @@ def run_power_reset_tests(dev): # ---- Run the tests ---- -i2c = I2C(1) ; dev = LIS2MDL(i2c) +i2c = I2C(1) +dev = LIS2MDL(i2c) test_reads(dev) test_sets(dev) run_all_calibration_tests(dev) diff --git a/lib/lis2mdl/lis2mdl/__init__.py b/lib/lis2mdl/lis2mdl/__init__.py index fe52efde..9b853c94 100644 --- a/lib/lis2mdl/lis2mdl/__init__.py +++ b/lib/lis2mdl/lis2mdl/__init__.py @@ -2,4 +2,4 @@ __all__ = [ "LIS2MDL", -] \ No newline at end of file +] diff --git a/lib/lis2mdl/lis2mdl/const.py b/lib/lis2mdl/lis2mdl/const.py index 63912f2a..382281f8 100644 --- a/lib/lis2mdl/lis2mdl/const.py +++ b/lib/lis2mdl/lis2mdl/const.py @@ -31,7 +31,7 @@ LIS2MDL_TEMP_OUT_H_REG = const(0x6F) # Temperature output high byte # Interrupt control and source registers -LIS2MDL_INT_CRTL_REG = const(0x63) # Interrupt control register +LIS2MDL_INT_CRTL_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 +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 index 957d6acf..d34d1ec7 100644 --- a/lib/lis2mdl/lis2mdl/device.py +++ b/lib/lis2mdl/lis2mdl/device.py @@ -6,6 +6,7 @@ import time import math + class LIS2MDL(object): # Default calibration offsets and scales for the magnetometer x_off = -132.0 @@ -16,7 +17,15 @@ class LIS2MDL(object): y_scale = 313.5 z_scale = 295.0 - def __init__(self, i2c, address=LIS2MDL_I2C_ADDR, odr_hz=10, temp_comp=True, low_power=False, drdy_pin=False): + def __init__( + self, + i2c, + address=LIS2MDL_I2C_ADDR, + odr_hz=10, + temp_comp=True, + low_power=False, + drdy_pin=False, + ): # Initialize the LIS2MDL sensor with the given I2C interface and settings. self.i2c = i2c self.address = address @@ -26,16 +35,16 @@ def __init__(self, i2c, address=LIS2MDL_I2C_ADDR, odr_hz=10, temp_comp=True, low # 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: - import time; time.sleep_ms(10) # Small delay for reset to complete + time.sleep_ms(10) # Small delay for reset to complete except: 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) + 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 + 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 @@ -44,71 +53,83 @@ def __init__(self, i2c, address=LIS2MDL_I2C_ADDR, odr_hz=10, temp_comp=True, low cfg_c = 0x10 | (0x01 if drdy_pin else 0x00) self.setReg(cfg_c, LIS2MDL_CFG_REG_C) - - ## - # --- SET functions --- + # --- 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) + 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) + 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) + 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) + 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) + 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): + 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) + 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) + 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) + 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) + if enable: + reg |= 1 << 2 + else: + reg &= ~(1 << 2) self.setReg(reg, LIS2MDL_CFG_REG_C) # --- Compass: heading offset & declination (software) --- @@ -123,7 +144,6 @@ def set_declination(self, deg: float): # (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 @@ -132,16 +152,16 @@ def setReg(self, data, reg): def _write_16(self, reg_l, value): value &= 0xFFFF self.setReg(value & 0xFF, reg_l) - self.setReg((value >> 8) & 0xFF, reg_l+1) + self.setReg((value >> 8) & 0xFF, reg_l + 1) - def set_hw_offsets(self, x:int, y:int, z:int): + 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 --- + ## + # --- READ functions --- ## def read_magnet_raw(self): @@ -164,6 +184,7 @@ 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 @@ -171,7 +192,11 @@ def read_reg(self, reg): 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) + 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).""" @@ -184,8 +209,8 @@ def read_magnet_calibrated_norm(self): 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) - + 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. @@ -231,11 +256,18 @@ def read_hw_offsets(self): 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) - + 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: @@ -245,17 +277,14 @@ def read_registers(self, start_addr: int, length: int) -> bytes: def read_all(self) -> dict: """Grouped reading useful for debug & logs.""" raw = self.read_magnet_raw() - uT = self.read_magnet_uT() + 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 - } + T = self.read_temperature_c() + st = self.read_status() + return {"raw": raw, "uT": uT, "cal_norm": cal, "tempC": T, "status": st} ## - # --- CALIBRATIONS --- + # --- CALIBRATIONS --- ## def set_calibrate_step(self, xoff, yoff, zoff, xscale, yscale, zscale): @@ -273,13 +302,15 @@ def calibrate_minmax_2d(self, samples=300, delay_ms=20): Slowly rotate the board FLAT during acquisition. Updates x_off, y_off, x_scale, y_scale (leaves Z unchanged). """ - xmin = ymin = 1e9 + xmin = ymin = 1e9 xmax = ymax = -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) + x, y, _ = self.read_magnet() + xmin = min(xmin, x) + xmax = max(xmax, x) + ymin = min(ymin, y) + ymax = max(ymax, y) time.sleep_ms(delay_ms) self.x_off = (xmax + xmin) / 2.0 @@ -296,14 +327,17 @@ 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 + 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) + xmin = min(xmin, x) + xmax = max(xmax, x) + ymin = min(ymin, y) + ymax = max(ymax, y) + zmin = min(zmin, z) + zmax = max(zmax, z) time.sleep_ms(delay_ms) self.x_off = (xmax + xmin) / 2.0 @@ -330,28 +364,37 @@ def calibrate_quality(self, samples_check=200, delay_ms=10): 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 = [] + 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) + xs.append(xc) + ys.append(yc) + zs.append(zc) time.sleep_ms(delay_ms) # Means (residual center) - mx = sum(xs)/len(xs); my = sum(ys)/len(ys); mz = sum(zs)/len(zs) + mx = sum(xs) / len(xs) + my = sum(ys) / len(ys) + mz = sum(zs) / len(zs) # Radius dispersion in the XY plane - import math as _m - radii = [_m.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 = _m.sqrt(r_var) + + 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 _m.sqrt(v) - sx = _std(xs, mx); sy = _std(ys, my); sz = _std(zs, mz) + 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 { @@ -373,12 +416,12 @@ def calibrate_step(self): # Simple alias to calibrate_minmax_3d return self.calibrate_minmax_3d() - ## - # --- Heading functions --- + ## + # --- Heading functions --- ## - _heading_offset_deg = 0.0 # user setting: align your physical 0° - _declination_deg = 0.0 # true north vs magnetic north + _heading_offset_deg = 0.0 # user setting: align your physical 0° + _declination_deg = 0.0 # true north vs magnetic north # angle filter via vector averaging (robust around 0/360) _hf_alpha = 0.0 _hf_cos = None @@ -412,7 +455,7 @@ 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 - import math + 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: @@ -435,7 +478,7 @@ def heading_from_vectors(self, x, y, z, calibrated=True): - calibrated=True: applies offset/scale per axis (recommended) - flat only (uses XY) """ - import math + if calibrated: x = (x - self.x_off) / (self.x_scale or 1.0) y = (y - self.y_off) / (self.y_scale or 1.0) @@ -457,7 +500,7 @@ def heading_with_tilt_compensation(self, read_accel): Tilt-compensated compass (if an accelerometer is available). read_accel() must return (ax, ay, az) ~g. """ - import math + x, y, z = self.read_magnet() # 3D calibration x = (x - self.x_off) / (self.x_scale or 1.0) @@ -466,12 +509,16 @@ def heading_with_tilt_compensation(self, read_accel): ax, ay, az = read_accel() # roll / pitch from accelerometer - roll = math.atan2(ay, az) - pitch = math.atan2(-ax, math.sqrt(ay*ay + az*az)) + 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) + 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) @@ -479,29 +526,29 @@ 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 + 22.5)//45) + dirs = ["N", "NE", "E", "SE", "S", "SW", "W", "NW", "N"] + idx = int((angle + 22.5) // 45) return dirs[idx] ## - # --- Power/reset functions --- + # --- 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") + 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 + 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) + 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) @@ -512,10 +559,10 @@ def soft_reset(self, wait_ms: int = 10): 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 + r |= 1 << 5 # SOFT_RST self.setReg(r, LIS2MDL_CFG_REG_A) try: - import time; time.sleep_ms(wait_ms) + time.sleep_ms(wait_ms) except: pass @@ -525,13 +572,13 @@ def reboot(self, wait_ms: int = 10): The bit auto-clears. """ r = self.read_reg(LIS2MDL_CFG_REG_A) - r |= (1 << 6) # REBOOT + r |= 1 << 6 # REBOOT self.setReg(r, LIS2MDL_CFG_REG_A) try: - import time; time.sleep_ms(wait_ms) + time.sleep_ms(wait_ms) except: 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 \ No newline at end of file + return (self.read_reg(LIS2MDL_CFG_REG_A) & 0b11) == 0b11 From 61d93d319d58a84513e903025f710b88fc157d4e Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Thu, 11 Dec 2025 11:17:41 +0100 Subject: [PATCH 09/15] lis2mdl : fix magic number errors. --- lib/lis2mdl/exemple/magnet_test.py | 40 +++++++++++++++++++++--------- lib/lis2mdl/lis2mdl/device.py | 4 +-- 2 files changed, 30 insertions(+), 14 deletions(-) diff --git a/lib/lis2mdl/exemple/magnet_test.py b/lib/lis2mdl/exemple/magnet_test.py index d9faec7b..cb3a0cf1 100644 --- a/lib/lis2mdl/exemple/magnet_test.py +++ b/lib/lis2mdl/exemple/magnet_test.py @@ -4,6 +4,22 @@ 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 @@ -254,10 +270,10 @@ def test_reads(dev): # MAGNITUDE B = dev.magnitude_uT() print( - f"magnitude_uT: |B|={B:.1f} µT (Earth ~25–65 µT). =>", - "OK" if 5.0 <= B <= 200.0 else "FAIL", + f"magnitude_uT: |B|={B:.1f} µT (Earth ~25-65 µT). =>", + "OK" if MAGNETIC_FIELD_MIN <= B <= MAGNETIC_FIELD_MAX else "FAIL", ) - ok &= 5.0 <= B <= 200.0 # wide, since local disturbances are possible + ok &= MAGNETIC_FIELD_MIN <= B <= MAGNETIC_FIELD_MAX # wide, since local disturbances are possible # CALIBRATION NORM xc, yc, zc = dev.read_magnet_calibrated_norm() @@ -276,8 +292,8 @@ def test_reads(dev): ok_temp = ( isinstance(t1, float) and isinstance(t2, float) - and (-100.0 < t1 < 150.0) - and (-100.0 < t2 < 150.0) + and (TEMP_MIN < t1 < TEMP_MAX) + and (TEMP_MIN < t2 < TEMP_MAX) ) print("Temp check =>", "OK" if ok_temp else "FAIL") ok &= ok_temp @@ -331,8 +347,8 @@ def test_calibrate_2d(dev): ) print("r_std_xy =", "{:.3f}".format(q["r_std_xy"]), " (smaller = better)") - ok_center = abs(q["mean_xy"][0]) < 0.2 and abs(q["mean_xy"][1]) < 0.2 - ok_round = q["anisotropy_xy"] < 1.4 # realistic tolerances + 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 @@ -362,8 +378,8 @@ def test_calibrate_3d(dev): "{:.3f}".format(q["r_std_xy"]), ) - ok_center = abs(q["mean_xy"][0]) < 0.3 and abs(q["mean_xy"][1]) < 0.3 - ok_round = q["anisotropy_xy"] < 1.6 + 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 @@ -425,7 +441,7 @@ def test_heading_offset_declination(dev): # difference mod 360 diff = (a1 - a0) % 360.0 # accept ~17° ±3° (due to noise/quantization/filtering) - ok = (14.0 <= diff <= 20.0) or (340.0 <= diff <= 346.0) # wrap + 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", @@ -456,7 +472,7 @@ def test_heading_span_turn(dev, duration_ms=6000, step_ms=50): if span < 0: span += 360.0 print(f"min={minA:.1f}°, max={maxA:.1f}°, span≈{span:.1f}°") - ok = span > 300.0 # we expect almost 360° for a full turn + 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 @@ -483,7 +499,7 @@ def test_heading_filter_wrap(dev): 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 < 90.0 for i in range(1, len(outs))) + 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 diff --git a/lib/lis2mdl/lis2mdl/device.py b/lib/lis2mdl/lis2mdl/device.py index d34d1ec7..3f1e754b 100644 --- a/lib/lis2mdl/lis2mdl/device.py +++ b/lib/lis2mdl/lis2mdl/device.py @@ -133,8 +133,8 @@ def use_spi_4wire(self, enable: bool): self.setReg(reg, LIS2MDL_CFG_REG_C) # --- Compass: heading offset & declination (software) --- - _heading_offset_deg = 0.0 - _declination_deg = 0.0 + _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) From 2f1559f2b72d0d6de0dd0832e31098399c2a033e Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Thu, 11 Dec 2025 14:24:39 +0100 Subject: [PATCH 10/15] lis2mdl : fix remove duplicate. --- lib/lis2mdl/exemple/magnet_test.py | 2 +- lib/lis2mdl/lis2mdl/device.py | 11 ++--------- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/lib/lis2mdl/exemple/magnet_test.py b/lib/lis2mdl/exemple/magnet_test.py index cb3a0cf1..887cb042 100644 --- a/lib/lis2mdl/exemple/magnet_test.py +++ b/lib/lis2mdl/exemple/magnet_test.py @@ -400,7 +400,7 @@ def test_heading_after_calib(dev, n=200, delay_ms=20): 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.") + print("=> If you rotated ~one complete turn flat, we expect ~300-360° span.") def run_all_calibration_tests(dev): diff --git a/lib/lis2mdl/lis2mdl/device.py b/lib/lis2mdl/lis2mdl/device.py index 3f1e754b..235b409a 100644 --- a/lib/lis2mdl/lis2mdl/device.py +++ b/lib/lis2mdl/lis2mdl/device.py @@ -420,19 +420,11 @@ def calibrate_step(self): # --- Heading functions --- ## - _heading_offset_deg = 0.0 # user setting: align your physical 0° - _declination_deg = 0.0 # true north vs magnetic north # angle filter via vector averaging (robust around 0/360) _hf_alpha = 0.0 _hf_cos = None _hf_sin = None - def set_heading_offset(self, deg: float): - self._heading_offset_deg = float(deg) - - def set_declination(self, deg: float): - self._declination_deg = float(deg) - def set_heading_filter(self, alpha: float): """ alpha=0 -> no filtering. 0.1..0.3 = light/medium smoothing. @@ -465,8 +457,9 @@ def _filter_heading(self, angle_deg): 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 > 1e-6: + if norm > NORMALIZATION_THRESHOLD: self._hf_cos /= norm self._hf_sin /= norm ang = math.degrees(math.atan2(self._hf_sin, self._hf_cos)) From b0cecf6ecb17040d6863c5ab77604ae7ef57c534 Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Tue, 10 Mar 2026 11:37:10 +0100 Subject: [PATCH 11/15] lis2mdl: Fix inverted axes and document calibration requirement. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Axes X and Y were inverted which caused a 90° heading offset. Calibration is required before usage because offsets depend on the board. --- .../{exemple => examples}/magnet_compass.py | 5 +++++ .../{exemple => examples}/magnet_fieldForce.py | 4 ++++ lib/lis2mdl/{exemple => examples}/magnet_test.py | 0 lib/lis2mdl/lis2mdl/device.py | 16 ++++++++-------- lib/lis2mdl/manifest.py | 6 ++++++ 5 files changed, 23 insertions(+), 8 deletions(-) rename lib/lis2mdl/{exemple => examples}/magnet_compass.py (71%) rename lib/lis2mdl/{exemple => examples}/magnet_fieldForce.py (70%) rename lib/lis2mdl/{exemple => examples}/magnet_test.py (100%) diff --git a/lib/lis2mdl/exemple/magnet_compass.py b/lib/lis2mdl/examples/magnet_compass.py similarity index 71% rename from lib/lis2mdl/exemple/magnet_compass.py rename to lib/lis2mdl/examples/magnet_compass.py index 12d1519c..a101de3e 100644 --- a/lib/lis2mdl/exemple/magnet_compass.py +++ b/lib/lis2mdl/examples/magnet_compass.py @@ -7,8 +7,13 @@ 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/exemple/magnet_fieldForce.py b/lib/lis2mdl/examples/magnet_fieldForce.py similarity index 70% rename from lib/lis2mdl/exemple/magnet_fieldForce.py rename to lib/lis2mdl/examples/magnet_fieldForce.py index b8efbc89..8a2123e8 100644 --- a/lib/lis2mdl/exemple/magnet_fieldForce.py +++ b/lib/lis2mdl/examples/magnet_fieldForce.py @@ -7,6 +7,10 @@ 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") diff --git a/lib/lis2mdl/exemple/magnet_test.py b/lib/lis2mdl/examples/magnet_test.py similarity index 100% rename from lib/lis2mdl/exemple/magnet_test.py rename to lib/lis2mdl/examples/magnet_test.py diff --git a/lib/lis2mdl/lis2mdl/device.py b/lib/lis2mdl/lis2mdl/device.py index 235b409a..b145a8b4 100644 --- a/lib/lis2mdl/lis2mdl/device.py +++ b/lib/lis2mdl/lis2mdl/device.py @@ -9,13 +9,13 @@ class LIS2MDL(object): # Default calibration offsets and scales for the magnetometer - x_off = -132.0 - y_off = -521.5 - z_off = -891.0 + x_off = 0 + y_off = 0 + z_off = 0 - x_scale = 273.0 - y_scale = 313.5 - z_scale = 295.0 + x_scale = 1 + y_scale = 1 + z_scale = 1 def __init__( self, @@ -476,7 +476,7 @@ def heading_from_vectors(self, x, y, z, calibrated=True): 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(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) @@ -520,7 +520,7 @@ def direction_label(self, angle=None): if angle is None: angle = self.heading_flat_only() dirs = ["N", "NE", "E", "SE", "S", "SW", "W", "NW", "N"] - idx = int((angle + 22.5) // 45) + idx = int((angle) // 45) return dirs[idx] ## diff --git a/lib/lis2mdl/manifest.py b/lib/lis2mdl/manifest.py index e69de29b..6dd49526 100644 --- a/lib/lis2mdl/manifest.py +++ b/lib/lis2mdl/manifest.py @@ -0,0 +1,6 @@ +metadata( + description="Driver of LIS2MDL magnetometer sensor.", + version="0.0.1", +) + +package("lis2mdl") From f9d08aa6a2272b656bc71e876b896bb2c25f84fb Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Tue, 10 Mar 2026 11:40:19 +0100 Subject: [PATCH 12/15] lis2mdl: Fix with ruff. --- lib/lis2mdl/examples/magnet_compass.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/lis2mdl/examples/magnet_compass.py b/lib/lis2mdl/examples/magnet_compass.py index a101de3e..52626af2 100644 --- a/lib/lis2mdl/examples/magnet_compass.py +++ b/lib/lis2mdl/examples/magnet_compass.py @@ -13,7 +13,7 @@ while True: angle = mag.heading_flat_only() - + direction = mag.direction_label(angle) print("Cap:", angle, "°", "-", direction) sleep_ms(100) From bfa303b954dba9931c111c71f37c9a0d5a3bbf3a Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Tue, 10 Mar 2026 11:46:41 +0100 Subject: [PATCH 13/15] lis2mdl: Fix copilots errors. --- lib/lis2mdl/lis2mdl/const.py | 2 +- lib/lis2mdl/lis2mdl/device.py | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/lib/lis2mdl/lis2mdl/const.py b/lib/lis2mdl/lis2mdl/const.py index 382281f8..c8150d01 100644 --- a/lib/lis2mdl/lis2mdl/const.py +++ b/lib/lis2mdl/lis2mdl/const.py @@ -31,7 +31,7 @@ LIS2MDL_TEMP_OUT_H_REG = const(0x6F) # Temperature output high byte # Interrupt control and source registers -LIS2MDL_INT_CRTL_REG = const(0x63) # Interrupt control register +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 index b145a8b4..bec68eed 100644 --- a/lib/lis2mdl/lis2mdl/device.py +++ b/lib/lis2mdl/lis2mdl/device.py @@ -3,7 +3,7 @@ from machine import I2C from lis2mdl.const import * -import time +from time import sleep_ms import math @@ -24,7 +24,7 @@ def __init__( odr_hz=10, temp_comp=True, low_power=False, - drdy_pin=False, + drdy_enable=False, ): # Initialize the LIS2MDL sensor with the given I2C interface and settings. self.i2c = i2c @@ -35,7 +35,7 @@ def __init__( # 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: - time.sleep_ms(10) # Small delay for reset to complete + sleep_ms(10) # Small delay for reset to complete except: pass @@ -50,7 +50,7 @@ def __init__( 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_pin else 0x00) + cfg_c = 0x10 | (0x01 if drdy_enable else 0x00) self.setReg(cfg_c, LIS2MDL_CFG_REG_C) ## @@ -311,7 +311,7 @@ def calibrate_minmax_2d(self, samples=300, delay_ms=20): xmax = max(xmax, x) ymin = min(ymin, y) ymax = max(ymax, y) - time.sleep_ms(delay_ms) + sleep_ms(delay_ms) self.x_off = (xmax + xmin) / 2.0 self.y_off = (ymax + ymin) / 2.0 @@ -338,7 +338,7 @@ def calibrate_minmax_3d(self, samples=600, delay_ms=20): ymax = max(ymax, y) zmin = min(zmin, z) zmax = max(zmax, z) - time.sleep_ms(delay_ms) + sleep_ms(delay_ms) self.x_off = (xmax + xmin) / 2.0 self.y_off = (ymax + ymin) / 2.0 @@ -373,7 +373,7 @@ def calibrate_quality(self, samples_check=200, delay_ms=10): xs.append(xc) ys.append(yc) zs.append(zc) - time.sleep_ms(delay_ms) + sleep_ms(delay_ms) # Means (residual center) mx = sum(xs) / len(xs) @@ -555,7 +555,7 @@ def soft_reset(self, wait_ms: int = 10): r |= 1 << 5 # SOFT_RST self.setReg(r, LIS2MDL_CFG_REG_A) try: - time.sleep_ms(wait_ms) + sleep_ms(wait_ms) except: pass @@ -568,7 +568,7 @@ def reboot(self, wait_ms: int = 10): r |= 1 << 6 # REBOOT self.setReg(r, LIS2MDL_CFG_REG_A) try: - time.sleep_ms(wait_ms) + sleep_ms(wait_ms) except: pass From c76068c5ddef9612f51d8435cf2d96a2cf3e0c66 Mon Sep 17 00:00:00 2001 From: Charly-sketch Date: Wed, 11 Mar 2026 11:01:24 +0100 Subject: [PATCH 14/15] lis2mdl: Resolve reviewer correction. --- lib/lis2mdl/README.md | 38 ++--------------------- lib/lis2mdl/examples/magnet_compass.py | 2 +- lib/lis2mdl/examples/magnet_fieldForce.py | 2 +- lib/lis2mdl/examples/magnet_test.py | 6 ---- lib/lis2mdl/lis2mdl/device.py | 4 +-- 5 files changed, 7 insertions(+), 45 deletions(-) diff --git a/lib/lis2mdl/README.md b/lib/lis2mdl/README.md index 768be723..a1043e41 100644 --- a/lib/lis2mdl/README.md +++ b/lib/lis2mdl/README.md @@ -50,32 +50,9 @@ Example setup: ```python from machine import I2C, Pin -from device import LIS2MDL +from lis2mdl import LIS2MDL -i2c = I2C(1, scl=Pin(22), sda=Pin(21)) -mag = LIS2MDL(i2c) -``` - ---- - -## 🧩 Installation - -### 1. Copy files to your board - -Upload both files using Thonny, mpremote, or ampy: - -``` -device.py -lis2mdl/const.py -``` - -### 2. Import and initialize - -```python -from machine import I2C, Pin -from device import LIS2MDL - -i2c = I2C(1, scl=Pin(22), sda=Pin(21)) +i2c = I2C(1) mag = LIS2MDL(i2c) ``` @@ -242,7 +219,7 @@ print("Register dump:", regs) ```python from machine import I2C, Pin -from device import LIS2MDL +from lis2mdl import LIS2MDL import time i2c = I2C(1, scl=Pin(22), sda=Pin(21)) @@ -257,15 +234,6 @@ while True: --- -## 🧾 Notes - -* The LIS2MDL outputs magnetic data in **LSB**, with a conversion factor of **0.15 µT/LSB**. -* Temperature readings are **relative only** (not absolute). -* Calibration should be repeated if the sensor environment changes (metal nearby, relocation, etc.). -* Tilt compensation requires an external **accelerometer** or IMU (e.g., LIS3DH, MPU6050, BNO055). - ---- - ## 📚 References * [STMicroelectronics LIS2MDL Datasheet](https://www.st.com/resource/en/datasheet/lis2mdl.pdf) diff --git a/lib/lis2mdl/examples/magnet_compass.py b/lib/lis2mdl/examples/magnet_compass.py index 52626af2..fb2d79c4 100644 --- a/lib/lis2mdl/examples/magnet_compass.py +++ b/lib/lis2mdl/examples/magnet_compass.py @@ -1,4 +1,4 @@ -# exemple of heading reading with direction label +# Example of heading reading with direction label from time import sleep_ms from machine import I2C from lis2mdl.device import LIS2MDL diff --git a/lib/lis2mdl/examples/magnet_fieldForce.py b/lib/lis2mdl/examples/magnet_fieldForce.py index 8a2123e8..9e8bbabf 100644 --- a/lib/lis2mdl/examples/magnet_fieldForce.py +++ b/lib/lis2mdl/examples/magnet_fieldForce.py @@ -1,4 +1,4 @@ -# exemple of heading reading with direction label +# Example of heading reading with direction label from time import sleep_ms from machine import I2C from lis2mdl.device import LIS2MDL diff --git a/lib/lis2mdl/examples/magnet_test.py b/lib/lis2mdl/examples/magnet_test.py index 887cb042..875b2800 100644 --- a/lib/lis2mdl/examples/magnet_test.py +++ b/lib/lis2mdl/examples/magnet_test.py @@ -513,12 +513,6 @@ def run_heading_tests(dev): all_ok &= test_heading_filter_wrap(dev) print("\n=== HEADING summary:", "OK ✅" if all_ok else "Partial ⚠️ (see details)") - -def _bits(v, hi, lo): - m = (1 << (hi - lo + 1)) - 1 - return (v >> lo) & m - - def test_power_modes(dev): print("\n=== TEST POWER MODES ===") ok = True diff --git a/lib/lis2mdl/lis2mdl/device.py b/lib/lis2mdl/lis2mdl/device.py index bec68eed..04b5e019 100644 --- a/lib/lis2mdl/lis2mdl/device.py +++ b/lib/lis2mdl/lis2mdl/device.py @@ -36,7 +36,7 @@ def __init__( self.setReg(0x20, LIS2MDL_CFG_REG_A) # SOFT_RST=1 (not 0x10) try: sleep_ms(10) # Small delay for reset to complete - except: + except Exception: pass # Configure the sensor's operating mode, output data rate, and other settings. @@ -569,7 +569,7 @@ def reboot(self, wait_ms: int = 10): self.setReg(r, LIS2MDL_CFG_REG_A) try: sleep_ms(wait_ms) - except: + except Exception: pass def is_idle(self) -> bool: From 4c4ac80e46da1db46fed24b2d31715b1d542708b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20NEDJAR?= Date: Wed, 11 Mar 2026 16:36:00 +0100 Subject: [PATCH 15/15] lis2mdl: Add test scenario for magnetometer driver. --- tests/runner/executor.py | 8 +++ tests/scenarios/lis2mdl.yaml | 99 ++++++++++++++++++++++++++++++++++++ 2 files changed, 107 insertions(+) create mode 100644 tests/scenarios/lis2mdl.yaml 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]