MMA7260 y Arduino

Tengo un acelerometro de 3 ejes MMA7260 y  estoy experimentando con el, pero creo que no es lo apropiado para estabilizar a Quadcopter V0. Estoy buscando un Wii motion plus a buen precio para quitarle los giroscopios y usar estos en el proyecto.

El codigo inicial  del acelerometro mma7260 fue sacado de aqui:

Y aqui la primera modificacion

#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(" x");
  Serial.print(aax);
  Serial.print(" y ");
  Serial.print(aay);
  Serial.print(" z");        
  Serial.print(aaz);
  Serial.println();
  Serial.println(); */
  Serial.print("X  ");
  Serial.print(rho);
  Serial.print("   Y  ");
  Serial.print(phi);
  Serial.print("   Z  ");
  Serial.print(theta);
  Serial.println();
// Serial.print("mS");
// Serial.print(tt);
#endif
  delay(1000);
}