Paso 10: Un filtro muy favorecedor.
ángulo = A * (ángulo + tasa * DT) + (1 - A) * (float) accel_raw * A_GAIN;
Bueno, por lo que es una línea larga. También puede hacerlo con un op-amp o dos.
Este filtro hace exactamente lo que es necesario en este escenario: favorece el giroscopio lectura para duraciones de corto plazo y la media de acelerómetro leyendo durante largo períodos de tiempo. Vamos a descomponerlo:
A es el factor que determina el tiempo límite para confiar en el giroscopio y filtrado en el acelerómetro. Está siempre entre 0 y 1, generalmente cerca de 1. En este caso, A se define como 0.962. Esto significa que en cada vez que paso, 96.2% de la nueva medida de ángulo proviene de la antigua medida del ángulo y la medida del giróscopo integrado. El 3.8% restante proviene del acelerómetro. Esto promedia lentamente en el acelerómetro sobre muchos pasos de tiempo.
DT es el tiempo en segundos entre bucles de programa, el paso del tiempo. Aquí se define para ser 0.020 y por delay(20) al final del bucle. El código en el bucle de sí mismo toma mucho menos de 20ms, por lo que el retraso domina el paso del tiempo.
el tipo es el giróscopo leer, convertir en grados por segundo.
accel_raw * A_GAIN es la lectura del acelerómetro, convertida a grados. Es muy importante que estos dos en la misma unidad base antes de sumar. (Se pueden agregar manzanas a las naranjas).
La constante de tiempo del filtro es la duración de tiempo en que la lectura de giroscopio comienza a filtrarse hacia fuera fuertemente y la lectura del acelerómetro comienza a ser promediadas en gran medida. Es realmente un proceso continuo, pero la constante de tiempo es una sola medida de donde el equilibrio empieza a cambiar. La constante de tiempo es:
Tau = DT*(A)/(1-A) = 0.5s
Por lo tanto, este filtro, el giro es de confianza para acerca de 0,5 segundos y el comienzo del acelerómetro en un promedio en forma significativa después de. Este valor puede ser ajustado cambiando A.
