Hola jfh900
Una de las cosas mas importantes es que el cálculo del ángulo de inclinación sea lo mas real posible. Este cálculo se hace usando esta fórmula "angulo = angulo + (gyro * tiempo);" Donde "angulo" es la inclinación en radianes, "gyro" la velocidad angular en radianes/segundo y "tiempo" es el tiempo en segundos transcurrido entre cada llamada a dicha fórmula.
A groso modo, si al leer el giroscopio tenemos una velocidad angular de 0.5 rad/seg y desde la anterior lectura han pasado 0.1 seg significa que nos hemos inclinado 0.05 radianes. Esta inclinación se la sumamos a la que teniamos en el bucle anterior y así sucesivamente.
angulo actual = angulo anterior + (0.5rad/s * 0.1s) => angulo actual = angulo anterior + 0.05
Este sistema de cálculo tiene dos problemas importantes, uno es que nunca sabemos cual es el ángulo de partida y el otro es que los giroscopios tienen una deriva en el tiempo por lo que hay que buscar una manera de ir actualizando el valor del ángulo calculado de alguna forma.
La forma de hacerlo es con el acelerómetro, que a su vez tiene un "pro" y un "contra". El "pro" es que no tiene deriva y en condiciones de aceleración cero el ángulo que podemos calcular con su lectura es bastante certero. El "contra" es que en condiciones de aceleración y/o vibración el ángulo que podremos determinar no será todo lo acertado que necesitamos.
Así que hay que buscar un sistema que nos permita reducir al mínimo el efecto de la deriva del giroscopio y "creernos" al acelerómetro solo en ciertas condiciones. (mínima aceleración)
Para realizar esto probé distintas soluciones pero lo que mejor funcionó es el famosso filtro Kalman.
Encontré un ejemplo de filtro Kalman para un eje de inclinación usando un acelerómetro de dos ejes y un giroscopio.
tilt.c y
tilt.hSon dos funciones, una de ellas se encarga (entre otras cosas) de calcular el ángulo de inclinación por el método que he comentado arriba "angulo += gyro * tiempo". La otra, entre otras cosas, "refresca" el valor de "angulo" con la lectura del acelerómetro y calcula un valor de error para el giroscopio (bias), de forma que puede eliminar la deriva antes comentada.
He intentado entender el filtro kalman leyendo todo lo que he encontrado sobre él y para ser sincero tengo que decir que como mucho me hago una ligera idea de como funciona pero de ninguna forma lo entiendo completamente. Mi consejo es que leas sobre él, matrices de covarianza y que con el código de ejemplo te puedas hacer una idea de como y porqué funciona.
Te pego las dos funciones tal y como las estoy empleando en mi bot. Podemos ir comentandolas si quieres.
// Variables globales
float P[2][2] = { { 1, 0 }, { 0, 1 }, };
float R_angle = 0.3;
float Q_angle = 0.001;
float Q_gyro = 0.003;
float q_bias, angulo, gyro, tiempo;
void state_update( void )
{
float Pdot[4];
Pdot[0]= Q_angle - P[0][1] - P[1][0];
Pdot[1]= -P[1][1];
Pdot[2]= -P[1][1];
Pdot[3]= Q_gyro;
gyro -= q_bias;
angulo += gyro * tiempo;
P[0][0] += Pdot[0] * tiempo;
P[0][1] += Pdot[1] * tiempo;
P[1][0] += Pdot[2] * tiempo;
P[1][1] += Pdot[3] * tiempo;
}
void kalman_update( float angle_m )
{
float K_0;
float K_1;
K_0 = P[0][0] / (R_angle + P[0][0]);
K_1 = P[1][0] / (R_angle + P[0][0]);
P[0][0] -= K_0 * P[0][0];
P[0][1] -= K_0 * P[0][1];
P[1][0] -= K_1 * P[0][0];
P[1][1] -= K_1 * P[0][1];
angulo += K_0 * (angle_m - angulo);
q_bias += K_1 * (angle_m - angulo);
}Un saludo...
