next topic »

[Reply #165 on:]

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:

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

[Reply #166 on:]

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

[Reply #167 on:]

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

[Reply #168 on:]

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

[Reply #169 on:]

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())

[Reply #170 on:]

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);
}

[Reply #171 on:]

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

[Reply #172 on:]

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

[Reply #173 on:]

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

[Reply #174 on:]

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

[Reply #175 on:]

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

[Reply #176 on:]

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?

[Reply #177 on:]

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

[Reply #178 on:]

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 

a short pdf about this stuff:

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

[Reply #179 on:]

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