Go Down

Topic: Kalman Filtered Nunchuck & Wii Motion Plus (Read 30 times) previous topic - next topic


Aug 05, 2009, 03:41 am Last Edit: Aug 05, 2009, 05:14 am by adr1an Reason: 1
Checkout http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus if you haven't.

In 'slow' mode, 20 represents turning at about 1 degree per second. So you divide by 20 to get the degrees per second. At high speed (Low speed bit = 0) 20 represents turning at about 5 degree per second, So you have to divide by 4 to get the degrees per second.

You will also see in the Data Format table that Byte 3 and 4, bits 0 and 1 contain flags for what mode it is in.

Thats why my original code is off - It only calculates in the 'slow mode' 20/sec mode correctly and doesn't look at the mode-bits to compensate...
Checkout my projects development blog @ SLiDA


Ahh...got it ! and I meant 14 bits from the a/d. It all makes sense now. Hope your coming along on the code. A lot of autopilots and other things sure can use an inexpensive GOOD 6DOF. I think your in the right track incorporating the kalman filter. That is going to be the secret in making this work as a 6DOF. Good work.


Yes, that's right.  Here's a code snippet of what this would look like:
Code: [Select]

bool slowYaw;
bool slowPitch;
bool slowRoll;

const int wmpSlowToDegreePerSec = 20;
const int wmpFastToDegreePerSec = 4;

void receiveRaw(){
 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++){
 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;

void loop() {

 const int rollScale  = slowRoll  ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
 const int pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
 const int yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
 readingsX[index] = int(roll/rollScale);  
 readingsY[index] = int(pitch/pitchScale);
 readingsZ[index] = int(yaw/yawScale);



Has anyone actually confirmed what wiibrew says about fast and slow divisions to be true. I had originally posted on that site that the division factor for the yaw axis only was 5 on fast mode. Since then it seems to have been changed to what it now says. I am running a similar code to what duckhead just posted through simple integration program and pitch and roll axes do not seem to be agreeing with wiibrew. Only yaw works as it should with the division factor of 4. Remember that wiibrew is a wiki so take it for what its worth.

Can anyone confirm correct working of the 3 axes in fast mode?


Can you post the latest code your playing with ?

Go Up