This is my first post of some of my code, so please be gentle. But please please help me optimise it!! ;D
I've been closely following Knuckles904's work getting the Wii MotionPlus working, and then in combination with the Nunchuck.
I have taken this work and combined it back into Jordi Muñoz's Kalman Filter example used in ArduPilot http://diydrones.com/profiles/blog/show?id=705844%3ABlogPost%3A23188
I have also switched the Nunchuck Init code to use the 'new' style init, which means the Nunchuck data is Non-Encrypted, avoiding the need to XOR the bytes...
The bits I am completely vague on - most of the Kalman stuff ;D ;D I have tried to extend it to all 3-axis, but I think its off somewhere... I eventually would love to incorporate this with GPS data .. But first things first, I want to extend the code to work properly to make a 6DOF IMU from Nintendo bits !! So any help here would be greatly appreciated.
I know the gyro data is off in the kalman filter - obviously need to play with some numbers - but i'm not real sure on to what and where !
Here is the code
I've had a play, and think I have it kind of working with 3 axis incorporated.
Its a modification of Ed Simmons code which also shares the same roots as mine from www.eclipseaudioservices.co.uk/extras/helicopter.html
The first line of output - Jordi's original 1-axis kalman works fine still. But the numbers on the other axis' are clearly screwy ! :-[
// Modified Kalman code using the Y-Axis from a Wii MotionPlus and the X and Z from a Nunchuck.
// Nunchuck joystick provides corrections - X for Gyro, Y for Accelerometer
// Also uses "new" style Init to provide unencrypted data from the Nunchuck to avoid the XOR on each byte.
// Requires the WM+ and Nunchuck to be connected to Arduino pins D3/D4 using the schematic from Knuckles904
// See http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html
// Kalman Code Originally By Jordi Munoz... //Si deseas la version en Espanol, no dudes en escribirme...
//
// Altered by Adrian Carter <adrian@canberrariders.org.au> to work with WiiM+ and Nunchuck
#include <math.h>
#include <Wire.h>
/////////////////////////////////////////
#define NUMREADINGS 5 //Gyro noise filter
// test for 3 axis
int readingsX[NUMREADINGS];
int readingsY[NUMREADINGS];
int readingsZ[NUMREADINGS]; // the readings
int totalX = 0;
int totalY = 0;
int totalZ = 0; // the running total
int averageX = 0; // the average
int averageY = 0;
int averageZ = 0;
int lastaverageZ =0;
// End of 3 axis stuff
int readings[NUMREADINGS]; // the readings from the analog input (gyro)
int index = 0; // the index of the current reading
int total = 0; // the running total
int average = 0; // the average
int inputPin =0; // Gyro Analog input
//WM+ stuff
byte data[6]; //six data bytes
int yaw, pitch, roll; //three axes
int yaw0, pitch0, roll0; //calibration zeroes
///////////////////////////////////////
float dt = .06; //( 1024.0 * 256.0 ) / 16000000.0; (Kalman)
int mydt = 20; //in ms.
static float P[2][2] = { //(Kalman)
{
1, 0 }
, //(Kalman)
{
0, 1 }
,//(Kalman)
}; //(Kalman)
// extra 2 axis..
static float P1[2][2] = { { 1, 0 } , { 0, 1 } ,};
static float P2[2][2] = { { 1, 0 } , { 0, 1 } ,};
static float P3[2][2] = { { 1, 0 } , { 0, 1 } ,}; //(Kalman)
/*
* Our two states, the angle and the gyro bias. As a byproduct of computing
* the angle, we also have an unbiased angular rate available. These are
* read-only to the user of the module.
*/
float angle; //(Kalman)
float angleX;
float angleY;
float angleZ;
float q_bias; //(Kalman)
float q_bias_X;
float q_bias_Y;
float q_bias_Z;
float rate_X;
float rate_Y;
float rate_Z;
float q_m_X; //(Kalman)
float q_m_Y;
float q_m_Z;
float rotationZ;
float rate; //(Kalman)
float q_m; //(Kalman)
// adjusts
int gyro_adjust = 1;
int accel_adjust = 128;
int joy_x_axis = 0; //NunChuck Joysticks potentiometers
int joy_y_axis = 0;
int ax_m=0; //NunChuck X acceleration
// Re-added Y Axis
int ay_m=0; //NunChuck Y acceleration
int az_m=0; //NunChuck Z acceleration
byte outbuf[6]; // array to store arduino output
int cnt = 0; //Counter
unsigned long lastread=0;
/*
* R represents the measurement covariance noise. In this case,
* it is a 1x1 matrix that says that we expect 0.3 rad jitter
* from the accelerometer.
*/
float R_angle = .3; //.3 deafault, but is adjusted externally (Kalman)
/*
* Q is a 2x2 matrix that represents the process covariance noise.
* In this case, it indicates how much we trust the acceleromter
* relative to the gyros.
*/
static const float Q_angle = 0.001; //(Kalman)
static const float Q_gyro = 0.003; //(Kalman)
void switchtonunchuck(){
/* PORTE ^= 32; // Toggle D3 LOW
PORTG |= 32; // Force D4 HIGH */ // Fast switching of pins... Arduino MEGA specific...
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(4, HIGH);
}
void switchtowmp(){
/* PORTG ^= 32; // Toggle D4 LOW
PORTE |= 32; // Force D3 HIGH */ // Fast switching of pins... Arduino MEGA Specific pin maps, so switched back to 'normal' code for example
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(3, HIGH);
}
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.send (0xF0); // sends memory address
Wire.send (0x55); // sends sent a zero.
Wire.endTransmission (); // stop transmitting
delay(100);
Wire.beginTransmission (0x52); // transmit to device 0x52
Wire.send (0xFB); // sends memory address
Wire.send (0x00); // sends sent a zero.
Wire.endTransmission (); // stop transmitting
}
void setup()
{
Serial.begin(115200);
/* DDRG |= 32; // Force D3 to Output // Arduino MEGA "fast" pin config. Reverted to 'normal' for example post.
DDRE |= 32; // Force D4 to Output
PORTG ^= 32; // Toggle D3 HIGH
PORTE ^= 32; // Toggle D4 HIGH */
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
digitalWrite(3, HIGH);
digitalWrite(4, HIGH);
Wire.begin (); // join i2c bus with address 0x52 (nunchuck)
switchtowmp();
wmpOn();
calibrateZeroes();
switchtonunchuck();
nunchuck_init (); // send the initilization handshake
for (int i = 0; i < NUMREADINGS; i++)
readings[i] = 0; // initialize all the readings to 0 (gyro average filter)
}
void send_zero () //(nunchuck)
{
Wire.beginTransmission (0x52); // transmit to device 0x52 (nunchuck)
Wire.send (0x00); // sends one byte (nunchuck)
Wire.endTransmission (); // stop transmitting (nunchuck)
}
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 wmpOff(){
Wire.beginTransmission(82);
Wire.send(0xf0);//address then
Wire.send(0x55);//command
Wire.endTransmission();
}
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;// for each zero
roll0+=(((data[5] >> 2) << 8)+data[2])/10;
}
}
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();
}
//see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
//for info on what each byte represents
yaw=((data[3] >> 2) << 8)+data[0]-yaw0;
pitch=((data[4] >> 2) << 8)+data[1]-pitch0;
roll=((data[5] >> 2) << 8)+data[2]-roll0;
}
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++){
data[i]=Wire.receive();
}
yaw=((data[3] >> 2) << 8)+data[0];
pitch=((data[4] >> 2) << 8)+data[1];
roll=((data[5] >> 2) << 8)+data[2];
}
void loop()
{
if((millis()-lastread) >= mydt) { // sample every dt ms -> 1000/dt hz.
lastread = millis();
total += readings[index];
totalX += readingsX[index];
totalY += readingsY[index];
totalZ += readingsZ[index]; // add the reading to the total
switchtowmp();
receiveData();
switchtonunchuck();
readings[index] = int(pitch/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
readingsX[index] = int(yaw/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
//Continued
). I need to re-do the breakout board I made so I can move the thing more than 6inches from the arduino