Go Down

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

#### Lauszus

#165
##### Sep 23, 2011, 02:40 pm
I really haven't got the time to play around with it But if you succeed in getting 360 degrees resolution, please post the code, and I will update it
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!

I think you could minimize the lag, by doing something like this:
Code: [Select]
`int STD_LOOP_TIME = 10;            int lastLoopUsefulTime = STD_LOOP_TIME;unsigned long loopStartTime = 0;setup(){//some code}loop(){//Calculate the angleslastLoopUsefulTime = 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

#### d0nut

#166
##### Sep 23, 2011, 03:38 pm
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

greets

d0nut

#### Lauszus

#167
##### Sep 23, 2011, 03:58 pm
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: [Select]
`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: [Select]
`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

#### mearsy

#168
##### Sep 25, 2011, 09:50 am
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

#### Lauszus

#169
##### Sep 25, 2011, 11:32 am
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: [Select]
`#define gX A4int gyroXadc;float gyroZeroX = 421.6;//1.36/3.3*1023 - you could also calculate this - have a look in my codegyroXadc = analogRead(gX);gyroXrate = (gyroXadc-gyroZeroX)/.62;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.002/3.3*1023=0.62gyroXangle += 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())`

#### mearsy

#170
##### Sep 25, 2011, 04:50 pm
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: [Select]
`#include <math.h>// Accelerometer variablesint 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 variablesfloat gyroZeroX = 421.6;                                                                  //1.36 volts /9which is Gyro zero / 3.3 which is AREF *1023 which is the ADCint 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);}`

#### Lauszus

#171
##### Sep 25, 2011, 04:54 pm
No that is normal. The gyro will always drift! The complementary filter or Kalman filter will take care of the rest

#### mearsy

#172
##### Sep 25, 2011, 05:31 pm
Unfortunately with my code, its not! the drift accumulates very quickly, within seconds.

#### Lauszus

#173
##### Sep 25, 2011, 05:33 pmLast Edit: Sep 25, 2011, 05:40 pm by Lauszus Reason: 1
Try add the following in setup():
Code: [Select]
`timer=millis();//start timing`
And this in loop():
Code: [Select]
`finalangle = ((0.98 * gyroXangle) + (0.02 * xAtan));  `

Or try adapting my code, and see if there still is a problem

#### mearsy

#174
##### Sep 25, 2011, 06:31 pm
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

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

#### Lauszus

#175
##### Sep 25, 2011, 06:45 pm
Maybe your have to multiply the gyro angle with -1. That might be the problem...

#### mearsy

#176
##### Sep 26, 2011, 09:00 am
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?

#### Lauszus

#177
##### Sep 26, 2011, 11:35 am
The only suggestions, I can give is to try using my code instead, as it is confirmed to work!

#### d0nut

#178
##### Sep 29, 2011, 10:18 am
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

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  ]