Slowly but surely. I hope to make a lot more progress this evening. Here's one small fix that I posted on another blog. (Note that I have switched roll and pitch from Adrian's (and other's) posts to make it inline with Wiimote/Extension Controllers - WiiBrew data structure definition.
void calibrateZeroes(){
long y0 = 0;
long p0 = 0;
long r0 = 0;
const int avg = 10;
for (int i=0;i<avg; i++){
wmpSendZero();
Wire.requestFrom(0x52,6);
for (int t=0;t<6;t++){
data[t]=Wire.receive();
}
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;
}
Some other changes that I'm making is honoring the dual speed modes of the Wii Motion Plus. There are 3 extra bits in the data that indicate if the output data is fast or slow. This presumably maps directly to the 500 degree/sec and the 2000 degree/sec modes of the Invensense IDG-600.
I also went all the way back to the original articles and code by Tom Pycke. A lot of changes have been made along the way - not all of them valid. Plus I wanted to wrap my head around the real issues with Kalman Filtering.
I'll post more soon if I can make progress.
Duck