Guide to gyro and accelerometer with Arduino including Kalman filtering

I'm using 3G for my quad, this seems to be a good compromise between pitch/roll accuracy and sensitivity to noise. If you try 1.5G, you might find it too sensitive to vibrations.

I have used the mag, I have to calibrate it by working out the offsets. The best accuracy I've got has been a calibration with the sensor in place and all four motors running (without props on obviously). I get pretty good stable results with the MARG algorithm as part as the FreeIMU library, but I haven't recently tried tilt compensation calculations with my current offsets so can't really comment on how well this works.

Your gyro code looks ok to me.

cristo829:
So you changed this:
gyroX = analogRead(gX);
gyroXrate = (gyroX-gyroZeroX)/1.0323;//(gyroX-gryoZeroX)/Sensitivity Sensitivity = 0.00333/3.31023=1.0323
gyroXangle += gyroXrate
dtime/1000;

to this?

(read x value on i2c gyro)
gyroXrate = (gyroX-gyroZeroX)
gyroXangle += gyroXrate*dtime/1000;

Im building a qudcopter, how many Gs do you think is a good idea to use on the BMA? Also, have you programmed the Magnetometer on your IMU im having some trouble with the tilt compensation, my measurement varies within 10 degrees when i tilt it which is finw for me, except when i tilt it to one specific side when i tilt it that way it varies something like 40 degrees is this normal?
Thanks for your help

Thanks AntR ill try that

Hello everyone --

I'm really glad to find this thread so active. I'm currently working on a project that involved tracking the orientation and position of an object.

I've got an ADXL345 accelerometer, ITG-3200 gyro and an HMC5883L magnetometer.

I've gotten Fabio Varesano's awesome code working which is giving me great position information.

But I'm hoping someone on this thread can point me to some code that I can use to get a good measurement of velocity so I can calculate position.

Thanks in advance.

I would be possible using the accelerometers and then multiply it with one g and the delta time. But you have to do some math to compensate for rotation. Please post the code if you get it working :slight_smile:

Hey,

I'm using this LISY300AL Gyro: http://www.sparkfun.com/datasheets/Sensors/LISY300AL.pdf
But i cant get a precise data..

Here is my calculation:

    sensitivity = 0.716454545;
    gyroAdc = analogRead(0);
    if(abs(gyroAdc - gyroZero) <= Threshold)
      gyroAdc = gyroZero;
    gyroRate = (gyroAdc-gyroZero)/sensitivity;
    gyroRotation=gyroRotation+gyroRate*dtime1/1000;
    gyroAngle = abs((gyroRotation - 360 * ceil(gyroRotation / 360))); // Round up between 0-360
    dtime = millis() - stime;
    stime = millis();

I dont get a precise angle, i move my object 90 degrees but the gyro drifts too much so it showes something else...
Maybe i'm doing something wrong ? or i should just add an Accelerometer to make it more precise ?

First of all, how did you calculate your sensitivity?
If you use either 3.3 or 5 volt as reference, it's either:
0.0033/3.31023=1.023
or
0.0033/5
1023=0.67518
As the sensor has a sensitivity of 3.3 mV/ °/s
Or do you use a different reference? As i can calculate you use 4.711952801 volt as reference (0.0033/4.711952801*1023=0.716454545).

Why do you need this line:

gyroAngle = abs((gyroRotation - 360 * ceil(gyroRotation / 360))); // Round up between 0-360

What you call "gyroRotation", is actually the angle :slight_smile:

For more precision use integration.
Something like this would work:

#define OFFSET 0.0005

gyroRaw = analogRead(0);
gOffset = OFFSET * gyroRaw + (1-OFFSET) * gOffset;
gyroSpeed = (gyroRaw - gOffset)/sensitivity;

gyroAngle += gyroSpeed*dtime/1000;

See this page for more information: http://www.hitechnic.com/blog/gyro-sensor/htway/

Regards
Lauszus

Lauszus:
First of all, how did you calculate your sensitivity?
If you use either 3.3 or 5 volt as reference, it's either:
0.0033/3.31023=1.023
or
0.0033/5
1023=0.67518
As the sensor has a sensitivity of 3.3 mV/ °/s
Or do you use a different reference? As i can calculate you use 4.711952801 volt as reference (0.0033/4.711952801*1023=0.716454545).

Why do you need this line:

gyroAngle = abs((gyroRotation - 360 * ceil(gyroRotation / 360))); // Round up between 0-360

What you call "gyroRotation", is actually the angle :slight_smile:

For more precision use integration.
Something like this would work:

#define OFFSET 0.0005

gyroRaw = analogRead(0);
gOffset = OFFSET * gyroRaw + (1-OFFSET) * gOffset;
gyroSpeed = (gyroRaw - gOffset)/sensitivity;

gyroAngle += gyroSpeed*dtime/1000;




See this page for more information: http://www.hitechnic.com/blog/gyro-sensor/htway/

Regards
Lauszus

Hey, thanks for the replay.

I'm using 3.3v so i changed the sensitivity to 1.023, now the numbers are even less precise.
the gyroZero after calibration is around 325, and the gyro goes about -+300.

I removed the gyroAngle = abs((gyroRotation - 360 * ceil(gyroRotation / 360))); // Round up between 0-360

When i use this:

#define OFFSET 0.0005

gyroRaw = analogRead(0);
gOffset = OFFSET * gyroRaw + (1-OFFSET) * gOffset;
gyroSpeed = (gyroRaw - gOffset)/sensitivity;

gyroAngle += gyroSpeed*dtime/1000;

The numbers goes crazy and keep going up even when the gyro doesn't move.

Bottom line:
Sensitivity = 1.023 - When i move my object 90 Degrees, the gyro showes 50-55 Degrees.
Sensitivity = 0.716454545 - When i move my object 90 Degrees, the gyro showes 83-88 Degrees.

Those drifts just getting worse every move so after few movements the gyro isn't helpfull.

Any ideas ?

Have you remembered to connect 3.3 Volt to the VREF pin on the Arduino? :slight_smile:

Regards
Lauszus

Lauszus:
Have you remembered to connect 3.3 Volt to the VREF pin on the Arduino? :slight_smile:

Regards
Lauszus

I used this guide: GyroLISY300AL \ Learning \ Wiring
I connected the VCC on the gyro to the Arduino 3.3v pin.

Is that what you ment ?

No you have to connect 3.3V to the VREF/AREF pin on the Arduino.

Regards
Lauszus

Hey, thanks alot for you replays i learn alot of new stuff and you are being very helpfull :slight_smile:

So in my Gyro i have VCC & GND pin, i connected the VCC to the Arduino 3.3v pin and the GND to one of the GND pins on the Arduino.
And i connected the OUT pin of the gyro to the Analog0 pin on the Arduino.

I use this external Power Supply

As you can see i'm giving 12v to the Arduino, and the Fixed volts are set to 3.3v and connected stright to the AREF pin and one of the GND pins on the arduino.

Now i changed my sensitivity back to 1.023, and added analogReference(EXTERNAL); to the setup() in my code.

You can see the purple black and red goes to the gyro, the big connector that connected to the 7,8,9 digitals is a Servo controller, and the 2 pins from the external supply to the AREF & GND.

Here is the code, every one milliseconds i call this:

void gyroTest()
{
    if(gyroInit) {
      finalGyroAngle = calculateGyroAngle(0);
      Serial.println(finalGyroAngle);
    }
}
float calculateGyroAngle(int pin)
{
    gyroAdc = calibrateGyro(0);
    if(abs(gyroAdc - gyroZero) <= Threshold)
      gyroAdc = gyroZero;
    gyroRate = (gyroAdc-gyroZero)/sensitivity;
    gyroRotation+=gyroRate*dtime/1000;
    unsigned long t = millis();
    dtime = t - stime;
    stime = t;
    return gyroRotation;
}

void InitGyro(int pin)
{
  Serial.println("Calibrating Gyro...");
  sensitivity = 1.023; //0.692454545; //0.716454545;
  gyroAdc = 0;
  gyroRate = 0;
  gyroRotation = 0;
  gyroAngle = 0;
  gyroZero = calibrateGyro(pin);
  Serial.print("Gyro Calibrated: ");
  Serial.println(gyroZero);
}

int calibrateGyro(int pin)
{
  long resultGyro = 0;
  for(int i=0;i<100;i++)
  {
    resultGyro += analogRead(pin);
    delay(0.5);
  }
  resultGyro = resultGyro/100;
  return resultGyro;
}

Am i doing everything ok ?

Glad to see you figured it out :slight_smile:
What are you going to use the setup for? Controlling the servo or something like that?

Regards
Lauszus

Hey, i still didn't solve it :stuck_out_tongue:

I'll explain why the setup for and what is my problem.
As you see i have a WiFi shield connected, i send him commands to spin the servo that is connected to a arm that the gyro is on so the gyro spins with it.

I want to tell the servo to spin 90 degrees, and then go back to 0 degrees, and do that in a loop.
That's why i need the Gyro for, to get the degrees.

My problem is that after 2-3 time it goes to 90 then 0 the drifting goes crazy and it goes to 110 then -20...
So my problam still isn't solved.

I posted all my setup so you can see if i'm doing everything right, i hope you can help me figure it out.

Sorry, I didn't see the last line in your post :slight_smile:

Hmm, it seams like you do everything correct.

But hey, why do you need a gyro to tell the degrees? You don't need that for a servo. Just send it the correct pulse, and it will go to the requested position!!

See this page for more information: Servo - Arduino Reference

Hey :slight_smile:

I have a continues Servo... and after doing all of this, my gyro is still like 95% accurate ...
i need 99% atleast..

I just think i'm gonna get an Absolut Angle sensor and connect it to the servo..

Thanks for the help anyway, you were very nice and helpfull :slight_smile:

Why not just use a ordinary Servo then?
Thank you for the kinds words :slight_smile:

Regards
Lauszus

Hello,

I am using an Arduino UNO and a Sparkfun 6 DOF Razor IMU. I am hoping to adapt it to control roll and pitch for a quadrotor stabilization system. I used your post as a guide to setup the wiring and used the version 3 code to get it going. My problem is I get about 2-3 lines of actual data and then it just scrolls jibberish. I double checked the wiring and I have not changed anything in the code. Could it be that I am using the IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU - SEN-10010 - SparkFun Electronics IMU with the high pass filters removed.

I would like to apologize in advance, I have a lot of RC experience and a mechanical engineering background but programming and debug has never been my forte.

All I want to do is read in the roll and pitch degrees and then convert that to a servo command to send to a brushless motor as an increase in thrust to compensate.

Thanks for any help you can provide.

Stefan

It is exactly the same IMU as mine. So it should be almost plug n' play. Triple check your wires, or try resetting the high pass filter, by pulling the HP pin high (see the datasheet page 7 http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf). Not sure if it would work, as the description says that it it removed, as you said.

You do not have to apologize :slight_smile: We have all been noobs once :smiley:

Regards
Lauszus

@StefanElsener
I also have the board with sensors removed and the program works great, if you say that you can read a few lines and then it just scrolls jibberish it might be the baud rate. 98% of the time that i see just jibberish on the Serial monitor is because the baud rate in it is not the same as the one on the program check that the baud rate on the script is the same as in the serial monitor (bottom right). If that doesnt work then do as Lauszus says check your wires, maybe download the program again and make sure not to erase anything important on it. If that doesnt work then im sorry to say that your board might be damaged (its painful i know :frowning: ...) good luck

Thank You Cristo! I had the baud rate set at 9600 baud (default setting I assume as this is the first time I have used the program) and when I changed it to 115,200 baud it worked!!

Thank you Lauszus for the quick reply. Now for my next task, creating the control function to provide stabilization for a quadcopter.... does anybody have a good starting point now that my IMU is working?

Thanks again!

Stefan