Wii Motion Plus not initializing

I am building a quadcopter and I want it to stabilize itself.

That said I have been messing around with a stock Nintendo Wii Motion plus and I have been unable to get it to start sending data...

Here is the code that I found to get it started.

#include 
byte data[6]; //six data bytes
int yaw, pitch, roll; //three axes
int yaw0, pitch0, roll0; //calibration zeroes

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

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

void calibrateZeroes(){
  for (int i=0;i<10;i++){
    wmpSendZero();
    Wire.requestFrom(0x52,6);
    for (int i=0;i<6;i++){
      data[i]=Wire.receive();
    }
    yaw0+=(((data[3]>>2)<<8)+data[0])/10; //average 10 readings
    pitch0+=(((data[4]>>2)<<8)+data[1])/10;
    roll0+=(((data[5]>>2)<<8)+data[2])/10;
  }
  Serial.print("Yaw0:");
  Serial.print(yaw0);
  Serial.print(" Pitch0:");
  Serial.print(pitch0);
  Serial.print(" Roll0:");
  Serial.println(roll0);
}

void receiveData(){
  wmpSendZero(); //send 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.receive();
  }
  yaw=((data[3]>>2)<<8)+data[0]-yaw0;
  pitch=((data[4]>>2)<<8)+data[1]-pitch0;
  roll=((data[5]>>2)<<8)+data[2]-roll0;
}/Extension_Controllers#Wii_Motion_Plus
//for info on what each byte represents
void setup(){
  Serial.begin(115200);
  Serial.println("WM+ tester");
  Wire.begin();
  wmpOn(); //turn WM+ on
  calibrateZeroes(); //calibrate zeroes
  delay(1000);
}

void loop(){
  receiveData(); //receive data and calculate yaw pitch and roll
  Serial.print("yaw:");
  Serial.print(yaw); //for info on which axis is which
  Serial.print(" pitch:");
  Serial.print(pitch);
  Serial.print(" roll:");
  Serial.println(roll);
  delay(100);
}

My nunchuck works fine with some other code so it cant be my board or one of the libraries...

Ideas ???

Does anyone have some basic sample code and a picture of their setup ?

Thanks.

Checkout.... er..... my next post

Checkout http://aeroquad.com/showthread.php?12-Low-cost-6DOF-for-this-project I'm guessing it's worth you scanning the whole thread :)

I've the same problem stuck with just:

WM+ tester

If I remove the cable, I start to receive just:

yaw:0 pitch:0 roll:0 yaw:0 pitch:0 roll:0 yaw:0 pitch:0 roll:0 yaw:0 pitch:0 roll:0 yaw:0 pitch:0 roll:0 yaw:0 pitch:0 roll:0 yaw:0 pitch:0 roll:0 yaw:0 pitch:0 roll:0 yaw:0 pitch:0 roll:0