domingo, 6 de febrero de 2011

Calculando la inclinación en grados de nuestro acelerómetro MMA7260

Una vez que disponemos de las aceleraciones estáticas de nuestro dispositivo tan sólo queda aplicar un poco de trigonometría para disponer para cada eje el ángulo.
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.

4 comentarios:

  1. Buenas noches, muy bueno tu post. Una pregunta, quiero construir un incinometro de que me de medidas precisas, como de 0,1g o algo asi, sabes si podria hacerlo con arduino? Gracias

    ResponderEliminar
  2. Hola,

    si, se puede hacer perfectamente con Arduino. Lo primordial es disponer de un giroscopio apropiado a la precisión que deseas. Puedes mirar estos enlaces para ver si te sirve alguno:

    http://www.cooking-hacks.com/
    http://www.sparkfun.com/

    Un saludo

    ResponderEliminar
  3. Estoy empezando a experimentar con arduino y tu codigo me a ahorrado mucho tiempo.
    Unicamente comentarte un pequeño apunte. Utilizas la funcion micros() para calcular el tiempo que cuesta hacer las operaciones, y despues explicas que cuesta alrrededor de 930 milisegundos, corrigeme si me equivoco pero creo que deberian ser microsegundos, casi un milisegundo.
    No veo la necesidad de depurar mucho el codigo.

    Saludos.

    ResponderEliminar
  4. En efecto, arriba hablaba de microsegundos y abajo de milisegundos... corregido. Muchas gracias.

    ResponderEliminar