Go Down

Topic: Guide to gyro and accelerometer with Arduino including Kalman filtering (Read 325423 times) previous topic - next topic

Lauszus

Oh okay, thats right. Had to do the same thing once, but forgot that you also should adjust that as well.
You're welcome :)

Laurencet

Hi All
I had a collection of problems when I started using the code with stability. It had a habit of rounding numbers to zero then dividing  by zero.  Initially I got around this by adding delays, but this increased the rate at which the gyroscope drifted.
Even though these were smoothed out by the complementary filter it didn't seem like the right way to go, especially when the input were so stable..
Not sure if it was my Mega causing problems.
By changing the equations around I'm finally happy with the outputs from the gyro and accelerometer...

Just need to work out how to add a PID loop and servos now...

If anyone is interested I can also post the code for the accelerometer and Gyros before they were combined...

Code: [Select]



float aX = A1;
float aY = A2;
float aZ = A3;

float gX = A5;
float gY = A6;
float gZ = A7;


float accXadc;
float accYadc;
float accZadc;

float accXval;
float accYval;
float accZval;

float accXangle;
float accYangle;
float accZangle;

float accZeroX;
float accZeroY;
float accZeroZ;

float AccSen;
float Rad;
float R;

float gyroZeroX; 
float gyroXadc;
float gyroXrate;
float gyroXangle;

float gyroZeroY;//y-axis
float gyroYadc;
float gyroYrate;
float gyroYangle;

float gyroZeroZ;//z-axis
float gyroZadc;
float gyroZrate;
float gyroZangle;

int i;
  int Loop;
 
unsigned long timer;
unsigned long dtime;

float compAngleX;
float compAngleY;

void setup()
{
Serial.begin(115200); // initialize the serial communications
analogReference(EXTERNAL);

delay (500);

Rad = 57.295779513082320876798154814105;
AccSen = 102.3;
Loop=500;


timer = micros();
  for (i = 0; i < 500; i++){
 
accZeroX = accZeroX + (analogRead(A1));
accZeroY = accZeroY + (analogRead(A2));
accZeroZ = accZeroZ + (analogRead(A3));
gyroZeroX = gyroZeroX + (analogRead(A5));
gyroZeroY = gyroZeroY + (analogRead(A6));
gyroZeroZ = gyroZeroZ + (analogRead(A7))

;

}
accZeroX = accZeroX / 500;
accZeroY = accZeroY / 500;
accZeroZ = accZeroZ / 500;

gyroZeroX = gyroZeroX / 500;
gyroZeroY = gyroZeroY / 500;
gyroZeroZ = gyroZeroZ / 500;

Rad = 57.295779513082320876798154814105;
AccSen = 102.3;

}
void loop()

{
timer = micros(); 
for (i = 0; i < 100; i++){
 
accXadc = analogRead(aX);
accXval = accXval + (accXadc-accZeroX);//AccSen;

accYadc = analogRead(aY);
accYval = accYval + (accYadc-accZeroY);//AccSen;

accZadc = analogRead(aZ);
accZval = accZval + (accZadc-accZeroZ);//AccSen;

gyroXadc = gyroXadc + analogRead(gX);
gyroYadc = gyroYadc + analogRead(gY);
gyroZadc = gyroZadc + analogRead(gZ); 

}
accXval =  accXval/(100 * AccSen) ;
accYval =  accYval/(100 * AccSen);
accZval =  accZval/(100 * AccSen)+1;

accXangle = atan (accXval/(sqrt(pow(accYval,2)+pow(accZval,2)))) * Rad;
accYangle = atan (accYval/(sqrt(pow(accXval,2)+pow(accZval,2)))) * Rad;

gyroXadc = gyroXadc / 100;    // average
gyroYadc = gyroYadc / 100;    // average
gyroZadc = gyroZadc / 100;    // average

gyroXrate = (gyroXadc-gyroZeroX)/1.0323;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
gyroYrate = (gyroYadc-gyroZeroY)/1.0323;
gyroZrate = (gyroZadc-gyroZeroZ)/1.0323;

dtime = micros()-timer;
gyroXangle=gyroXangle+(gyroXrate*dtime/1000000 )*-1;//Without any filter
gyroYangle=gyroYangle+(gyroYrate*dtime/1000000 )*-1;
gyroZangle=gyroZangle+(gyroZrate*dtime/1000000 )*-1;

compAngleX = (0.98 *(compAngleX +(gyroXrate*dtime*-1/1000000 ))) + (0.02*accXangle);
compAngleY = (0.98 *(compAngleY +(gyroYrate*dtime*-1/1000000 ))) + (0.02*accYangle);

Serial.print(" Gyro X Angle:  ");
Serial.print (gyroXangle);
Serial.print("\t");

Serial.print(" accXangle:  ");
Serial.print (  accXangle);
Serial.print("\t");

Serial.print(" Gyro Y Angle:  ");
Serial.print (gyroYangle);
Serial.print("\t");

Serial.print(" accYangle:  ");
Serial.print (  accYangle);
Serial.print("\t");

  Serial.print("compAngleX:  ");
  Serial.print (compAngleX);
  Serial.print("\t");

  Serial.print("compAngleY:  ");
  Serial.print (compAngleY);
  Serial.print("\t");

  Serial.println();
gyroXadc = 0;
gyroYadc = 0;
gyroZadc = 0; 
;


}
[/size]




Kashif

Hi Lauszus,

I stumbled upon your article at "Instructables" and then followed the link on Arduino forum.
I have read the whole 8 pages long thread and am pretty impressed by your work.

I wanted to buy the same IMU to give it a try but Sparkfun's website calls it "Retired". So, I am thinking of buying another 6DOF digital IMU http://www.sgbotic.com/index.php?dispatch=products.view&product_id=787 and want to couple it with HMC5843 to get roll, pitch and yaw all.

What changes your code may requie in order to talk to the above IMU?

This IMU uses "ITG3200" gyro and "ADXL345" accel.

I am from Computer Science background and have just started playing with Arduino and electronics. Co-incidently, google is my only friend and some times it takes ages to figure out a tiny thing. .... well, that's part & parcel of the game though, just wanted to give you a little bacground of mine, in case my questions sound silly to you ;-)

I will greatly appreciate your reply.

Lauszus

I really have no experience  with I2C IMU's, but it would not be that hard to implement it, as you only have to change the code, that reads the g's (accelerometer) and rad/s (gyro), and then just use the complimentary filter or the kalman filter as I provided.

I presume that you know the I2C protocol? If not, have a look at the wire library: http://www.arduino.cc/en/Reference/Wire
See the datasheet for the accelerometer page 14: http://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf and the gyros datasheet page 22: http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
Also check out the github for example code: https://github.com/a1ronzo/6DOF-Digital .
- Lauszus

Laurencet

It's scary how fast there developing new Gyros / Accelerometers... I only order mine a couple of weeks ago and that had just been updated.  Depending on your knowledge of I2C, I would stick with a analogue IMU.
Based on how little i know about  I2C

Can you get hold of
IMU Analog Combo Board - 5 Degrees of Freedom IDG500/ADXL335
http://www.sparkfun.com/products/9268

If you do go for the above board the gyroscope / accelerometer the sensitivity values will need changing..
You should be fine without the a gyroscope on the Yaw as it's not needed to calculate pitch or roll angles.. All partially depends what your using it for though. 

Laurence

Kashif


I really have no experience  with I2C IMU's, but it would not be that hard to implement it, as you only have to change the code, that reads the g's (accelerometer) and rad/s (gyro), and then just use the complimentary filter or the kalman filter as I provided.

I presume that you know the I2C protocol? If not, have a look at the wire library: http://www.arduino.cc/en/Reference/Wire
See the datasheet for the accelerometer page 14: http://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf and the gyros datasheet page 22: http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
Also check out the github for example code: https://github.com/a1ronzo/6DOF-Digital .
- Lauszus


Thanx a lot Lauszus for such a prompt and descriptive reply. Yup, I've some working knowledge of I2C as my HMC5843 is working fine.
Ok, I'll give it a try. But I have to find a code sample which works with Arduino, the Github code seems to be for standard AVR programming.
If anyone already have a resource, please forward me the link.

The interfacing looks simple, 3.3V, GND of IMU to respective pins of Arduino and SDA, SCL to A4, A5 of Arduino.
Please correct me, if I am wrong.


It's scary how fast there developing new Gyros / Accelerometers... I only order mine a couple of weeks ago and that had just been updated.  Depending on your knowledge of I2C, I would stick with a analogue IMU.
Based on how little i know about  I2C

Can you get hold of
IMU Analog Combo Board - 5 Degrees of Freedom IDG500/ADXL335
http://www.sparkfun.com/products/9268

If you do go for the above board the gyroscope / accelerometer the sensitivity values will need changing..
You should be fine without the a gyroscope on the Yaw as it's not needed to calculate pitch or roll angles.. All partially depends what your using it for though. 

Laurence


Thanx to you too Laurence, may I just ask why you prefer Analogue IMU over digital? Based upon my research on internet, digital is better than analogue. Not to offend you, just asking to clarify my concepts.

Lauszus

Okay good, then it should not be that hard to get it working.
Have you seen this post: http://www.varesano.net/blog/fabio/my-first-6-dof-imu-sensors-fusion-implementation-adxl345-itg3200-arduino-and-processing ?
Especially take a look at the bottom, as he provided his code.

Yes, that is correct. The INT1 and INT2 input, is interrupt pins for the ADXL345. For more info, see this comment: http://www.sparkfun.com/products/10121#comment_20331

I do not know if digital is more accurate in general, but in this case it is, because the Arduino has a 10-bit analog-to-digital converter (value 0-1023), but the build in digital-to-analog converter, in both the gyro and the accelerometer, on the digital IMU has a 16-bit digital-to-analog converter (value 0-65535).

Laurencet

I've just never played with I2c and the code for analogue imu was readily available and looked easier to understand.
Let me know how you get on with digital. It would be great if you could post your code..
I might have to swap over at some point...... 



Kashif

Thanx Lauszus, I have already started following Fabio's blog, it's a great source of information.
I must appreciate the way you reply and provide on-the-dot links. I am feeling truly grateful.

Laurence, I will probably receive the IMU by coming Tuesday, will give it a try and surely post my findings.
With you people around, I feel so encouraged.

Thanx once again to both of you.

Lauszus

Thanks for your feedback :) Really glad I could help!
Looking forward to see your work :D

Kashif

What a blessed day today, I got my  6DOF Digital IMU earlier this morning (3 days ahead of time) and just a couple of hours' effort and it's up and running.

I have used Fabio's blog (the one mentioned above by Lauszus)  http://www.varesano.net/blog/fabio/my-first-6-dof-imu-sensors-fusion-implementation-adxl345-itg3200-arduino-and-processing

All the Arduino & Processing code is available on the link above. It took me a couple of hours to establish the whole link as it was my first ever interaction with "Processing", Otherwise it was just a matter of a few minutes. Fabio's code is quite nicely written and commented wherever necessary. The only change I had to make in the code was to change Gyro Address  (from 0x69 to 0x68), that too mentioned by Fabio.

The interfacing of the sensor with Arduino is too simple (3.3V, GND, SDA and SCL) of sensor to (3.3V, GND, A4 and A5 respectively) of Arduino.

Since all is hooked up now and running like a charm, it's time to give another "good reading" to Lauszus's article ;-)
Lauszus, I am just loving the way you write.

I don't know about you guys but since I am new to Arduino/electroics, my way of accomplishing a task is usually as follows.
1. Interfacing - Get the electronics (sensors, wires, resistors, capacitors etc) and hook them all up
2. Interaction - Get the code, algorithm (wherever applicable) and read the data
3. Understanding - Since by now I have had a working model of what I wanted to experiment on. At this point I start playing with the "toy" and do a lot of R&D to understand the concept and know and insight of the blackbox. And that's done by Google readings and Forum postings.

OK guys, billions of thanx to all of you. it's like a big milestone hit for me. Thanx to Google but people like you are cherry on the cake (make it '"cherries" instead ;-)

Lauszus

Nice to hear you got it working :)
Haha why exactly do you "love" the way I write? Be more specific, so I can do more of that in the future.
Normally I actually read a lot about the subject, before even hooking it up, so I got a understanding of what I am doing before I hook it up.

Go Up