Estoy haciendo Autonomous Quad. Estoy programando mi IMU con ADXL335 -3 Axis analog Accelerometer & L3G4200D Gyro digital de 3 ejes. Estoy usando el launchpad LM4F120XL de TI basado en ARM Cortex M4F
Tengo un problema al implementar IMU. Aquí está mi algoritmo de IMU ...
Por ADC estoy leyendo el acelerómetro en Dx, Dy & Dz. Ahora que tengo 12bit ADC estoy usando la siguiente fórmula para obtener las lecturas en "g"
Rx = [(Dx*(3.3/4095) - Tx] / 0.3
Ry = [(Dy*(3.3/4095) - Ty] / 0.3
Rz = [(Dz*(3.3/4095) - Tz] / 0.3
En condición estable, he comprobado el Tx, Ty, Tz:
Tx = 1.630820
Ty = 1.658477
Tz = 1.997728
Para Gyro (a 250 dps con 8.75 mdps / dígito)
Para obtener la lectura en dps, estoy usando la siguiente fórmula
X = Gx*8.75/1000
Y = Gy*8.75/1000
Z = Gz*8.75/1000
(Aquí Gx, Gy & Gz son datos de giroscopios ... los he comprobado, son valores firmados y también son precisos (ruido mínimo))
Quiero medir los ángulos con respecto a todos los ejes (inclinación, balanceo y giro). Para eso estoy haciendo según lo siguiente
Th = 5;
T = 0.05; // approx. 5ms I've checked it using counter
c = 180/3.14;
while (1) {
// initial values of Rxe, Rye, Rze are same as Rx, Ry, Rz
Xd = atan2(Rxe, sqrt( (Rze*Rze) + (Rye*Rye) ));
Yd = atan2(Rye, sqrt( (Rze*Rze) + (Rxe*Rxe) ));
Zd = atan2(Rze, sqrt( (Rye*Rye) + (Rxe*Rxe) ));
// To calculate Xavg, Yavg, Zavg from two consecutive X, Y, Z values respectively
gyro();
if ((Xavg >= Th) || (Xavg <= Th)) {
Xd = Xd + (Xavg*T);
}
if ((Yavg >= Th) || (Yavg <= Th)) {
Yd = Yd + (Yavg*T);
}
if ((Zavg >= Th) || (Zavg <= Th)) {
Zd = Zd + (Zavg*T);
}
Rxg = sin(Yd) / (sqrt(1 + ((cos(Yd) * cos(Yd)) * (tan(Xd) * tan(Xd)))));
Ryg = sin(Xd) / (sqrt(1 + ((cos(Xd) * cos(Xd)) * (tan(Yd) * tan(Yd)))));
Rzg = ((Rzg > 0) ? 1 : ((Rzg < 0) ? -1 : 1))*(sqrt(1 - (Rxg*Rxg)-(Ryg*Ryg)));
get_accl_data(); // To get ADC values
accelerometer(); // To convert ADC valuse in "g" - Rx, Ry, Rz
Rxe = (Rx*0.015)+(Rxg*0.985); // weighted average
Rye = (Ry*0.015)+(Ryg*0.985);
Rze = (Rz*0.015)+(Rzg*0.985);
Xd *= c; // converting in degree
Yd *= c;
Zd *= c;
Print(Xd);
Print(Xd);
Print(Xd);
Xd /*=c; // converting in radians again
Yd /*=c;
Zd /*=c;
}
Estoy imprimiendo Xd, Yd, Zd ... Está bien si se puede jugar o dentro de +/- 10 grados ... Pero por más de 10 grados estoy obteniendo valores de falla. Quiero decir, es informar si el dispositivo es satable o no O si se está moviendo en el lado positivo (en sentido contrario a las agujas del reloj) o en el lado negativo (en el sentido de las agujas del reloj) PERO no da la salida exacta de +/- 40 grados mientras está en un ángulo de 40 con respeto al suelo!
¿Qué me estoy perdiendo? Por favor, ayúdame ... Lo he hecho recomendando algunos documentos y blogs.
// todas las variables se toman correctamente (según mi conocimiento).
Una pregunta más: ¿Realmente necesito conocer estos ángulos? ¿O puedo usar los valores actuales también para hacer que mi cuadrángulo sea estable? ¿Qué otra información necesitaré para que mi cuadrángulo sea estable?