Pages: 1 ... 10 11 [12] 13 14 ... 37   Go Down
Author Topic: Guide to gyro and accelerometer with Arduino including Kalman filtering  (Read 304798 times)
0 Members and 4 Guests are viewing this topic.
Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

I really haven't got the time to play around with it smiley-sad But if you succeed in getting 360 degrees resolution, please post the code, and I will update it smiley
I am thinking about buying a magnetometer, and update the guide, so one can calculate yaw as well. But that might be in the future, as I have so many things going on right now! smiley-sad

I think you could minimize the lag, by doing something like this:
Code:
int STD_LOOP_TIME = 10;           
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

setup()
{
//some code
}
loop()
{
//Calculate the angles

lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME)         
    delay(STD_LOOP_TIME-lastLoopUsefulTime);
loopStartTime = millis();
}

Then it will only read the IMU every 10 ms - that might do the trick smiley
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 3
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hello,

i´ve got a question about the accelero angle calculation. i´m trying myself to calculate the accelero angles with a 3-axis accelerometer.

in my opinion your calcultion is wrong, because you use all three components fromthe accelero. because you want to calculate the angle on the y-axis for example you dont need data of the x-axis (it may cause a certain error).

Here:

R = sqrt(accXval2+accYval2+accZval2)

accYangle = acos(accYval/R)

So the accYangle contains information about the xAxis (accXval2) but you only need the vectorlength of the YZ-plane to compute the angle. or am i wrong ? i´m trying do get 360° angle calculaton, but i don't get results that arent jumping in some situations, where accZval2 is about zero .. :/ tried tan/cos/scalarcos ... grrml ...

oh and thank you Lauszus for the tutorial smiley

greets

d0nut
Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

The reason why I use all three, is to compensate for tiny errors. In theory the force vector should be 1, but if you debug the data, you will see that it's not. If you don't need high precision you could just do the following:
Code:
accXangle = asin(accXval)*RAD_TO_DEG;
accYangle = asin(accYval)*RAD_TO_DEG;


Also to calculate the force vector you should write instead - as in the original code:
Code:
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector

The reason why it jumps around, is because if the roll (x-axis) is more than 90 degrees, it will also affect the pitch (y-axis) - I think you might need a magnetometer for full 360 degrees resolution smiley
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi,

I finally got my accelerometer giving sensible angles but I am totally stuck on understanding the Gyro, I am using the following code with a IMU 5DOF Sparkfun SEN-09268.

int gyroPin = 4;               //Gyro is connected to analog pin 4
float gyroVoltage = 3.3;         //Gyro is running at 3.3V
float gyroZeroVoltage = 1.36;   //Gyro is zeroed at 1.36V
float gyroSensitivity = .002;  //Our example gyro is 2mV/deg/sec
float rotationThreshold = 5;   //Minimum deg/sec to keep track of - helps with gyro drifting

float currentAngle = 0;          //Keep track of our current angle

void setup() {
  Serial.begin (115200);
  analogReference(EXTERNAL);
}

void loop() {
  float gyroRate = (analogRead(gyroPin) * gyroVoltage) / 1023;

  //This line finds the voltage offset from sitting still
  gyroRate -= gyroZeroVoltage;

  //This line divides the voltage we found by the gyro's sensitivity
  gyroRate /= gyroSensitivity;

  //Ignore the gyro if our angular velocity does not meet our threshold
  if (gyroRate >= rotationThreshold || gyroRate <= -rotationThreshold) {
    //This line divides the value by 100 since we are running in a 10ms loop (1000ms/10ms)
   // gyroRate /= 100;
    currentAngle += gyroRate;
  }

  //Keep our angle between 0-359 degrees
  if (currentAngle < 0)
    currentAngle += 90;
  else if (currentAngle > 89)
    currentAngle -= 90;

  //DEBUG
  Serial.println(gyroRate);

  delay(10);
}

When the IMU is level the numbers are stable, when I tip it the numbers increase. When I tip the other way the numbers decrease. Trouble is these numbers dont seem to make any sense in terms of degrees. All I am after is a clean -90 to +90 degree sweep. Please can you help me to achieve this with my code

Kind Regards
Logged

Physics is an elephant, you just have to go fast enough.

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

You have made a couple of mistakes. Remember that the output is in deg/s, so you have to know the time since your last reading (delta time). Have a look at my code, and change the gyro reading to the following:

Code:
#define gX A4
int gyroXadc;
float gyroZeroX = 421.6;//1.36/3.3*1023 - you could also calculate this - have a look in my code

gyroXadc = analogRead(gX);
gyroXrate = (gyroXadc-gyroZeroX)/.62;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.002/3.3*1023=0.62
gyroXangle += gyroXrate*dtime/1000;//Without any filter - dtime is in millis, so we divide it by 1000, to get in into secounds (use 1000000 if using micros())



Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Thanks but I am still having problems. When I use your code the gyro constantly drifts toward the negative. I know they drift but this is the part I dont understand. All the complementry filter is doing it trusting the Gyro a lot and the accelerometer a little.

So as my gyro figures constantly drift. My final angle also will constantly drift. I must be doing something really obviously wrong.

Any takers?

Code below

Code:
#include <math.h>

// Accelerometer variables
int xAcc_ADC;
int zAcc_ADC;
int xOffset=506; 
int zOffset=610;
float xAngle;
float xAtan;
float vRef = 3.3;
float IMU_sensitivity = 0.329;
float finalangle;

// Gyro variables
float gyroZeroX = 421.6;                                                                  //1.36 volts /9which is Gyro zero / 3.3 which is AREF *1023 which is the ADC
int gyroXadc;                                                                             
float gyroXrate;
float gyroXangle;
unsigned long timer=0;
unsigned long dtime=0;

void setup()
{
  analogReference(EXTERNAL);
  Serial.begin(115200);
}

void loop()
{
  xAcc_ADC=analogRead(0);
  zAcc_ADC=analogRead(2);
  xAcc_ADC= xAcc_ADC - xOffset;
  zAcc_ADC= zAcc_ADC - zOffset;
 
  xAtan=atan2(xAcc_ADC*vRef/(1024*IMU_sensitivity),(zAcc_ADC*vRef/(1024*IMU_sensitivity)+1))*180/PI;    // Final X value from acelerometer

  gyroXadc = analogRead(4);
  gyroXrate = (gyroXadc-gyroZeroX)/.62;                                                                  //(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.002/3.3*1023=0.62
  gyroXangle += gyroXrate*dtime/1000;                                                                    //Without any filter - dtime is in millis, so we divide it by 1000, to get in into secounds (use 1000000 if using micros())

  finalangle = (float)(0.98 * gyroXangle) + (0.02 * xAtan); 

  Serial.print("Acc X: ");
  Serial.print(xAtan);
  Serial.print(" Gyro X: ");
  Serial.print(gyroXangle);
  Serial.print(" Filtered Angle ");
  Serial.print(finalangle);

  dtime = millis()-timer;
  timer = millis();
 
  Serial.print(" Millis:  ");
  Serial.println(dtime);
}
Logged

Physics is an elephant, you just have to go fast enough.

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

No that is normal. The gyro will always drift! The complementary filter or Kalman filter will take care of the rest smiley
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Unfortunately with my code, its not! the drift accumulates very quickly, within seconds.
Logged

Physics is an elephant, you just have to go fast enough.

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Try add the following in setup():
Code:
timer=millis();//start timing
And this in loop():
Code:
finalangle = ((0.98 * gyroXangle) + (0.02 * xAtan)); 


Or try adapting my code, and see if there still is a problem smiley
« Last Edit: September 25, 2011, 10:40:43 am by Lauszus » Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I cant see any difference between your complementry filter and mine.. Still getting the same problem

Heres the serial output from when its started. You can see the gyro decreasing at a fast rate

Anyone got any idea please?

Acc X: 1.11 Gyro X: -0.03 Filtered Angle -0.01 Millis:  5
Acc X: 1.11 Gyro X: -0.07 Filtered Angle -0.04 Millis:  6
Acc X: 0.56 Gyro X: -0.11 Filtered Angle -0.10 Millis:  5
Acc X: 0.56 Gyro X: -0.15 Filtered Angle -0.13 Millis:  6
Acc X: 1.12 Gyro X: -0.19 Filtered Angle -0.17 Millis:  5
Acc X: 0.56 Gyro X: -0.23 Filtered Angle -0.21 Millis:  6
Acc X: 1.12 Gyro X: -0.27 Filtered Angle -0.25 Millis:  6
Acc X: 1.12 Gyro X: -0.32 Filtered Angle -0.29 Millis:  6
Acc X: 1.12 Gyro X: -0.36 Filtered Angle -0.33 Millis:  5
Acc X: 1.12 Gyro X: -0.40 Filtered Angle -0.37 Millis:  6
Acc X: 1.12 Gyro X: -0.45 Filtered Angle -0.41 Millis:  5
Acc X: 1.11 Gyro X: -0.48 Filtered Angle -0.45 Millis:  6
Acc X: 0.56 Gyro X: -0.53 Filtered Angle -0.51 Millis:  5
Acc X: 0.56 Gyro X: -0.56 Filtered Angle -0.54 Millis:  6
Acc X: 1.12 Gyro X: -0.61 Filtered Angle -0.57 Millis:  6
Acc X: 1.12 Gyro X: -0.65 Filtered Angle -0.62 Millis:  6
Acc X: 1.12 Gyro X: -0.70 Filtered Angle -0.66 Millis:  5
Acc X: 0.56 Gyro X: -0.73 Filtered Angle -0.71 Millis:  6
Acc X: 1.12 Gyro X: -0.78 Filtered Angle -0.74 Millis:  5
Acc X: 0.56 Gyro X: -0.82 Filtered Angle -0.79 Millis:  6
Acc X: 0.56 Gyro X: -0.86 Filtered Angle -0.83 Millis:  5
Acc X: 1.11 Gyro X: -0.90 Filtered Angle -0.86 Millis:  7
Acc X: 0.56 Gyro X: -0.95 Filtered Angle -0.92 Millis:  5
Acc X: 1.11 Gyro X: -0.99 Filtered Angle -0.94 Millis:  6
Acc X: 0.56 Gyro X: -1.03 Filtered Angle -1.00 Millis:  5
Acc X: 0.56 Gyro X: -1.07 Filtered Angle -1.04 Millis:  6
Acc X: 1.12 Gyro X: -1.11 Filtered Angle -1.07 Millis:  5
Acc X: 1.12 Gyro X: -1.15 Filtered Angle -1.10 Millis:  6
Logged

Physics is an elephant, you just have to go fast enough.

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Maybe your have to multiply the gyro angle with -1. That might be the problem...
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Thanks for the suggestion Lauszus but I don't see how multiplying the gyroangle with minus 1 is going to solve the ever increasing negative drift. eg if the drift is -10, -11, -12 etc all thats going to happen by multiplying by -1 is it will become 10,11,12 etc.

I really need to understand why my code is not working as you suggested.

To recap. The board is a sparkfun 5dof IMU (SEN-09268) and the Gyro is an IDC 500. I am powering it of 3.3v and the AREF is also on 3.3v.

All I am trying to get from the gyro is a stable angle in the range -90 to +90 degrees. I need to understand the logic of where my code is going wrong. Can someone please help?
Logged

Physics is an elephant, you just have to go fast enough.

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

The only suggestions, I can give is to try using my code instead, as it is confirmed to work! smiley
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 3
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I found some interesting functions for the calculation of angles between 0 and 360°. They work almost perfect. perhaps you can help me to improve them smiley-wink

alpha = atan( Ax /(sqrt( Ay^2 + Az^2 ) ) )
beta = atan( Ay  /(sqrt( AX^2 + Az^2 ) ) )

Ai = Accelero data

alpha = roll
beta = pitch

i´m doing stuff in scilab, so i cant give you c-code  smiley-twist

a short pdf about this stuff:

http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/APPLICATION_NOTE/CD00268887.pdf
Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

It's the same as the original one. It's only 180 degrees of resolution too :/
Logged

Pages: 1 ... 10 11 [12] 13 14 ... 37   Go Up
Jump to: