Je travaille sur un magnétomètre AK8975 faisant partie d'une IMU. Ce qui me semble très délicat. Cette puce donne un vecteur 3D en sortie décrivant le champ magnétique terrestre à n'importe quel endroit sur la terre ou à proximité.
J'ai essayé deux types d'algorithmes de calcul de cap: l'un est simple arctan(-y/x)et l'autre est le calcul annulé par inclinaison (tangage) et inclinaison (roulis) comme mentionné ci-dessous. Tant l'inclinaison que les inclinaisons donnent une sortie incorrecte.
Je suis capable d'obtenir le cap correct par rapport à la terre (en utilisant les ressources d'étude disponibles simples disponibles) quand il est tourné en gardant horizontal par rapport au plan au sol en utilisant l'un des deux algos.
J'ai essayé l'étalonnage pour les erreurs de fer doux et dur. Je pourrais le tracer en 3D et montre une sphère 3D parfaite. Ne fonctionne toujours pas sur l'inclinaison ou la déclinaison.
Tout pointeur sera utile.
Le code et ses implémentations sont les suivants:
void Compass_Heading()
{
double MAG_X;
double MAG_Y;
double cos_roll;
double sin_roll;
double cos_pitch;
double sin_pitch;
cos_roll = cos(roll);
sin_roll = sin(roll);
cos_pitch = cos(pitch);
sin_pitch = sin(pitch);
//// Tilt compensated Magnetic filed X:
MAG_X = magnetom_x*cos_pitch + magnetom_y*sin_roll*sin_pitch + magnetom_z*cos_roll*sin_pitch;
//// Tilt compensated Magnetic filed Y:
MAG_Y = magnetom_y*cos_roll-magnetom_z*sin_roll;
//// Magnetic Heading
MAG_Heading = atan2(-MAG_Y, MAG_X) ;
}
Où magnetom_x, #_yet #_zsont des composantes d'un vecteur 3D qui sont en fait des valeurs brutes du magnétomètre. le roulis et le tangage proviennent d'un mystérieux filtre Kalman émis par l'accéléromètre et le gyroscope embarqués. Ces trois capteurs sont en ATAVRSBIN1 . Le roulis et le tangage sont corrects jusqu'à cette étape.
Maintenant, un simple calcul de cap selon journal_of_sensors_renaudin et al_2010c.pdf aurait dû être MAG_Heading = atan2(-magnetom_y, magnetom_x) ;et avec compensation comme ci-dessus.
Le code global provient simplement de OPEN AHRS .
Données au format Roll, Pitch et Yaw. J'ai tourné l'appareil uniquement avec ma main. Les trois premiers se sont concentrés respectivement sur Roll, Pitch et Yaw. Les deux autres ont d'abord fait pivoter l'appareil d'environ 45 degrés le long de X (roulé) puis tourné le long du Z local du magnétomètre.
Les graphiques tracés dans la plage de -180 à 180 degrés.
Angles en degrés dans un fichier Les caractéristiques YAW sur Roll.
Angles en degrés dans un fichier Les caractéristiques YAW sur Pitch.
Angles en degrés dans un fichier Les caractéristiques YAW sur le lacet lui-même.
Angles en degrés dans un fichier Les caractéristiques YAW sur Yaw avec 45 degrés roulés.
Angles en degrés dans un fichier Les caractéristiques YAW sur Yaw avec 45 degrés inclinés.
Remarque: Pour les 2 dernières photos: Tout d'abord maintenu en position d'origine, c'est la même chose pour tous (reportez-vous aux fichiers txt). Puis roulé à 45 degrés puis en utilisant l'appareil plan (avec magnétomètre) a été tourné le long de l'axe Z du magnétomètre.
De même pour la dernière image, l'appareil a été incliné à 45 degrés puis le long de l'axe Z du magnétomètre.
J'espère que cela aidera à résoudre mon problème.
Les nouveaux développements sont les suivants:
J'en ai travaillé sur la rubrique. J'ai obtenu la sortie suivante.
csv
la source
Réponses:
J'aime bien tes graphiques. Ils montrent clairement que le roulis, le tangage et le lacet semblent fonctionner. Toutes nos félicitations! C'est déjà plus de progrès que la plupart des gens.
Je suppose que le code que vous avez présenté calcule "la mauvaise" valeur MAG_Heading, différente de la valeur MAG_Heading que vous attendiez.
Il nous serait beaucoup plus facile de vous aider si vous nous donniez: (Ceci est la section "décrivez les symptômes" de "Comment poser des questions de manière intelligente" )
Donc je suis parti pour spéculer que peut-être vous rencontrez les mêmes types de problèmes que je me crée :-).
Il semble que d'autres personnes discutent d'un code très similaire ailleurs: http://diydrones.com/forum/topics/heading-from-3d-magnetometer ; http://diydrones.ning.com/profiles/blogs/dcm-imu-theory-first-draft ; http://aeroquad.com/showthread.php?1138-REVOLUTION!!!-New-IMU !!! ; http://www.rcgroups.com/forums/showthread.php?t=1436742&page=6 ; http://aeroquad.com/showthread.php?691-Hold-your-heading-with-HMC5843-Magnetometer ; etc.
la source
La note d'application du LSM303 contient un guide utile pour calibrer une boussole à inclinaison compensée qui s'applique à votre problème. C'est assez détaillé, sinon j'aurais réécrit les calculs ici. Notez que les valeurs de l'accéléromètre sont nécessaires pour les calculs de tangage complet, de roulis et de lacet car une rotation autour de l'axe des lignes de champ magnétique n'entraîne aucun changement dans les valeurs du magnétomètre. De même pour la gravité avec l'accéléromètre.
la source