Etape 10 : Un filtre très flatteur.
angle = A * (angle + taux * DT) + (1 - A) * (float) accel_raw * A_GAIN ;
Bon, alors c’est une longue lignée. Vous pouvez également le faire avec un ampli-op ou les deux.
Ce filtre est exactement ce qui est nécessaire dans ce scénario : il favorise le gyroscope lecture pour courtes durées et la lecture pour les longues durées moyenne accéléromètre. Nous allons décomposer :
A est le facteur qui détermine l’heure de tombée pour le gyroscope de confiance et de filtrage dans l’accéléromètre. Il est toujours compris entre 0 et 1, généralement proche de 1. Dans ce cas, A est défini pour être 0,962. Cela signifie qu’à chaque fois étape, vient de 96,2 % de la nouvelle mesure de l’angle de la mesure d’angle ancien, plus la mesure de gyroscope intégré. Le 3,8 % restant provient de l’accéléromètre. Cette moyenne lentement dans l’accéléromètre sur pas beaucoup de temps.
DT est le temps en secondes entre les boucles du programme, le pas de temps. Ici, il est défini pour être 0,020 et définie à le delay(20) à la fin de la boucle. Le code dans la boucle elle-même prendre beaucoup moins de 20 ms, donc le retard domine le pas de temps.
le taux est le gyroscope lecture, convertie en degrés par seconde.
accel_raw * A_GAIN est la lecture de l’accéléromètre, convertie en degrés. Il est très important que ces deux être dans la même unité de base avant d’ajouter à l’ensemble. (Vous ne pouvez pas ajouter des pommes et des oranges.)
La constante de temps du filtre est la durée au cours de laquelle la lecture de gyroscope commence à être filtrées fortement et la lecture de l’accéléromètre commence à moyennés dans fortement. C’est en fait un processus continu, mais la constante de temps est un simple mesure d’où l’équilibre commence à changer. La constante de temps est :
Tau = DT*(A)/(1-A) = 0. 5 s
Donc, pour ce filtre, le gyroscope est approuvé pour la sur 0,5 secondes et le début de l’accéléromètre en moyenne en significativement après cela. Cette valeur peut être modifiée en changeant A.