Modified the FreeIMU library for Wii Motion plus and Nunchuk

hi,
once again thanks for you great FreeIMU library, i work on reading data from combine wii motion plus and nunchuk with the help of this blog http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html and able to get value from these and then i modified the freeimu library to get the yaw pitch roll value by applying the sensor fusion i-e AHRSupdate() function but the value of yaw pitch and roll is not correct ,it gives me y=15, p=37, r=43 even if i place the sensor horizontal and did not move it, as it is shown in the picture

 https://mail-attachment.googleusercontent.com/attachment/u/0/?ui=2&ik=58f2ad39b5&view=att&th=139634ca232a0a0b&attid=0.2&disp=inline&realattid=f_h6c7u6do1&safe=1&zw&saduie=AG9B_P-HEsbSzOf0c1t9f7IJF-lX&sadet=1345991177669&sads=600Au34B5FGMBz6eGXlDGPPFZ78&sadssc=1
,
 and if i rote the sensor in one direction let say in Roll direction all the values changes i-e the yaw pitch and roll.the sampling frequency is about 57Hz.plz help me out i will b very thankful. sorry for english

the following is the code:

#include <math.h>
#include <Wire.h>

 float polarities[3];
 float gains[3];

//The WMP has two modes: 2000 deg/s (slow) and 500 deg/s (fast). It is assumed that the sensitivity of the output follows
//the same ratio (it seems that the sensitivity is 20 mV/deg/s and 0.5 mV/deg/s), which is to say it's proportional to 4:1.
static const int wmpSlowToDegreePerSec = 20;
static const int wmpFastToDegreePerSec = wmpSlowToDegreePerSec/5;

// raw 3 axis gyro readings
float readingsX;
float readingsY;
float readingsZ;

byte data[6]; //six data bytes

// normalized (scaled per WM+ mode) rotation values
float yaw = 0;
float pitch = 0;
float roll = 0;

// calibration zeroes
float yaw0 = 0;
float pitch0 = 0;
float roll0 = 0;

// WM+ state variables - if true, then in slow (high res) mode
bool slowYaw;
bool slowPitch;
bool slowRoll;
//bool debug = false;

// Nunchuck variables

float accelAngleX=0; //NunChuck X angle
float accelAngleY=0; //NunChuck Y angle
float accelAngleZ=0; //NunChuck Z angle

void switchtonunchuck(){
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(4, HIGH);
}

/*
 * Switch control from Nunchuck to WM+ via I2C.
 */
void switchtowmp(){
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(3, HIGH);
}

/*
 * Initialize Nunchuck.
 */
void nunchuck_init () //(nunchuck)
{
  // Uses New style init - no longer encrypted so no need to XOR bytes later... might save some cycles
  Wire.beginTransmission (0x52); // transmit to device 0x52
  Wire.write (0xF0); // writes memory address
  Wire.write (0x55); // writes sent a zero.
  Wire.endTransmission (); // stop transmitting
  delay(100);
  Wire.beginTransmission (0x52); // transmit to device 0x52
  Wire.write (0xFB); // writes memory address
  Wire.write (0x00); // writes sent a zero.
  Wire.endTransmission (); // stop transmitting
}

/*
 * Initialize everything.
 */
void setup()
{
  Serial.begin(115200);

  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  digitalWrite(3, HIGH);
  digitalWrite(4, HIGH);
 setGains(1.0,1.0,1.0);
     setRevPolarity(0,0,0);
  Wire.begin (); // join i2c bus with address 0x52 (nunchuck)
  switchtowmp();
  wmpOn();
  calibrateZeroes();
  switchtonunchuck();
  nunchuck_init (); // write the initilization handshake

}

void wmpOn(){
  Wire.beginTransmission(0x53);//WM+ starts out deactivated at address 0x53
  Wire.write(0xfe); //write 0x04 to address 0xFE to activate WM+
  Wire.write(0x04);
  Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
}

void wmpOff(){
  Wire.beginTransmission(82);
  Wire.write(0xf0);//address then
  Wire.write(0x55);//command
  Wire.endTransmission();
}

void wmpwriteZero(){
  Wire.beginTransmission(0x52);//now at address 0x52
  Wire.write(0x00); //write zero to signal we want info
  Wire.endTransmission();
}

/*
 * Find the steady-state of the WM+ gyros. This function reads the gyro values 100 times and averages them.
 * Those values are used as the baseline steady-state for all subsequent reads. As a result it is very
 * important that the device remain relatively still during startup.
 */
void calibrateZeroes(){
  long y0 = 0;
  long p0 = 0;
  long r0 = 0;
  const int avg = 100;
  for (int i=0;i<avg; i++){
    wmpwriteZero();
    Wire.requestFrom(0x52,6);
    for (int t=0;t<6;t++){
      data[t]=Wire.read();
    }
    y0+=(((data[3] >> 2) << 8)+data[0]);
    r0+=(((data[4] >> 2) << 8)+data[1]);
    p0+=(((data[5] >> 2) << 8)+data[2]);
  }

  yaw0 = y0/avg;
  roll0 = r0/avg;
  pitch0 = p0/avg;
}

void readData(){
  wmpwriteZero(); //write zero before each request (same as nunchuck)
  Wire.requestFrom(0x52,6);//request the six bytes from the WM+
  for (int i=0;i<6;i++){
    data[i]=Wire.read();
  }
  yaw=((data[3] >> 2) << 8)+data[0];
  roll=((data[4] >> 2) << 8)+data[1];
  pitch=((data[5] >> 2) << 8)+data[2];

  slowPitch = data[3] & 1;
  slowYaw = data[3] & 2;
  slowRoll = data[4] & 2;

  //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
  //for info on what each byte represents
  yaw -= yaw0;
  roll -= roll0;
  pitch -= pitch0;

}

void loop()
{

    switchtowmp();
    readData();
    switchtonunchuck();
    readNunchuck();

    // Handle the fast/slow bits of the Wii MotionPlus
    const int rollScale = slowRoll ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    const int pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    const int yawScale = slowYaw ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;

    readingsX = (roll/rollScale)* polarities[0] * gains[0]; // read from the gyro sensor
    readingsY = (pitch/pitchScale)* polarities[1] * gains[1]; // read from the gyro sensor
    readingsZ = (yaw/yawScale)* polarities[2] * gains[2]; // read from the gyro sensor

    float ypr[3];
    getYawPitchRoll(ypr);
    Serial.print(" Yaw : ");
    Serial.print(ypr[0],5);
    Serial.print(" Pitch : ");
    Serial.print(ypr[1],5);
    Serial.print(" Roll : ");
    Serial.println(ypr[2],5);

      delay(10);

}

/*
 * Write a zero byte to the nunchuck.
 */
void write_zero () //(nunchuck)
{
  Wire.beginTransmission (0x52); // transmit to device 0x52 (nunchuck)
  Wire.write (0x00); // writes one byte (nunchuck)
  Wire.endTransmission (); // stop transmitting (nunchuck)
}

/*
 * Request and read the current accelerometer values from the Nunchuck.
 */
void readNunchuck () {
    int cnt = 0;
    byte outbuf[6]; // array to store NunChuck output

    Wire.requestFrom (0x52, 6); // request data from nunchuck

    while (Wire.available ()) //(NunChuck)
    {
      outbuf[cnt] = Wire.read (); // read byte as an integer //(NunChuck)
      cnt++;//(NunChuck)
    }

    write_zero (); // write the request for next bytes
    // If we readd the 6 bytes, then process them //(NunChuck)
    if (cnt >= 5) //(NunChuck)
    {
 processAccelerometers (outbuf); //(NunChuck)
    }
}

/*
 * This function processes the byte array from the wii nunchuck.
 *
 * The execution of this function results in the updating of 3 globals, accelAngleX, accelAngleY, and accelAngleZ with the angle in radians.
 * If the Wii Motion Plus and the Nunchuck are oriented together in the way this code assumes then:
 * Nunchuck X maps to Wii Motion Plus Roll
 * Nunchuck Y maps to Wii Motion Plus Pitch
 * Nunchuck Z maps to Wii Motion Plus Yaw
 *
 */
void processAccelerometers (byte outbuf[])
{
  //Pull the bits out of the array for each axis. Other implementations floating around are incorrect with the way
  //they operate on the lower 2 bits. Thanks to evilBunny for this code.
  int ax_m = (outbuf[2] << 2) + ((outbuf[5] >> 2) & 0x03);
  int ay_m = (outbuf[3] << 2) + ((outbuf[5] >> 4) & 0x03);
  int az_m = (outbuf[4] << 2) + ((outbuf[5] >> 6) & 0x03);
  accelAngleX = ax_m;
  accelAngleY = ay_m;
  accelAngleZ = az_m;
}

///////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////

#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef (2.0f * 0.1f) // 2 * integral gain
    float iq0, iq1, iq2, iq3;
    float exInt=0, eyInt=0, ezInt=0; // scaled integral error
    volatile float twoKp=twoKpDef; // 2 * proportional gain (Kp)
    volatile float twoKi=twoKiDef; // 2 * integral gain (Ki)
    volatile float q0=1.0f, q1=0.0f, q2=0.0f, q3=0.0f; // quaternion of sensor frame relative to auxiliary frame
    volatile float integralFBx= 0.0f, integralFBy= 0.0f, integralFBz= 0.0f;
    unsigned long lastUpdate=0, now=0; // sample period expressed in milliseconds
    float sampleFreq; // half the sample period expressed in seconds
    int startLoopTime;

     //setGains(1.0,1.0,1.0);
     //setRevPolarity(0,0,0);
  //q0 = 1.0f;
 // q1 = 0.0f;
 // q2 = 0.0f;
 // q3 = 0.0f;
  //exInt = 0.0;
  //eyInt = 0.0;
  //ezInt = 0.0;
  //twoKp = twoKpDef;
  //twoKi = twoKiDef;
 // integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;
 // lastUpdate = 0;
  //now = 0;

 void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az)
  {

    float recipNorm;
  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
  float qa, qb, qc;

  // Auxiliary variables to avoid repeated arithmetic
  q0q0 = q0 * q0;
  q0q1 = q0 * q1;
  q0q2 = q0 * q2;
  q0q3 = q0 * q3;
  q1q1 = q1 * q1;
  q1q2 = q1 * q2;
  q1q3 = q1 * q3;
  q2q2 = q2 * q2;
  q2q3 = q2 * q3;
  q3q3 = q3 * q3;

  // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
 /* if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) {
    float hx, hy, bx, bz;
    float halfwx, halfwy, halfwz;

    // Normalise magnetometer measurement
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;

    // Reference direction of Earth's magnetic field
    hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
    hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
    bx = sqrt(hx * hx + hy * hy);
    bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

    // Estimated direction of magnetic field
    halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
    halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
    halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);

    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex = (my * halfwz - mz * halfwy);
    halfey = (mz * halfwx - mx * halfwz);
    halfez = (mx * halfwy - my * halfwx);
  }
 */

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
    float halfvx, halfvy, halfvz;

    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);

    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Estimated direction of gravity
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;

    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex += (ay * halfvz - az * halfvy);
    halfey += (az * halfvx - ax * halfvz);
    halfez += (ax * halfvy - ay * halfvx);

  }

  // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
    // Compute and apply integral feedback if enabled
    if(twoKi > 0.0f) {
      integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0f / sampleFreq);
      integralFBz += twoKi * halfez * (1.0f / sampleFreq);

      gx += integralFBx; // apply integral feedback
      gy += integralFBy;
      gz += integralFBz;

    }
    else {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }

    // Apply proportional feedback
  /// Serial.print(sampleFreq,6);
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;

  }

  // Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));

  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;

}

float invSqrt(float number)
  {

  volatile long i;
  volatile float x, y;
  volatile const float f = 1.5F;

  x = number * 0.5F;
  y = number;
  i = * ( long * ) &y;
  i = 0x5f375a86 - ( i >> 1 );
  y = * ( float * ) &i;
  y = y * ( f - ( x * y * y ) );
  return y;
}

void getValues(float * values)
{ float val[6];

     val[3]=readingsX;
    val[4]=readingsY;
    val[5]=readingsZ;
    val[0]=accelAngleX;
    val[1]=accelAngleY;
    val[2]=accelAngleZ;

    values[0]=val[0];
    values[1]=val[1];
    values[2]=val[2];
    values[3]=val[3];
    values[4]=val[4];
    values[5]=val[5];

}

void getQ(float * q) {
  float val[6];
  getValues(val);

 now = micros();
  sampleFreq = 1.0 / ((now - lastUpdate) / 1000000.0);
  lastUpdate = now;

    AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2]);
 /* filterUpdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2]);
   q[0] =SEq_1 ;
  q[1] = SEq_2;
  q[2] = SEq_3;
  q[3] = SEq_4;*/
  q[0] = q0;
  q[1] = q1;
  q[2] = q2;
  q[3] = q3;
}

void getEuler(float * angles) {
  float q[4]; // quaternion
  getQ(q);
  angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1) * 180/M_PI; // psi
  angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]) * 180/M_PI; // theta
  angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 180/M_PI; // phi
}

void getYawPitchRoll(float * ypr) {
  float q[4]; // quaternion
  float gx, gy, gz; // estimated gravity direction
  getQ(q);

  gx = 2 * (q[1]*q[3] - q[0]*q[2]);
  gy = 2 * (q[0]*q[1] + q[2]*q[3]);
  gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];

  ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1) * 180/M_PI;
  ypr[1] = atan(gx / sqrt(gy*gy + gz*gz)) * 180/M_PI;
  ypr[2] = atan(gy / sqrt(gx*gx + gz*gz)) * 180/M_PI;
}
void setGains(float _Xgain, float _Ygain, float _Zgain) {
  gains[0] = _Xgain;
  gains[1] = _Ygain;
  gains[2] = _Zgain;
}
void setRevPolarity(bool _Xpol, bool _Ypol, bool _Zpol) {
  polarities[0] = _Xpol ? -1 : 1;
  polarities[1] = _Ypol ? -1 : 1;
  polarities[2] = _Zpol ? -1 : 1;
}