Mise en œuvre du magnétomètre 3D LIS2MDL et programmation d’une boussole

L’exemple qui suit montre comment mesurer le champ magnétique selon trois axes orthogonaux à l’aide du capteur MEMS LIS2MDL de STMicroelectronics.

Matériel requis

La carte NUCLEO-WB55
Une carte d’extension I2C équipée du capteur MEMS LIS2MDL (par exemple la X-NUCLEO-IKS01A3)

Pour commencer

Nous allons dans ce premier temps mettre en œuvre un script qui mesure les valeurs du champ magnétique à proximité de la carte NUCLEO.

Vous pouvez télécharger les scripts MicroPython de ce tutoriel (entre autres) en cliquant ici

Le fichier lis2mdl.py est la bibliothèque contenant les classes pour gérer ce capteur 3D de champs magnétiques. Il doit être copié dans le répertoire du disque USB virtuel associé à la NUCLEO-WB55 : PYBFLASH.

Éditez maintenant le script main.py et copiez-y le code qui suit :

# Objet du script :
# Mesure le champ magnétique selon 3 axes orthogonaux (en µT) toutes les 
# secondes et calcule son intensité (son module).
# Nécessite un magnétomètre MEMS LIS2MDL 
# (par exemple celui du shield X-NUCLEO-IKS01A3).
# Intensité du champ terreste : 30µT à 60µT (en théorie)

# Importation des fonctions nécessaires
from machine import I2C
import lis2mdl
from time import sleep_ms
from math import sqrt

# On utilise l'I2C n°1 de la carte NUCLEO-WB55 pour communiquer avec le capteur
i2c = I2C(1)

# Pause d'une seconde pour laisser à l'I2C le temps de s'initialiser
sleep_ms(1000)

# Instanciation du magnétomètre
magnetometer = lis2mdl.LIS2MDL(i2c)

while True:

	# Lecture du champ magnétique sur les trois axes orthogonaux
	magnetometer.get()
	bx = magnetometer.x()
	by = magnetometer.y()
	bz = magnetometer.z()

	# Affiche les composantes du champ magnétique (entiers signés avec 3 digits)
	print("Composantes du champ magnétique :")
	print(" - Selon l'axe Ox : %6d µT" % bx)
	print(" - Selon l'axe Oy : %6d µT" % by)
	print(" - Selon l'axe Oz : %6d µT" % bz)

	# Calcul du module du champ magnétique
	b = sqrt(bx*bx + by*by + bz*bz)

	# Affiche le module du champ magnétique avec une décimale après la virgule
	print("Intensité du champ magnétique : %.1f µT" % b)
	print("")
	
	# Temporisation
	sleep_ms(1000)

Vous pouvez à présent lancer le script avec la combinaison de touches CTRL + D dans le terminal série et observer les amplitudes magnétiques mesurées par le capteur :

Ces valeurs “brutes” de champ magnétique sont cependant faussées de quantités qui changent d’un capteur à l’autre et selon l’environnement. Ce script est donc parfaitement inutile en l’état. Pour que les valeurs renvoyées par le LIS2MDL soient interprétables, il faut d’abord le calibrer afin d’évaluer quelles sont les corrections à appliquer à ses mesures. C’est ce que nous allons expliquer maintenant.

Programmer une boussole avec le LIS2MDL

La plupart des capteurs MEMS sont inutilisables s’ils ne sont pas préalablement calibrés. C’est un principe de base prévalant à toute expérience de physique sérieusement menée : avant d’utiliser un capteur, il faut toujours s’assurer qu’il est correctement calibré pour être sûr d’interpréter correctement ce que l’on va mesurer !

Le MEMS le plus sensible à cette problématique est sans doute le magnétomètre. Nous allons donc expliquer pourquoi chaque LIS2MDL doit impérativement être calibré et nous donnerons dans cette section une procédure de calibrage simplifiée pour programmer une boussole précise uniquement lorsque le PCB qui supporte le LIS2MDL sera horizontal.

Pour aller plus loin, vous pourrez consulter cette référence, ou encore celle-ci qui ont inspiré les codes qui suivent.

Pourquoi faut-il calibrer le LIS2MDL ?

Nous commençons par raisonner sur le cas simplifié où le plan (O_x, O_y) du PCB sur lequel le magnétomètre est soudé reste parfaitement horizontal (nous reviendrons sur ce point à la fin).

Rappelons qu’un magnétomètre mesure l’intensité de tous les champs magnétiques dans son environnement. En l’absence de sources de champ magnétique “artificielles” proches, il produit donc un signal maximum selon l’un de ses axes lorsque celui-ci est aligné avec l’axe nord-sud terrestre.

Mais le champ que le magnétomètre mesure n’est en général pas exactement celui de la Terre car il est perturbé par d’autres champs magnétiques à proximité, et/ou modifié par :

Des distorsions “hard iron” ;
Des distorsions “soft iron” ;
Un effet de”tilt” éventuel.

Voyons cela plus en détails…

Les distorsions hard iron

Les distorsions hard iron proviennent d’éléments sources de champs magnétiques présents dans le système sur lequel le magnétomètre est intégré. Par exemple, si le LIS2MDL est installé dans un smartphone, l’aimant du haut-parleur ou la batterie perturberont ses mesures.

Ces distorsions sont en général très fortes et faussent complètement les mesures du magnétomètre. Heureusement, elles sont isotropes (i.e. identiques dans toutes les directions) et, de ce fait, elles s’ajoutent au champ magnétique terrestre. En conséquence, les valeurs lues par le capteur peuvent être corrigées simplement des distorsions hard iron en soustrayant un décalage (offset) constant propre à chacun des axes O_x et O_y.

En pratique nous utiliserons la méthode facile et exacte présentée sur ce site. On commence par placer la carte NUCLEO-WB55 et le shield X-NUCLEO-IKS01A3 dans le plan horizontal (O_x, O_y). On fait tourner l’ensemble dans ce plan pendant un temps suffisant pour déterminer les valeurs minimum et maximum du champ magnétique selon les axes O_x et O_y. Une fois ces valeurs extrêmes connues, on en déduit directement les offsets qu’il faut soustraire aux mesures selon ces mêmes axes pour compenser les distorsions.

Le graphique qui suit illustre la procédure de correction des distorsions hard iron en reprenant les mêmes noms de variables que ceux des scripts MicroPython que nous présenterons par la suite :

Au départ, les mesures du magnétomètre, lorsqu’on le fait tourner dans le plan horizontal (O_x, O_y) sont toutes contenues à l’intérieur ( la région bleue) du périmètre de l’ellipse rouge. Cette ellipse n’est pas centrée en (0, 0). La correction va donc consister, à partir des amplitudes min_bx, max_bx, min_by et max_by, à calculer les composantes offset_x et offset_y pour recentrer les mesures autour de (0, 0).

Les distorsions soft iron

Les distorsions soft iron sont également dues aux matériaux qui entourent le magnétomètre sur son PCB, mais spécifiquement à ceux qui ont la capacité de déformer les lignes de champ magnétique (plutôt que d’en générer comme le faisaient les perturbateurs hard iron). Les matériaux ferromagnétiques tels que le fer, le nickel et le cobalt génèrent des effets soft iron.

A l’opposé des distorsions hard iron, les distorsions soft iron sont anisotropes. Cela signifie qu’on ne peut pas les compenser simplement par des soustractions d’offsets. Pour les corriger rigoureusement, il faut à la fois appliquer une rotation et un facteur d’échelle aux valeurs lues.

Nous utiliserons la méthode approximative et simplifiée présentée sur ce site pour ne pas surcharger le code. Mais cela suffira car, heureusement, ces distorsions sont faibles et ne perturbent que très peu la détermination du nord.

Le graphique suivant montre en quoi consiste la procédure de correction des distorsions soft iron :

L’ellipse étant centrée en (0, 0) par suite des corrections hard iron, on réaligne à présent son grand axe avec O_x par une rotation, puis on applique des homothéties selon 0_x et 0_y pour que les mesures du magnétomètre soient finalement toutes contenues dans un cercle.

Nous proposons plus loin des scripts qui calculent puis compensent les distorsions soft iron et hard iron pour simuler une boussole “à plat”. Dans ces scripts, nous généralisons le raisonnement ci-dessus en intégrant l’axe O_z dans la procédure de calibration. Cette modification ne remet pas en question les explications que nous avons données. Mais elle sera utile lorsque nous programmerons le cas général pour intégrer l’effet de tilt à notre boussole électronique.

L’effet du tilt sur la détermination du cap

Rappelons-nous l’hypothèse simplificatrice de départ : tous nos raisonnements sont valables dans le cas où le plan du PCB reste parfaitement horizontal. Que se passe-t’il si cette condition n’est plus remplie et comment surmonter cette difficulté supplémentaire ?

Si le plan du PCB n’est plus tangent à la surface du globe terrestre (i.e. horizontal), la précision de notre boussole s’effondre. Ce ne sera pas une surprise pour celles et ceux qui ont déjà utilisé une véritable boussole, qui savent bien que son aiguille n’indique la direction correcte du nord (magnétique) que si elle est maintenue parfaitement horizontale. Une correction d’inclinaison ou, en anglais, de tilt s’imposera si on veut mesurer correctement la direction du nord lorsque le PCB n’est plus horizontal. Ce perfectionnement sera nécessaire, par exemple, si on souhaite utiliser la boussole pour donner un cap à un drone volant ou à un robot évoluant sur un terrain accidenté.

Petite disgression géophysique : puisque les lignes du champ géomagnétique sont rigoureusement tangentes à la surface du sol à l’équateur, la correction de tilt n’y sera pratiquement pas nécessaire (composante du champ magnétique quasiment nulle selon O_z). Et puisqu’elles sont presque verticales à proximité des pôles (magnétiques) la correction de tilt y sera indispensable (composante du champ magnétique maximale selon O_z).

On peut corriger l’effet de tilt du magnétomètre à l’aide d’un accéléromètre (le LIS2DW12 qui accompagne le LIS2MDL sur la X-NUCLEO-IKS01A3 fera opportunément l’affaire). L’accéléromètre permet de mesurer l’angle entre le PCB et la verticale sur la base de sa mesure de l’accélération de la pesanteur (qui est justement verticale et vaut 9,81 m²/s²). Une fois cet angle déterminé, on peut appliquer une rotation aux mesures du magnétomètre pour déduire les valeurs du champ magnétique dans le plan horizontal à partir de celles relevées dans le plan incliné du PCB.

Le graphique qui suit illustre la problématique du tilt et la méthode pour sa résolution :

Dans le plan du magnétomètre, on mesure un vecteur champ magnétique B. En pratique nous avons besoin des composantes de B après sa projection dans le plan horizontal, c’est à dire bx et by dans le repère (O_xy). Celles-ci permettront de nous ramener à l’exemple en deux dimensions discuté jusqu’ici et de calculer directement la direction du nord magnétique par la formule atan2(b_y/b_x). Pour projeter les composantes du vecteur B nous avons bien évidemment besoin de connaître la désorientation du plan du magnétomètre par rapport au plan horizontal. Celle-ci est déterminée au moyen d’un accéléromètre qui “connaît” à tout instant la direction (O_z) grâce à sa mesure du vecteur accélération de la pesanteur.

Important : Puisque nous utilisons un accéléromètre pour évaluer l’inclinaison du magnétomètre, il faut prendre soin de n’utiliser la boussole que lorsque le système dans lequel elle est intégrée est soit immobile, soit en mouvement rectiligne uniforme sur une surface aussi lisse que possible pour limiter les vibrations. Si ces contraintes ne sont pas respectées, le vecteur accélération mesuré ne sera plus dirigé vers le centre de la Terre, et l’inclinaison ainsi que le cap ne seront plus déterminés correctement. Cette limitation pourra être partiellement levée en utilisant un algorithme de fusion de données (voir ci-dessous).

Vous trouverez un sketch Arduino qui traite ce problème sur ce site pour d’autres capteurs que les nôtres. Vous pouvez également consulter la note d’application AN3192 de STMicroelectronics qui explique les calculs pour compenser l’effet de tilt avec le composant LSM303DLH.

Nous proposons plus loin un code qui corrige l’effet de tilt pour simuler une boussole “en trois dimensions”, en faisant appel à l’accéléromètre LIS2DW12 intégré à la X-NUCLEO-IKS01A3.

Séquence des corrections

Pour résumer et en pratique, la correction complète de la lecture du magnétomètre se fait en 4 étapes, dans cet ordre :

Détermination des valeurs minimum et maximum du champ magnétique mesuré suivant les trois axes du magnétomètre ;
Correction des distorsions hard iron isotropes ;
Correction des distorsions soft iron anisotropes ;
Correction du tilt pour obtenir les composantes du champ magnétique dans le plan horizontal.

Si le magnétomètre se déplace suivant une trajectoire accidentée, il faudra régulièrement corriger son tilt. En revanche, si on prend soin de le conserver horizontal, il ne sera pas nécessaire d’appliquer cette dernière correction.

Complément : la fusion de capteurs

Pour finir, toutes ces corrections nous assurent que la réponse du magnétomètre ne sera pas perturbée par l’environnement de son PCB. Mais si le PCB se déplace, il est évidemment possible que le magnétomètre rencontre des champs magnétiques autres que le champ terrestre, d’intensité comparable ou supérieure, dans son environnement extérieur immédiat. Dans ces cas, sa mesure sera brièvement mais fortement perturbée (exactement comme une boussole en présence d’un aimant).

La stratégie adoptée pour limiter ces perturbations imprévisibles de l’environnement extérieur est la fusion de capteurs ou sensor fusion en anglais. Son principe est le suivant : en plus du magnétomètre, on utilise les données renvoyées au cours du mouvement, à des fréquences différentes, par un gyroscope, un accéléromètre, un baromètre altimétrique, un récepteur GPS… et on les “injecte” dans un algorithme mathématique, un filtre de Kalman par exemple, qui atténue dynamiquement les perturbations et permet de conserver une estimation stable et optimale du cap au cours du temps. On tire ainsi profit de plusieurs capteurs conçus avec des technologies différentes et complémentaires, qui ne sont pas sensibles simultanément aux mêmes perturbations extérieures. Ce tutoriel montre comment réaliser la fusion de capteurs sur la X-NUCLEO-IKS01A3.

Le code pour obtenir les valeurs qui serviront à calibrer la boussole

Vous pouvez télécharger les scripts MicroPython de ce tutoriel (entre autres) en cliquant ici

Voici le script main.py qui permettra de mesurer les valeurs min et max du champ magnétique selon les axes x, y et z :

# Objet du script :

# Obtention des valeurs extrêmes du champ magnétique mesuré par le magnétomètre LIS2MDL 
# selon ses axes x, y, z. Ces valeurs sont nécessaires pour calibrer le LIS2MDL, en 
# l'occurence supprimer les décalages systématiques de ses mesures selon chaque axe.
# (effets "Hard Iron" et "Soft Iron")

# Vous devrez lancer le script et, aussi longtemps que la LED clignote déplacer la
# carte selon des mouvements amples en forme de "8". Les six valeurs qui serviront pour
# le calibrage du LIS2MDL seront finalement affichées sur le port série du USB-USER.

# IMPORTANT : 
# - Pour calibrer le LIS2MDL en vue d'en faire une boussole et suivre un cap, il faudra 
#  faire tourner le LIS2MDL dans l'espace avec des mouvements "en 8" pendant que la LED clignote. 
# - Le processus de calibration doit être réalisé pour chaque LIS2MDL, chaque LIS2MDL
# ayant des décalages propres différents.
# - La réponse du LIS2MDL est fortement modifiée par tous les champs magnétiques présents.
# Lorsque vous utilisez ce script, prenez soin de vous éloigner des haut-parleurs, aimants, 
# masses métalliques contenant du fer ou du nickel et des autres appareils électriques en 
# fonctionnement qui pourraient perturber le processus.

from machine import I2C
import lis2mdl # Pilote du magnétomètre
from time import sleep_ms
from math import sqrt
from pyb import Timer # Pour gérer les timers

# Routine de service de l'interruption (ISR) du Timer 1, pour faire clignoter la LED 1
def blink_LED1(timer):
	pyb.LED(1).toggle()

# On utilise l'I2C n°1 de la carte NUCLEO-WB55 pour communiquer avec le capteur
i2c = I2C(1)

# Pause d'une seconde pour laisser à l'I2C le temps de s'initialiser
sleep_ms(1000)

# Instanciation du magnétomètre
magnetometer = lis2mdl.LIS2MDL(i2c)

# Valeur par défaut des amplitudes du champ magnétique selon chaque axe
DUMMY_VALUE = const(1000)
# Nombre d'itérations pour le calibrage
MAX_ITER = const(100)

# Initialisation des valeurs extêmes pour le calibrage

min_bx = DUMMY_VALUE
min_by = DUMMY_VALUE
min_bz = DUMMY_VALUE

max_bx = -DUMMY_VALUE
max_by = -DUMMY_VALUE
max_bz = -DUMMY_VALUE

print("Démarrage du calibrage")

tim1 = Timer(1, freq= 10) # Fréquence du timer 1 fixée à 10 Hz
tim1.callback(blink_LED1) # Appelle l'ISR de l'interruption de dépassement du timer 1

for index in range (1, MAX_ITER):

	# Lecture du champ magnétique sur les trois axes orthogonaux
	magnetometer.get()
	bx = magnetometer.x()
	by = magnetometer.y()
	bz = magnetometer.z()

	# A chaque itération, on détermine les valeurs min et max du champ magnétique selon x,y et z

	min_bx = min(min_bx, bx)
	min_by = min(min_by, by)
	min_bz = min(min_bz, bz)

	max_bx = max(max_bx, bx)
	max_by = max(max_by, by)
	max_bz = max(max_bz, bz)

	# Module du champ magnétique
	b = sqrt(bx*bx + by*by + bz*bz)

	# Affiche le module du champ magnétique avec une décimale après la virgule
	print("%3d" %index + " - Intensité du champ magnétique : " + "%.1f" % b + " µT")

	sleep_ms(250)

# Arrêt du timer qui fait clignoter la LED 1
tim1.deinit()

# Affichage sur le port série des valeurs extêmes du champ magnétique selon les trois axes
print("Fin calibrage")
print("Valeurs pour le calibrage (en µT):")
print("min_bx : %3d, max_bx : %3d" % (min_bx, max_bx))
print("min_by : %3d, max_by : %3d" % (min_by, max_by))
print("min_bz : %3d, max_bz : %3d" % (min_bz, max_bz))

Vous pouvez à présent lancer le script avec la combinaison de touches CTRL + D dans le terminal série et observer les valeurs calculées pour calibrer votre capteur :

Le code pour simuler une boussole sans compensation d’inclinaison

Vous pouvez télécharger les scripts MicroPython de ce tutoriel (entre autres) en cliquant ici

Voici le script main.py qui permettra de mesurer l’écart angulaire entre l’axe (O_x) du magnétomètre et le vecteur champ magnétique, soit, lorsque le champ magnétique présent le plus intense est celui de la Terre, la déviation angulaire par rapport au nord magnétique. En bref : on simule une boussole ! Comme les vraies boussoles, celle-ci ne fonctionnera correctement que si la NUCLEO-WB55 et son shield X-NUCLEO-IKS01A3 sont posés sur un plan horizontal..

Cet algorithme utilise les valeurs min et max du champ magnétique selon les axes x, y et z fournies par le script de calibrage qui précède.

# Objet du script :
# Simulation d'une boussole sans compensation d'inclinaison à l'aide du magnétomètre LIS2MDL.
# Avant sa mise en œuvre, vous devrez calibrer le LIS2MDL avec le script adéquat.

# Cet exemple nécessite un magnétomètre MEMS LIS2MDL (comme celui sur le shield X-NUCLEO-IKS01A3). 
# Attention, il ne tient pas compte de la déclinaison qui doit être ajoutée à l'angle
# avec le Pôle nord magnétique pour que la boussole pointe vers le pôle nord 
# géographique. La valeur de la déclinaison dépend de votre localisation géographique.

from machine import I2C
import lis2mdl
from time import sleep_ms
from math import atan2, pi

# On utilise l'I2C n°1 de la carte NUCLEO-WB55 pour communiquer avec le capteur
i2c = I2C(1)

# Pause d'une seconde pour laisser à l'I2C le temps de s'initialiser
sleep_ms(1000)

# Instanciation du magnétomètre
magnetometer = lis2mdl.LIS2MDL(i2c)

# Facteur de conversion entre les radians et les degrés pour les angles
RadToDeg = 180 / pi

# Valeurs extrêmes obtenues par le script de calibration

min_bx = const(-99)
max_bx = const(-8)
min_by = const(-23)
max_by = const(66)
min_bz = const(-23)
max_bz = const(30)

# Calcul des décalages (offsets) "Hard Iron" pour chaque axe :

offset_x = (max_bx + min_bx) / 2
offset_y = (max_by + min_by) / 2
offset_z = (max_bz + min_bz) / 2

# Calcul des coefficients pour la correction approximative des 
# distorsions "Soft Iron" pour chaque axe :

avg_delta_x = (max_bx - min_bx) / 2
avg_delta_y = (max_by - min_by) / 2
avg_delta_z = (max_bz - min_bz) / 2

avg_delta = (avg_delta_x + avg_delta_y + avg_delta_z) / 3

scale_x = avg_delta / avg_delta_x
scale_y = avg_delta / avg_delta_y
scale_z = avg_delta / avg_delta_z

while True:

	# Lecture du champ magnétique sur les trois axes orthogonaux
	magnetometer.get()
	bx = magnetometer.x()
	by = magnetometer.y()
	bz = magnetometer.z()

	# On applique les corrections hard iron et soft iron.
	# Ceci permet d'obtenir les valeurs physiques dimensionnées en
	# micro teslas des mesures du magnétomètre.

	bx = (bx - offset_x) * scale_x
	by = (by - offset_y) * scale_y
	bz = (bz - offset_z) * scale_z

	# Calcul de l'orientation du vecteur champ magnétique par rapport 
	# à l'axe (Ox) (angle entre le nord magnétique et l'axe (Ox)).

	angle_deg = atan2(by, bx) * RadToDeg

	# On ne veut pas d'angles négatifs. Lorsque cela se produit on calcule le
	# le complément à 360° pour que l'angle reporté varie entre 0° et 360°.

	if angle_deg < 0:
		angle_deg += 360

	# Affichage de l'écart angulaire entre (Ox) et le nord magnétique
	print("Ecart angulaire avec le nord magnétique : %.1f°" % angle_deg)
	print("")
	
	# Temporisation
	sleep_ms(1000)

Vous pouvez à présent lancer le script avec la combinaison de touches CTRL + D dans le terminal série et tester la boussole :

Le code pour simuler une boussole avec compensation d’inclinaison

Vous pouvez télécharger les scripts MicroPython de ce tutoriel (entre autres) en cliquant ici

Ce dernier script main.py introduit la compensation d’inclinaison. A présent, vous pouvez incliner la X-NUCLEO-IKS01A3 dans l’espace et obtenir malgré tout des valeurs pertinentes de la direction du nord magnétique. Cet algorithme est adapté de la note d’application AN3192 de STMicroelectronics que vous pouvez télécharger ici. Il nécessite le magnétomètre LIS2MDL et l’accéléromètre LIS2DW12.

Sa mise en œuvre est loin d’être évidente compte tenu des adaptations nécessaires sur la définition des axes (a_x, a_y, a_z) du LIS2DW12 et (b_x, b_y, b_z) du LIS2MDL avant le calcul des angles d’Euler. De très précieuses explications sont données aux pages 19 et 20 de AN3192 pour obtenir les bonnes formules pour la compensation. Dans notre cas, conformément à la définition des axes des MEMS sur le X-NUCLEO-IKS01A3 donnée ici ; dans cet ordre il faut :

Permuter les axes x et y du LIS2DW12 ;
Inverser l’axe y du LIS2DW12 ;
Inverser l’axe y du LIS2MDL.

De cette façon, les axes des deux MEMS sont alignés et orientés dans le même sens : x pointe vers le nord, y pointe vers l’ouest et z pointe vers le haut. On applique ensuite les formules données par AN3192, sans modifications, pour calculer les angles d’Euler.

# Objet du script :
# Simulation d'une boussole avec compensation d'inclinaison à l'aide du magnétomètre LIS2MDL
# et de l'accéléromètre LIS2DW12.

# Cet exemple nécessite un shield X-NUCLEO-IKS01A3 pour le magnétomètre LIS2MDL
# et l'accéléromètre LIS2DW12.

# La compensation d'inclinaison est calculée à l'aide des instructions du document AN3192 
# de STMicroelectronics disponible en téléchargement sur le site Web STM32python. 
# Les formules ont du être adaptées (inversions et permutations de certains axes) conformémet 
# aux explications données aux pages 19 à 23 de AN3192 car les orientations 
# des axes des MEMS ne sont pas les mêmes sur le LSM303DLH et sur les composants LIS2MDL 
# et LIS2DW12 de la X-NUCLEO-IKS01A3.

from machine import I2C
import lis2mdl # Pilote du magnétomètre
import lis2dw12 # Pilote de l'accéléromètre
from time import sleep_ms # Pour les temporisations
from math import atan2, asin, cos, sin, pi, sqrt # Fonctions trigonométriques

# On utilise l'I2C n°1 de la carte NUCLEO-WB55 pour communiquer avec le capteur
i2c = I2C(1)

# Pause d'une seconde pour laisser à l'I2C le temps de s'initialiser
sleep_ms(1000)

# Instanciation du magnétomètre
magnetometer = lis2mdl.LIS2MDL(i2c)

# Instanciation de l'accéléromètre
accelerometer = lis2dw12.LIS2DW12(i2c)

# Facteur de conversion entre les radians et les degrés pour les angles
RadToDeg = 180 / pi

# Précalculs de constantes trigonométriques
twopi = 2 * pi
halfpi = pi * 0.5
three_halfpi = 3 * halfpi

# Valeurs extrêmes obtenues par le script de calibration

min_bx = const(-78)
max_bx = const(-9)
min_by = const(-27)
max_by = const(60)
min_bz = const(-117)
max_bz = const(-58)

# Calcul des décalages (offsets) "Hard Iron" pour chaque axe :

offset_x = (max_bx + min_bx) / 2
offset_y = (max_by + min_by) / 2
offset_z = (max_bz + min_bz) / 2

# Calcul des coefficients pour la correction approximative des 
# distorsions "Soft Iron" pour chaque axe :

avg_delta_x = (max_bx - min_bx) / 2
avg_delta_y = (max_by - min_by) / 2
avg_delta_z = (max_bz - min_bz) / 2

avg_delta = (avg_delta_x + avg_delta_y + avg_delta_z) / 3

scale_x = avg_delta / avg_delta_x
scale_y = avg_delta / avg_delta_y
scale_z = avg_delta / avg_delta_z

while True:

	# Lecture du champ magnétique sur les trois axes orthogonaux
	magnetometer.get()

	# On applique les corrections hard iron et soft iron.
	# On corrige les définitions des axes du magnétomètre pour 
	# les orienter comme ax, ay, az
	# (le trièdre obtenu doit être direct !)
	bx = -1 * (magnetometer.x() - offset_x) * scale_x
	by = -1 * (magnetometer.y() - offset_y) * scale_y
	bz = -1 * (magnetometer.z() - offset_z) * scale_z

	# Norme du vecteur champ magnétique
	mag = sqrt(bx*bx + by*by + bz*bz)

	# On corrige les définitions des axes de l'accéléromètre.
	# (le trièdre obtenu doit être direct !)
	ax = accelerometer.y()
	ay = accelerometer.x()
	az = accelerometer.z()

	# Norme du vecteur accélération
	acc = sqrt(ax*ax + ay*ay + az*az)

	# Si les normes ne sont pas nulles
	if mag > 0 and acc > 0:

		# On normalise les composantes des vecteurs accélération et champ magnétique
		# (indispensable pour les calculs des arcsinus et arccosinus qui suivent)

		inv_acc = 1 / acc

		ax *= inv_acc
		ay *= inv_acc
		az *= inv_acc

		inv_mag = 1 / mag

		bx *= inv_mag
		by *= inv_mag
		bz *= inv_mag

		# Calcul des angles d'Euler.
		# Les trois angles d'Euler (pitch, roll et yaw / heading) donnent l'orientation du magnétomètre
		# dans l'espace. Ils doivent varier selon la règle donnée à la page 19 de AN3192.
		
		pitch = -1 * asin(-ax)
		temp = cos(pitch) 
		if temp != 0:
			roll = -1 * asin(ay/cos(pitch))
		else:
			roll = 0

		xh = bx * cos(pitch) + bz * sin(pitch)
		yh = bx * sin(roll) * sin(pitch) + by * cos(roll) - bz * sin(roll) * cos(pitch)
		# zh = -bx * cos(roll) * sin(pitch) + by * sin(roll) + bz * cos(roll) * cos(pitch)

		# Heading (ou Yaw) : cap (ou lacet) que l'on recherche, la rotation autour de (Oz)
		if xh > 0 and yh >= 0:
			heading = atan2(yh, xh)
		elif xh < 0:
			heading = pi + atan2(yh, xh)
		elif xh > 0 and yh <= 0:
			heading = twopi + atan2(yh, xh)
		elif xh == 0 and yh < 0:
			heading = halfpi
		elif xh == 0 and yh > 0:
			heading = three_halfpi

		# Conversions en degrés 
		pitch_deg = pitch * RadToDeg
		roll_deg = roll * RadToDeg
		heading_deg = heading * RadToDeg

		# M = sqrt(xh*xh + yh*yh + zh*zh)

		# Affichage des angles d'Euler
		print("Pitch (tangage) = %.1f°" % pitch_deg)
		print("Roll (roulis) = %.1f°" % roll_deg)
		print("Heading (cap) = %.1f°" % heading_deg)
		# print("M = %.3f" % M)
		print("")

		sleep_ms(1000)

La figure qui suit illustre les définitions retenues avec la X-NUCLEO-IKS01A3 pour les angles pitch, roll et heading. Une fois ce script chargé et démarré, vous pouvez vérifier sur le terminal série de PuTTY que l’angle du cap varie peu lorsque l’inclinaison de la X-NUCLEO-IKS01A3 varie autour du plan horizontal :

Definition angles de tilt pour boussole IKS01A3

Terminal angles de tilt pour boussole IKS01A3

Egalement, et conformément aux explications de AN3192, on vérifie bien que dans cette configuration :

L’angle Heading varie de 0 à 359° lorsqu’on fait tourner l’axe z dans le sens horaire ;
L’angle Pitch varie de 0 à 90° lorsqu’on effectue une rotation autour de l’axe y dans le sens qui fait tourner l’axe x vers le haut ;
L’angle Roll varie de 0 à 90° lorsqu’on effectue une rotation autour de l’axe x dans le sens qui fait tourner l’axe y vers le bas.

Remarques :

Cette application est difficile à réaliser du fait de l’orientation différente des axes des MEMS inertiels sur la X-NUCLEO-IKS01A3 que nous avons utilisée (voir les schémas en bas de cette page). Si vous n’utilisez pas une X-NUCLEO-IKS01A3, vous devrez adapter les calculs de ax, ay, az, bx, by, bz, pitch et roll (en ajoutant des ‘-1’* là où ce sera nécessaire) à la configuration de vos MEMS.
Même lorsque vos calculs seront corrects, il est presque certain que le Heading indiqué vous paraisse significativement erroné. La cause est essentiellement l’environnement magnétique très perturbé en intérieur ; si possible, calibrez et testez votre boussole à l’extérieur, vous constaterez qu’elle y sera bien plus précise et cohérente.

Pour aller plus loin

Ce tutoriel est une opportunité pour en apprendre plus sur les sujets ludiques de l’orientation avec une boussole et de la lecture d’une carte.
Notamment, sachez qu’une boussole n’indique par la direction du pôle nord géographique mais celle du pôle nord magnétique qui ne coïncident pas. L’écart angulaire entre les deux s’appelle la déclinaison magnétique qui change au cours du temps et selon l’emplacement de la boussole.
Quelques ressources sur ce sujet :

La page Wikipedia sur la déclinaison magnétique terrestre ;
Un tutoriel de l’Institut Géographique National (IGN) sur l’orientation avec une boussole et une carte topographique ;
Le calculateur en ligne du National Oceanic and Atmospheric Administration (NOAA) qui donne la déclinaison magnétique d’un emplacement sur le globe terrestre à partir de ses coordonnées GPS.