Go Down

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

#15
##### Aug 05, 2009, 03:41 amLast 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

#### uphiearl

#16
##### Aug 05, 2009, 05:06 am
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.
Earl

#17
##### Aug 05, 2009, 04:12 pm
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++){    data[i]=Wire.receive();  }  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); ....        }`

Duck

#### knuckles904

#18
##### Aug 05, 2009, 11:39 pm
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?

#### uphiearl

#19
##### Aug 06, 2009, 01:20 am
Knuckles,
Can you post the latest code your playing with ?
Earl

#### knuckles904

#20
##### Aug 06, 2009, 02:00 am
Sure, here's the integration sketch i have been using. Its also configured so that it can be used with the rgb cube tester with minimal modifications for visual. But using this you can see that while turning the wm+ quickly, only one axis gives valid angular position.
Code: [Select]
`//Trapezoid or Runge-kutta 4th order integration program of wm+#include <Streaming.h>#include <Wire.h>//#include <WiiChuck.h>#define steps_per_deg_slow 20#define steps_per_deg_fast 4byte data[6];          //six data bytesint yaw, pitch, roll;  //three axesint yaw0, pitch0, roll0;  //calibration zeroesint time, last_time;float delta_t;int last_yaw[3], last_pitch[3], last_roll[3];float yaw_deg, pitch_deg, roll_deg;int yaw_deg2, pitch_deg2, roll_deg2;int startTag=0xDEAD;int accel_x_axis, accel_y_axis, accel_z_axis;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 for each zero    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();  }  if(bitRead(data[3], 1)==1) yaw=(((data[3]>>2)<<8)+data[0]-yaw0)/steps_per_deg_slow;        //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus  else yaw=(((data[3]>>2)<<8)+data[0]-yaw0)/steps_per_deg_fast;  if(bitRead(data[3], 0)==1) pitch=(((data[4]>>2)<<8)+data[1]-pitch0)/steps_per_deg_slow;    //for info on what each byte represents  else pitch=(((data[4]>>2)<<8)+data[1]-pitch0)/steps_per_deg_fast;  if(bitRead(data[4], 1)==1) roll=(((data[5]>>2)<<8)+data[2]-roll0)/steps_per_deg_slow;  else roll=(((data[5]>>2)<<8)+data[2]-roll0)/steps_per_deg_fast;}void setup(){  Serial.begin(115200);  Serial.println("WM+ tester");  pinMode(3, OUTPUT);  digitalWrite(3, HIGH);  delay(1);  Wire.begin();  wmpOn();                        //turn WM+ on  calibrateZeroes();              //calibrate zeroes  delay(1000);}void loop(){  receiveData();                  //receive data and calculate yaw pitch and roll  time=millis();  delta_t=(time-last_time);  /*yaw_deg+=rk4Integrate(yaw, last_yaw[0], last_yaw[1], last_yaw[2], delta_t);  pitch_deg+=rk4Integrate(pitch, last_pitch[0], last_pitch[1], last_pitch[2], delta_t);  roll_deg+=rk4Integrate(roll, last_roll[0], last_roll[1], last_roll[2], delta_t);*/  yaw_deg+=trapIntegrate(yaw, last_yaw[0], delta_t);  pitch_deg+=trapIntegrate(pitch, last_pitch[0], delta_t);  roll_deg+=trapIntegrate(roll, last_roll[0], delta_t);  /*last_yaw[2]=last_yaw[1];  last_pitch[2]=last_pitch[1];  last_roll[2]=last_roll[1];  last_yaw[1]=last_yaw[0];  last_pitch[1]=last_pitch[0];  last_roll[1]=last_roll[0];*/  last_yaw[0]=yaw;  last_pitch[0]=pitch;  last_roll[0]=roll;  last_time=time;    yaw_deg2=yaw_deg;//+360;  pitch_deg2=pitch_deg;//+360;  roll_deg2=roll_deg;//+360;    Serial<<yaw_deg2<<"\t"<<pitch_deg2<<"\t"<<roll_deg2<<"\n";  /*  Serial.write((unsigned byte*)&startTag, 2);  Serial.write((unsigned byte*)&accel_x_axis, 2);      Serial.write((unsigned byte*)&accel_y_axis, 2);  Serial.write((unsigned byte*)&accel_z_axis, 2);  Serial.write((unsigned byte*)&pitch_deg2, 2);  Serial.write((unsigned byte*)&yaw_deg2, 2);  Serial.write((unsigned byte*)&roll_deg2, 2);*/  delay(10);}float trapIntegrate(int y2, int y1, float deltax){  float area=0;  area=(y2+y1)/2*deltax/1000;  return area;}float rk4Integrate(int y4, int y3, int y2, int y1, float deltax){  float area=0;  area=((y4+2*y3+2*y2+y1)/6)*deltax/1000;  return area;}`
As for the kalman sketch im working on, its going really slowly. No real kalman filtering stuff as of now cuz im still trying to wrap my head around it-I like jordi's code but its worthless to me unless I understand what everything does. Ill have to be the one to tune and problem solve so I need to fully understand it. Right now im working on that aspect. Luckily there are lots of great posts like tom pycke's but unluckily, very few give the right amount of physical examples and (lack of) abstraction for me to figure it all out.

#### uphiearl

#21
##### Aug 06, 2009, 03:50 am
OK thanks. The thing I noticed is if you turn on or reset the arduino the wmp numbers are about 8000 on yaw pitch and roll. So far so good.
Then if you don't touch anything, the numbers keep increasing and increasing at a rate of about 4 counts per second. If I physically move the wmp, the numbers don't change that much, and then they go back to increasing at a rate of about 4 counts per second. Is that what yours does?
I have another program that seams to work properly almost. So I don't suspect the wmp itself. I tried a second wmp and the same behavior.

Earl

#### uphiearl

#22
##### Aug 06, 2009, 03:56 am
Ahh... I think I found my problem. I was just using the WMP alone and not the WMP/NC with the transistor switches. I'll hook that circuit back up and see what I get.
Earl

#### uphiearl

#23
##### Aug 06, 2009, 04:53 am
Yaw seams to work ok. That was the problem. I now am on with the WMP/NC combo.

That's strange. Having just the WMP should work fine also because we aren't getting any data from the NC.
Must be because there is a state digital port 3 and 4 both low. But if it isn't connected to anything it shouldn't matter.

In the mode with the WMP always connected, the data gets screwed up.
Mmmm......
Earl

#24
##### Aug 06, 2009, 07:10 am
Here's my latest.  It's seems to be working well for me, but I have more cleanup and optimizations, as well as some more thought on correlating the linear axes from the nunchuk to the rotational axes of the WMP.
Code: [Select]
`// Modified Kalman code using Roll, Pitch, and Yaw from a Wii MotionPlus and X, Y, and Z accelerometers from a Nunchuck.// 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 and johnnyonthespot// See http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html// Kalman Code By Tom Pycke. http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data// Original zipped source with very instructive comments: http://tom.pycke.be/file_download/4//// Some of this code came via contributions or influences by Ed Simmons, Jordi Munoz, and Adrian Carter//// Created by Duckhead   v0.2   some cleanup and other optimization work remains.  also need to investigate correlation of nunchuk and WMP (which axis is which)#include <math.h>#include <Wire.h>/////////////////////////////////////////struct GyroKalman{  /* These variables represent our state matrix x */  float x_angle,        x_bias;  /* Our error covariance matrix */  float P_00,        P_01,        P_10,        P_11;        /*   * Q is a 2x2 matrix of the covariance. Because we   * assume the gyro and accelerometer noise to be independent   * of each other, the covariances on the / diagonal are 0.   *   * Covariance Q, the process noise, from the assumption   *    x = F x + B u + w   * with w having a normal distribution with covariance Q.   * (covariance = E[ (X - E[X])*(X - E[X])' ]   * We assume is linear with dt   */  float Q_angle, Q_gyro;  /*   * Covariance R, our observation noise (from the accelerometer)   * Also assumed to be linair with dt   */  float R_angle;};struct GyroKalman rollData;struct GyroKalman pitchData;struct GyroKalman yawData;// test for 3 axisint readingsX;int readingsY;int readingsZ;                // the readings// End of 3 axis stuffint index = 0;                            // the index of the current readingint inputPin =0;                          // Gyro Analog input//WM+ stuffbyte data[6]; //six data bytesint yaw = 0;int pitch = 0;int roll = 0; //three axesint yaw0 = 0;int pitch0 = 0;int roll0 = 0; //calibration zeroesbool slowYaw;bool slowPitch;bool slowRoll;const int wmpSlowToDegreePerSec = 20;const int wmpFastToDegreePerSec = 4;///////////////////////////////////////float accelAngleX=0;      //NunChuck X anglefloat accelAngleY=0;      //NunChuck Y anglefloat accelAngleZ=0;      //NunChuck Z anglebyte outbuf[6];            // array to store arduino outputint cnt = 0;            //Counterunsigned 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. */static const float      R_angle      = .3; //.3 default/* * 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.01; //(Kalman)static const float      Q_gyro      = 0.04; //(Kalman)//These are the limits of the values I got out of the Nunchuk accelerometers (yours may vary).const int lowX = -215;const int highX = 221;const int lowY = -215;const int highY = 221;const int lowZ = -215;const int highZ = 255;/* * Nunchuk accelerometer value to degree conversion.  Output is -90 to +90 (180 degrees of motion).   */float angleInDegrees(int lo, int hi, int measured) {  float x = (hi - lo)/180.0;  return (float)measured/x;}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  initGyroKalman(&rollData, Q_angle, Q_gyro, R_angle);  initGyroKalman(&pitchData, Q_angle, Q_gyro, R_angle);  initGyroKalman(&yawData, Q_angle, Q_gyro, R_angle);    lastread = millis();}void initGyroKalman(struct GyroKalman *kalman, const float Q_angle, const float Q_gyro, const float R_angle) {  kalman->Q_angle = Q_angle;  kalman->Q_gyro  = Q_gyro;  kalman->R_angle = R_angle;    kalman->P_00 = 0;  kalman->P_01 = 0;  kalman->P_10 = 0;  kalman->P_11 = 0;}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(){  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;}void receiveData(){  receiveRaw();  //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 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];  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(){  unsigned long now = millis();  float dt = ((float)(now - lastread))/1000.0; //compute the time delta since last iteration, in seconds.  if (dt >= 0.01) {  //Only process delta angles if at least 1/100 of a second has elapsed    lastread = now;    switchtowmp();    receiveData();    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); // read from the gyro sensor    readingsY = (pitch/pitchScale); // read from the gyro sensor    readingsZ = (yaw/yawScale); // read from the gyro sensor        float tmpX = accelAngleX * (PI/180.0);    float tmpY = accelAngleY * (PI/180.0);    float tmpZ = accelAngleZ * (PI/180.0);    predict(&rollData, readingsX, dt);    predict(&pitchData, readingsY, dt);    predict(&yawData, readingsZ, dt);      float rollAngle = update(&rollData, tmpX);    float pitchAngle = update(&pitchData, tmpY);    float yawAngle = update(&yawData, tmpZ);    Serial.print("Roll: ");    Serial.print(rollAngle*(180.0/PI));    Serial.print("Pitch: ");    Serial.print(pitchAngle*(180.0/PI));    Serial.print("Yaw: ");    Serial.println(yawAngle*(180.0/PI));  }}//Stitch Here`

Duck

#25
##### Aug 06, 2009, 07:11 am
Continuation:
Code: [Select]
`//Stitch here/* * The kalman predict method.  See http://en.wikipedia.org/wiki/Kalman_filter#Predict * * kalman    the kalman data structure * dotAngle  Derivitive Of The (D O T) Angle.  This is the change in the angle from the gyro.  This is the value from the *           Wii MotionPlus, scaled to fast/slow. * dt        the change in time, in seconds; in other words the amount of time it took to sweep dotAngle * * Note: Tom Pycke's ars.c code was the direct inspiration for this.  However, his implementation of this method was inconsistent *       with the matrix algebra that it came from.  So I went with the matrix algebra and tweaked his implementation here. */void predict(struct GyroKalman *kalman, float dotAngle, float dt) {  kalman->x_angle += dt * (dotAngle - kalman->x_bias);  kalman->P_00 += -1 * dt * (kalman->P_10 + kalman->P_01) + dt*dt * kalman->P_11 + kalman->Q_angle;  kalman->P_01 += -1 * dt * kalman->P_11;  kalman->P_10 += -1 * dt * kalman->P_11;  kalman->P_11 += kalman->Q_gyro;}/* * The kalman update method.  See http://en.wikipedia.org/wiki/Kalman_filter#Update * * kalman    the kalman data structure * angle_m   the angle observed from the Wii Nunchuk accelerometer, in radians */float update(struct GyroKalman *kalman, float angle_m) {  const float y = angle_m - kalman->x_angle;  const float S = kalman->P_00 + kalman->R_angle;  const float K_0 = kalman->P_00 / S;  const float K_1 = kalman->P_10 / S;  kalman->x_angle += K_0 * y;  kalman->x_bias  += K_1 * y;    kalman->P_00 -= K_0 * kalman->P_00;  kalman->P_01 -= K_0 * kalman->P_01;  kalman->P_10 -= K_1 * kalman->P_00;  kalman->P_11 -= K_1 * kalman->P_01;    return kalman->x_angle;}void readNunchuck () {    Wire.requestFrom (0x52, 6);      // request data from nunchuck    while (Wire.available ()) //(NunChuck)    {      outbuf[cnt] = Wire.receive ();      // receive byte as an integer //(NunChuck)      cnt++;//(NunChuck)    }    send_zero (); // send the request for next bytes    // If we recieved the 6 bytes, then print them //(NunChuck)    if (cnt >= 5) //(NunChuck)    {      readAccelerometers (); //(NunChuck)    }    cnt = 0; //(NunChuck)}void readAccelerometers ()//This is the function to get the bytes from nintendo wii nunchuck{  int ax_m = (outbuf[2] * 2 * 2); //Axis X Accelerometer.  Shift 2 bits to the left.  int ay_m = (outbuf[3] * 2 * 2); //Axis Y Accelerometer.  Shift 2 bits to the left.  int az_m = (outbuf[4] * 2 * 2); //Axis Z Accelerometer.  Shift 2 bits to the left.  // byte outbuf[5] contains bits for z and c buttons  // it also contains the least significant bits for the accelerometer data  // so we have to check each bit of byte outbuf[5]  if ((outbuf[5] >> 2) & 1)  {    ax_m += 2;  }  if ((outbuf[5] >> 3) & 1)  {    ax_m += 1;  }  if ((outbuf[5] >> 4) & 1)  {   ay_m += 2;  }  if ((outbuf[5] >> 5) & 1)  {   ay_m += 1;  }  if ((outbuf[5] >> 6) & 1)  {    az_m += 2;  }  if ((outbuf[5] >> 7) & 1)  {    az_m += 1;  }  //The nunchuk accelerometers read ~511 steady state.  Remove that to zero the values.  ax_m -= 511;  ay_m -= 511;  az_m -= 511;      //Translate the raw accelerometer counts into corresponding degrees  accelAngleZ = angleInDegrees(lowZ, highZ, az_m);  accelAngleY = angleInDegrees(lowY, highY, ay_m);  accelAngleX = angleInDegrees(lowX, highX, ax_m);}`

#26
##### Aug 06, 2009, 07:18 am
cool - i'll have a fiddle shortly, just back from my local chip pimp with some new supplies...
Checkout my projects development blog @ SLiDA

#27
##### Aug 06, 2009, 07:28 am
Sorry.  Posted the wrong loop() method in my earlier post.  Use this one instead (the difference is that the gyro derivative of the angle needs to be in radians).
Code: [Select]
`void loop(){  unsigned long now = millis();  float dt = ((float)(now - lastread))/1000.0; //compute the time delta since last iteration, in seconds.  if (dt >= 0.01) {  //Only process delta angles if at least 1/100 of a second has elapsed    lastread = now;    switchtowmp();    receiveData();    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); // read from the gyro sensor    readingsY = (pitch/pitchScale); // read from the gyro sensor    readingsZ = (yaw/yawScale); // read from the gyro sensor        float tmpX = accelAngleX * (PI/180.0);    float tmpY = accelAngleY * (PI/180.0);    float tmpZ = accelAngleZ * (PI/180.0);    predict(&rollData, readingsX * (PI/180.0), dt);    predict(&pitchData, readingsY * (PI/180.0), dt);    predict(&yawData, readingsZ * (PI/180.0), dt);      float rollAngle = update(&rollData, tmpX);    float pitchAngle = update(&pitchData, tmpY);    float yawAngle = update(&yawData, tmpZ);    Serial.print("Roll: ");    Serial.print(rollAngle*(180.0/PI));    Serial.print("Pitch: ");    Serial.print(pitchAngle*(180.0/PI));    Serial.print("Yaw: ");    Serial.println(yawAngle*(180.0/PI));  }}`

Duck

#28
##### Aug 06, 2009, 07:52 amLast Edit: Aug 06, 2009, 07:52 am by adr1an Reason: 1
BTW.. for anyone looking for some help on what all the numbers mean.. heres some good ideas (in terms of what some of the constants in the kalman calcs are etc etc). Its for a straight complementary filter, but it shows the radians and angle calcs etc.

http://www.ohscope.com/
Checkout my projects development blog @ SLiDA

#### knuckles904

#29
##### Aug 06, 2009, 07:16 pm
Looks absolutely great. Beautiful even. Thats exactly what I want my code to look like structure wise. (I was planning on wrapping it all in a class) Very efficient. I do have one problem with it however. You didnt use any trig functions to process accelerometer data. I see that your way does work very well for some things but it makes it much more susceptible to high error values upon vibration and reduces resolution of the accelerometer by a full 180 degrees on pitch and roll as well as makes yaw pretty much useless(though yes it is useless and erratic around horizontal). I switched your last three lines of code to:
Code: [Select]
`//Translate the raw accelerometer counts into corresponding degrees  accelAngleZ = atan2(ax_m, ay_m)*180/PI;  accelAngleY = atan2(ay_m, az_m)*180/PI;  accelAngleX = atan2(ax_m, az_m)*180/PI;}` Let me know if you see a reason I and others shouldnt

Go Up

Please enter a valid email to subscribe