Mise en oeuvre 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

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

Le code MicroPython

Les scripts présentés ci-après sont disponibles dans la zone de téléchargement

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) tous les quarts de seconde. 
# Cet exemple nécessite un shield X-NUCLEO IKS01A3 pour le magnétomètre MEMS LIS2MDL 
# Intensité du champ terreste : 30µT à 60µT (en théorie)

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
magnetometre = lis2mdl.LIS2MDL(i2c)

while True:

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

	# Affiche les composantes du champ magnétique (entiers signés avec 3 digits)
	print("Composantes du champ magnétique :")
	print(" - Selon x : %6d µT" % bx)
	print(" - Selon y : %6d µT" % by)
	print(" - Selon z : %6d µT" % bz)

	# Module du champ magnétique
	module = sqrt(bx*bx + by*by + bz*bz)
	
	# Affiche le module du champ magnétique avec une décimale après la virgule
	print("Module du champ magnétique : %.1f µT" % module)
	print("")
	sleep_ms(250)

Important : Gardez à l’esprit que les valeurs “brutes” de champ magnétique mesurées avec ce script seront toujours décalées et faussées de quantités qui changent d’un capteur à l’autre. Vous ne pourrez donc pas l’utiliser tel quel pour faire des mesures physiques. Pour que le LIS2MDL renvoie des valeurs exploitables, il faut le calibrer selon une procédure précise. Nous abordons juste après la problématique du calibrage du LIS2MDL afin de programmer une boussole.

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é !

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 une procédure de calibrage simplifiée qui permettra de programmer une boussole précise uniquement lorsque le PCB qui supporte le LIS2MDL sera horizontal.

Le lecteur intéressé par ce sujet pourra aller plus loin en consultant cette référence, cette référence, ou enfin celle-ci d’où sont adaptés les codes qui suivront.

Pourquoi faut-il calibrer le LIS2MDL ?

Nous commençons par raisonner sur le cas simplifié où le plan (Ox, Oy) 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 des distorsions “hard iron” et des distorsions “soft iron” et par un effet de tilt éventuel que nous allons expliciter.

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 (ie 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 Ox et Oy.

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 IKS01A3 dans le plan horizontal (Ox, Oy). 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 Ox et Oy. Une fois ces valeurs extrêmes connues, on en déduit directement les offsets qu’il faut soustraire aux mesures selon les axes Ox et Oy 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 :

Correction hard iron


Au départ, les mesures du magnétomètre, lorsqu’on le fait tourner dans le plan horizontal (Ox, Oy) sont toutes contenues à l’intérieur (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, comme expliqué ici.

En pratique nous utiliserons la méthode approximative et simplifiée présentée sur ce site pour ne pas surcharger le code. 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 :

Correction 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 Ox par une rotation, puis on applique des homothéties selon 0x et 0y 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 Oz dans la procédure de calibration. Cette modification ne remet pas en question les explications que nous avons données. Elle sera utile lorsque nous programmerons le cas général pour intégrer l’effet de tilt à notre boussole virtuelle.

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 Oz). 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 Oz).

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 m2/s2). 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 :

Correction tilt


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 (Oxy). 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(by/bx). 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 (Oz) 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 capteurs (voir ci-dessous).

Vous trouverez un sketch Arduino qui traite ce problème sur ce site pour d’autres capteurs que les notres. 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 :

  1. Détermination des valeurs minimum et maximum du champ magnétique mesuré suivant les trois axes du magnétomètre
  2. Correction des distorsions hard iron isotropes
  3. Correction des distorsions soft iron anisotropes
  4. 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 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.

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

Les scripts présentés ci-après sont disponibles dans la zone de téléchargement

Voici le script main.py qui pemettra 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 IKS01A3, 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 hauts parleurs, aimants, 
# masses métalliques contenant du fer ou du nikel et des autres appareils électriques en 
# fonctionnement qui pourraient perturber le processus.

from machine import I2C
import lis2mdl # Bibliothèque pour le magnétomètre
from time import sleep_ms
from math import sqrt
from pyb import Timer # Bibliothèque 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
magnetometre = lis2mdl.LIS2MDL(i2c)

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

# 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
	magnetometre.get()
	bx = magnetometre.x()
	by = magnetometre.y()
	bz = magnetometre.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
	module = 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 + " - Module du champ magnétique : " + "%.1f" % module + " µ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 pour le calibrage
print("Fin calibrage (µT)")
print("Valeurs pour le calibrage :")
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))

Le code pour simuler une boussole sans compensation d’inclinaison

Les scripts présentés ci-après sont disponibles dans la zone de téléchargement

Voici le script main.py qui permettra de mesurer l’écart angulaire entre l’axe (Ox) 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 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 oeuvre, vous devrez calibrer le LIS2MDL avec le script adéquat.
# Source expliquant le calcul des corrections sur le magnétomètre : 
# https://appelsiini.net/2018/calibrate-magnetometer/

# Cet exemple nécessite un shield X-NUCLEO IKS01A3 pour le magnétomètre MEMS LIS2MDL
# 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 # Pilote du magnétomètre
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
magnetometre = 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(-12)
min_by = const(-28)
max_by = const(53)
min_bz = const(-45)
max_bz = const(47)

# 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
	magnetometre.get()
	bx = magnetometre.x()
	by = magnetometre.y()
	bz = magnetometre.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 avec le nord magnétique
	print("Ecart angulaire avec le nord magnétique : %.1f°" % angle_deg)
	print("")

	sleep_ms(250)

Le code pour simuler une boussole avec compensation d’inclinaison

Les scripts présentés ci-après sont disponibles dans la zone de téléchargement

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 (ax, ay, az) du LIS2DW12 et (bx, by, bz) 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 IS01A3 donnée ici ; dans cet ordre il faut :

  1. Permuter les axes x et y du LIS2DW12
  2. Inverser l’axe y du LIS2DW12
  3. 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 et 20 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 la boussole
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
magnetometre = lis2mdl.LIS2MDL(i2c)

# Instanciation de l'accéléromètre
accelerometre = lis2dw12.LIS2DW12(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(-12)
min_by = const(-28)
max_by = const(53)
min_bz = const(-45)
max_bz = const(47)

# 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
	magnetometre.get()

	# On applique les corrections hard iron et soft iron et on corrige les
	# définitions des axes du magnétomètre.
	bx = (magnetometre.x() - offset_x) * scale_x
	by = -1 * (magnetometre.y() - offset_y) * scale_y
	bz = (magnetometre.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.
	ax = accelerometre.y()
	ay = -1 * accelerometre.x()
	az = accelerometre.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 = asin(-ax)
		roll = asin(ay/cos(pitch))
	
		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)
		heading = atan2(yh, xh)
		
		# Conversions en degrés 
		pitch_deg = pitch * RadToDeg
		roll_deg = roll * RadToDeg
		heading_deg = heading * RadToDeg
	
		# On ne veut pas de caps négatifs. Lorsque cela se produit on calcule le
		# le complément à 360° pour que l'angle reporté varie entre 0° et 360°.
		if heading_deg < 0:
			heading_deg += 360

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

		sleep_ms(250)

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’ange 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.