Las formulas que he usado son las siguientes:
x=analogRead(xaxis);
y=analogRead(yaxis);
z=analogRead(zaxis);
unsigned long tt = micros();
float aax = (((x*5000.0)/1023.0)-XError)/RESOLUTION;
float aay = (((y*5000.0)/1023.0)-YError)/RESOLUTION;
float aaz = (((z*5000.0)/1023.0)-ZError)/RESOLUTION;
float rho = atan(aax/sqrt(pow(aay,2)+pow(aaz,2)))*(360/2*PI);
float phi = atan(aay/sqrt(pow(aax,2)+pow(aaz,2)))*(360/2*PI);
float theta = atan(sqrt(pow(aay,2)+pow(aax,2))/aaz)*(360/2*PI);
tt = micros() - tt;
el último término es para pasar de radianes a grados.
Como puede verse, el cálculo es bastante costoso debido a la gran cantidad de cálculos que hay que realizar por lo que intentaremos acelerar y simplificar dichas operaciones. Utilizando la función micros() podemos ver los microsegundos que se tardan en resolver todos los cálculos lo que nos será muy útil para ver si vamos mejorando el algoritmo.
El valor de "tt" es aproximadamente de 980 microsegundos por lo que disponemos ahora de un indicador de lo que tarda en calcularse los grados a partir del voltaje de entrada del acelerómetro.
Aplicando algunas optimizaciones podemos disponer ya de los valores en grados de los tres ejes de nuestro acelerómetro
#define DEBUGMODE
#define _360div2xPI 57.29577951
#define _5000div1023 4.887585533
#define MMA7260_RESOLUTION 800 //0.8 v/g -> resolucion de 1.5g -> 800mV/g
#define MMA7260_VOLTAGE 3300.0; //mv voltage al que está conectado el acelerómetro
const float ZOUT1G = 2450; // mv Voltage en Zout a 1G
const float ZOUT0G = 1650; // mv Voltage en Zout a 1G
#define MMA7260_ZOUT_1G 850 // mv Voltage en Zout a 1G
const float XOUT1G = 2450; // mv Voltage en Zout a 1G
const float XOUT0G = 1650; // mv Voltage en Zout a 1G
const float YOUT1G = 2450; // mv Voltage en Zout a 1G
const float YOUT0G = 1650; // mv Voltage en Zout a 1G
#define NADJ 50 // Número de lecturas para calcular el error
// Entradas analógicas donde van los sensores
#define XAXIS_PORT 0
#define YAXIS_PORT 1
#define ZAXIS_PORT 2
// Salida digital del led de la placa
#define LEDPIN_PORT 13
float XError,YError,ZError;
float z,x,y;
float AccelAdjust(int axis)
{
float acc = 0;
for (int j=0;j < NADJ;j++)
{
float lectura=analogRead(axis);
acc = acc + (lectura*_5000div1023);
delay(11); //número primo para evitar ciclos de lectura proporcionales
}
return acc/NADJ;
}
void setup()
{
Serial.begin(9600); // 9600 bps
pinMode(XAXIS_PORT,INPUT);
pinMode(YAXIS_PORT,INPUT);
pinMode(ZAXIS_PORT,INPUT);
pinMode(LEDPIN_PORT, OUTPUT);
digitalWrite(LEDPIN_PORT, HIGH);
XError = AccelAdjust(XAXIS_PORT);
YError = AccelAdjust(YAXIS_PORT);
ZError = AccelAdjust(ZAXIS_PORT);
ZError = ZError - MMA7260_ZOUT_1G;
digitalWrite(LEDPIN_PORT, LOW);
#ifdef DEBUGMODE
Serial.println("Ajustado acelerometro eje X");
Serial.print("Error de X= ");
Serial.println(XError);
Serial.println("Ajustado acelerometro eje Y");
Serial.print("Error de Y= ");
Serial.println(YError);
Serial.println("Ajustado acelerometro eje Z");
Serial.print("Error de Z= ");
Serial.println(ZError);
#endif
}
void loop()
{
x=analogRead(XAXIS_PORT);
y=analogRead(YAXIS_PORT);
z=analogRead(ZAXIS_PORT);
unsigned long tt = micros();
float aax = ((x*_5000div1023)-XError)/MMA7260_RESOLUTION;
float aay = ((y*_5000div1023)-YError)/MMA7260_RESOLUTION;
float aaz = ((z*_5000div1023)-ZError)/MMA7260_RESOLUTION;
float rho = atan(aax/sqrt((aay*aay)+(aaz*aaz)))*_360div2xPI;
float phi = atan(aay/sqrt((aax*aax)+(aaz*aaz)))*_360div2xPI;
float theta = atan(sqrt((aay*aay)+(aax*aax))/aaz)*_360div2xPI;
tt = micros()-tt;
#ifdef DEBUGMODE
Serial.print(aax);
Serial.print(" ");
Serial.print(aay);
Serial.print(" ");
Serial.print(aaz);
Serial.println();
Serial.print(rho);
Serial.print(" ");
Serial.print(phi);
Serial.print(" ");
Serial.print(theta);
Serial.println();
Serial.print(tt);
Serial.println();
#endif
delay(1000);
}
En esta imagen puede verse, con el acelerómetro inclinado hacia arriba, el eje Y y Z cerca de 90º.
Con este código modificado dispondremos de las inclinaciones en reposo así como un tiempo de alrededor de 930 microsegundos (50 microsegundos más rápido). Se han cambiado algunas variables por #defines así como algunos cálculos se han convertido en constantes. Sin embargo el mayor tiempo se consume en el cálculo de las funciones trigonométricas que espero poder mejorar usando por ejemplo lookup-tables.