Cette partie 2 d’une série en plusieurs parties sur l’utilisation d’une boussole numérique (magnétomètre) avec votre Raspberry Pi.
La partie 1 peut être trouvée ici et est une condition préalable à la partie 2.
Dépôt Git ici
Le code peut être extrait sur votre Raspberry Pi avec ;
pi@raspberrypi ~ $ git clone https://github.com/ozzmaker/BerryIMU.git
Le code de ce guide se trouve sous le compass_tutorial02_tilt_compensation annuaire.
La partie 1 a expliqué comment obtenir le cap du magnétomètre, mais cela n’est fiable que lorsque le magnétomètre est sur une surface plane. Si le magnétomètre est incliné, le cap sera biaisé et ne sera pas correct.
Le graphique ci-dessous montre une boussole tenue à 200 degrés et inclinée dans diverses directions. La ligne bleue est le cap brut, la ligne orange est le cap après application de la compensation d’inclinaison. Comme vous pouvez le voir, sans compensation d’inclinaison, le cap changera si la boussole est inclinée.
L’image ci-dessous provient d’une page de la note d’application AN3192 de ST, la fabrication du LSM9DS0 qui est le magnétomètre et l’accéléromètre que nous utilisons dans ce guide. Cette note d’application explique en détail comment calculer un cap compensé en inclinaison.
En résumé, il indique que si le magnétomètre est à une position de niveau, alors le seul calcul qui doit être effectué est ;
heading = 180*atan2(magRawY,magRawX)/M_PI;
Si le magnétomètre est incliné, nous devons prendre en compte les valeurs d’inclinaison pour obtenir un cap précis. C’est ce que dit la note d’application à ce sujet ;
En résumé; Nous utiliserons un accéléromètre pour calculer le tangage et le roulis qui seront ensuite inclus dans notre formule pour calculer le cap avec compensation d’inclinaison. Voici la formule ;
float magXcomp = mag_raw[0]*cos(asin(accXnorm))+mag_raw[2]*sin(pitch); float magYcomp = mag_raw[o]*sin(asin(accYnorm/cos(pitch)))*sin(asin(accXnorm))+mag_raw[1]*cos(asin(accYnorm/cos(pitch)))-mag_raw[2]*sin(asin(accYnorm/cos(pitch)))*cos(asin(accXnorm)); heading = 180*atan2(magYcomp,magXcomp)/M_PI;
La formule ci-dessus semble très complexe, vous n’avez pas vraiment besoin de la comprendre et nous la simplifions plus loin dans cet article.
Un point à noter ;La fonction arcsin a une bonne linéarité entre environ -45º et +45º, de sorte que la précision du
le calcul du tangage et du roulis se dégrade lorsque les angles d’inclinaison dépassent cette plage
La plupart des accéléromètres sont sur la même puce qu’un magnétomètre, qui est dans ce cas un LSM9DS0 sur un BerryIMU. Dans certains cas, l’adresse i2c de l’accéléromètre sera différente de celle du magnétomètre (par exemple sur le LSM303DLHC). Sur le LSM9DS0, l’adresse i2c est la même pour l’accéléromètre et le magnétomètre.
Pour le BerryIMU, l’adresse par défaut est 0x1E.
Nous allons créer une nouvelle fonction pour écrire dans l’accéléromètre ;
void writeMagReg(uint8_t reg, uint8_t value)
{
selectDevice(file,MAG_ADDRESS);
int result = i2c_smbus_write_byte_data(file, reg, value);
if (result == -1)
{
printf ("Failed to write byte to I2C Mag.");
exit(1);
}
}
Et nous allons également créer une nouvelle fonction pour lire à partir de l’accéléromètre. La lecture des valeurs de l’accélérateur est similaire à la lecture des valeurs du magnétomètre, nous allons lire un bloc de 6 octets.
void readACC(int *a)
{
uint8_t block[6];
selectDevice(file,ACC_ADDRESS);
readBlock(0x80 | OUT_X_L_A, sizeof(block), block);
*a = (int16_t)(block[0] | block[1] << 8);
*(a+1) = (int16_t)(block[2] | block[3] << 8);
*(a+2) = (int16_t)(block[4] | block[5] << 8);
}
Voici ce qui se passe dans la fonction readACC();
1) Un tableau de 6 octets est d’abord créé pour stocker les valeurs.
2) En utilisant la fonction readBlock() (créée dans la partie 1), nous lisons 6 octets à partir de OUT_X_L_A (0x28). Ceci est indiqué à la page 61 de la fiche technique.
3) Les valeurs sont exprimées en complément à 2 (MSB pour le signe puis 15 bits pour la valeur) il faut donc combiner ;
bloquer[0] & bloquer[1] pour l’axe X
bloquer[2] & bloquer[3] pour l’axe Y
bloquer[4] & bloquer[5] pour l’axe Z
En travaillant avec le code que nous avons créé dans la partie 1, nous devons maintenant ajouter du code pour activer l’accéléromètre ;
// Enable accelerometer. writeAccReg(CTRL_REG1_XM, 0b01100111); writeAccReg(CTRL_REG2_XM, 0b00100000);
CTRL_REG1_XM indique à l’accéléromètre d’activer tous les axes, de le mettre en mode de mise à jour continue et d’un débit de données de 100 Hz – Page 55 de la fiche technique.
CTRL_REG2_XM, réglera l’accéléromètre à +/-16 gauss pleine échelle.
Premièrement, nous devons normaliser les données brutes de l’accéléromètre ;
accXnorm = accRaw[0]/sqrt(accRaw[0]* accRaw[0]+ accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]); accYnorm =accRaw[1]/sqrt(accRaw[0] *accRaw[0] + accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);
Comme le tangage et le roulis sont calculés un certain nombre de fois dans la formule de compensation d’inclinaison, nous les séparerons et effectuerons les deux calculs une fois au préalable ;
pitch = asin(accXnorm); roll = -asin(accYnorm/cos(pitch));
Nous calculons maintenant les nouvelles lectures du magnétomètre compensé en inclinaison ;
magXcomp = *mag_raw*cos(pitch)+*(mag_raw+2)*sin(pitch); magYcomp = *mag_raw*sin(roll)*sin(pitch)+*(mag_raw+1)*cos(roll)-*(mag_raw+2)*sin(roll)*cos(pitch);
Nous utilisons ensuite ces nouvelles valeurs compensées dans notre formule de cap ;
heading = 180*atan2(magYcomp,magXcomp)/M_PI;
Nous allons également convertir les valeurs afin qu’elles soient comprises entre 0 et 360 ;
if(heading < 0)
heading += 360;
Après le dernier calcul, nous pouvons ajouter une autre instruction print pour imprimer le nouveau cap compensé.
printf("Compensated Heading %7.3f n", heading);
Lorsque vous exécutez maintenant le programme, vous remarquerez que le cap reste la même valeur si le magnétomètre est incliné à moins de 40 degrés.
Cap RAW en degrés 172.000 ##### Cap Tilt Comp 170.043
Cap RAW en degrés 170.044 ##### Cap Tilt Comp 168.015
Cap RAW en Degrés 167.341 ##### Cap Tilt Comp 164.817
Cap RAW en degrés 163.133 ##### Cap Tilt Comp 172.429
Cap RAW en degrés 155.924 ##### Cap Tilt Comp 162.065
Cap RAW en degrés 149.081 ##### Cap Tilt Comp 153.045
Cap RAW en degrés 144.548 ##### Cap Tilt Comp 152.323
Cap RAW en Degrés 139.844 ##### Cap Tilt Comp 157.862
Cap RAW en Degrés 135.603 ##### Cap Tilt Comp 171.924
Cap RAW en degrés 130.305 ##### Tilt Comp cap 172.325
Cap RAW en Degrés 122.636 ##### Cap Tilt Comp 146.452
Cap RAW en degrés 118.951 ##### Cap Tilt Comp 143.692
pi@raspberrypi ~ $ gcc compass_tutorial01.c -o compass_tutorial01 -lm
Si votre IMU est à l’envers, vous devrez inverser les valeurs d’accélérateur X et Y pour que la compensation d’inclinaison fonctionne.
Ajoutez simplement les deux lignes ci-dessous juste après que les valeurs brutes aient été lues à partir de l’accéléromètre.
accRaw[0] = -accRaw[0];
accRaw[1] = -accRaw[1];