PDA

View Full Version : [Question(s)] L3G4200D Gyro Integration on Arduino



ROBOTMAN
04-14-2012, 02:25 AM
I have been banging my head against the wall with this for days and I know I am missing something obvious but I can't quite wrap my head around what I am doing wrong. I am trying to use the L3G4200D (http://www.robotshop.com/pololu-3-axis-gyro-breakout-board-l3g4200d-2.html) gyro with an arduino and integrate the z-axis value using trapezoidal approximation. The code I started with came from here (http://bildr.org/2011/06/l3g4200d-arduino/)"I know it is a different break out board but the same pins are broken out".

I am running in 200 degrees per second mode. To integrate I convert my reading to milli-degrees by multiplying the voltage reading by the 200 DPS scale "8.75" then divide that by 100 to convert to milli-degrees per second "the loops cycles every 10 milliseconds".

degreesPerSecond = (((float)z * 8.75))/100;

Then I check if the gyro is actually moving and if so integrate my currentAngle using code based off this (http://scuola.arduino.cc/en/content/integrating-gyro-obtain-angular-position) tutorial:

currentAngle += ((p_z + degreesPerSecond) * 10)/2000;
p_z is the previous reading for the z axis in degrees per second.

However this dose not work. When I print currentAngle/1000 "to convert from milli-degrees" it is < 1 degree when I move the gyro 90 degrees.

I know I am missing something obvious most likely because I have yet to take calculus :) .
Any help would be appreciated!

The code is attached:
3956

Th232
04-14-2012, 02:41 AM
I'm not sure what troubleshooting steps you've taken, so apologies if this is stuff you've already done.

I only had a brief look at your code (not having any Arduino development tools, I opened your code using Notepad...), and I've got a sneaking suspicion that you might have something wrong with your scaling factors. Personally I just convert everything to radians immediately, but I digress. I'm curious as to what exactly you're changing currentAngle by when you turn your gyro 90 deg. If that "<1 degree" when you move the gyro 90 degrees turns out to be "0.9 degrees", then there's a clue that it's a scaling issue.

Edit: Maybe I'm reading this bit wrong:

I am running in 200 degrees per second mode. To integrate I convert my reading to milli-degrees by multiplying the voltage reading by the 200 DPS scale "8.75" then divide that by 100 to convert to milli-degrees per second "the loops cycles every 10 milliseconds".

You have an initial input that's multiplied by 8.75. What exactly are the units for input*8.75? You say you convert milli-degrees to milli-degrees per second by dividing by 100, but that doesn't make sense. It's like me saying I've converted metres to metres per second by dividing by 100.

Edit 2: On a hunch, multiply by 100 instead of dividing, and tell me what happens.

tician
04-14-2012, 03:55 AM
If I actually understand any of this in my rather sleep deprived state, the L3F4200D gives you a signed 16-bit integer without any units. In 250 dps mode, you multiply by 8.5 [mdps/digit] to get the rotational rate in [millidegrees/second]. To then get [degrees/second], you will need to divide by 1000 [mdeg/deg], not 100.

z [digits] * 8.5 [mdeg/s/digit] * (1/1000) [deg/mdeg] => [deg/s]

The integration step seems fine although the math might be a little bit unusual. Since p_z and degreesPerSecond are already float's, why not just multiply by 0.005 instead of multiplying by 10 and dividing by 2000? Then again, the compiler probably automatically optimizes that since 10 and 2000 are not going to be changing.

It would appear to be the Serial.println(currentAngle/1000) that is causing the confusion. 'currentAngle' is already in [deg], if you want to to get [mdeg], you multiply by 1000 [mdeg/deg], not divide. As is, your code is giving you [kdeg], which will be pretty low unless you are spinning the board really quickly. It is a matter of scale. The sensor measures in [mdeg/s], 1 [digit] * 8.5 [mdeg/s/digit] * (1/1000) [deg/mdeg] * (1/1000) [kdeg/deg] = 0.0000085 [kdeg/s] (which becomes [kdeg] after the integration step).

ROBOTMAN
04-14-2012, 11:36 PM
Thanks a ton for the help! Yeah you where correct and I am now getting much more reasonable readings. However things seem to be a little bit off because a manual rotation of 90 reads 70, I am wondering if this is because I am not factoring in the sample rate anywhere? Also there seems to be a high level of data loss. When rotating from 0 to 90 then back to zero currentAngle reads 11... This is not due to drift because I take 10,000 samples and use the peak values from that to make sure the gyro is actually moving before integrating.

My experience with gyros comes from FTC where we use the nxt's hi-tech gyro and basic trapezoidal approximation. With the NXT gyro we have no drift or data loss. We can rotate it back and forth from 90 degress all day and only lose 1 degree.

I expected to be able to achieve the same thing with this gyro but so far something seems to be different. I wrote most of the code for integrating the gyro on the NXT and there is noting special or different about it that I can tell.

Also I am wondering if the fact that I am running at 3.3 volts could make a difference? Those scales where for 3 volts but because of the wide range of voltages the breakout board can accept I am sure there is a regulator.

Th232
04-14-2012, 11:42 PM
How fast are you moving the gyro? Might be a saturation issue if you're moving it really fast.

When going 0->90->0, is it always reading 11 degrees, and if not how much does it vary by?

ROBOTMAN
04-14-2012, 11:58 PM
I think my test rig was the source of the data loss. It is fixed now. I switched to 3v but it still claims 90 is 70... I could manually configure the sensitivity but I should not have to right?

Th232
04-15-2012, 12:13 AM
I personally calibrate all my sensors anyway, but being off by over 20% doesn't sound right to me. If you rotate it by 360 degrees, will it say you're at 280 degrees, or something else?

Or maybe it's the part of your code where you decide whether the gyro's actually moving or not. Try removing that bit and see what happens.

ROBOTMAN
04-15-2012, 12:18 AM
Without the calibration code it still goes to 70 and 360 resulted in exactly 278.5 :) So I am guessing that it just needs manual calibration? 20% off sounds pretty ridiculous but I suppose it is what it is. Any tips for getting accurate manual calibration?

Thanks again for the help everyone!

Th232
04-15-2012, 12:47 AM
Bummer. If you're doing it manually, just put a multiplier in as early as you can, e.g. along with the 8.75 multiplier. So in your case multiply your result by 1.293 (360/278.5 = 1.293). Don't forget to comment it, I've calibrated before then come back and wondered why the heck I put such a strange number in there...:confused: Then do multiple tests using varying angles (e.g. 30, 60, 180, 270 deg) and at varying speeds to confirm that it's right.

tician
04-15-2012, 01:08 AM
How sure are you that the loop is actually taking 10 ms? That delay function is not taking into account the length of time required to actually run through the loop (to retrieve the gyro values, perform the data processing, and finally print out the value). A big problem I had with first attempting to use the HaViMo2 for tracking a target was that the time taken to print out just the basic x,y coordinates of the target drastically reduced the ability of the pan-tilt servo arrangement to actually track a moving target (the lower the baud rate, the longer the delay while printing).

It would be a much better idea to use the pastMillis variable. Record pastMillis at the beginning of the loop function and add a while loop at the end of the loop function that keeps repeating until the change in time is greater than or equal to 10 milliseconds.



unsigned long pastMillis = 0;
void loop()
{
pastMillis = millis();

getGyroValues(); // This will update x, y, and z with new values

degreesPerSecond = (((float)z * 8.75))/100;

if(z >= gyroHigh || z <= gyroLow)
{
currentAngle += ((p_z + degreesPerSecond) * 10)/2000;
}

Serial.println(currentAngle);

p_z = degreesPerSecond;

while ( (millis()-pastMillis)<10)
{
delayMicroseconds(200);
}
}

ROBOTMAN
04-15-2012, 01:37 AM
Ah you are correct!


while((millis() - pastMillis)<9);

Insted of dealy(10); results in about 93 degrees for every 90. I am guessing the easiest way to eliminate inconsistent sample rate as a factor would be to use millis() in the math itself but "excuse my stupidity here :)" in the formula where are the "kilo-degrees per sample time" coveted to kilo-degrees per second?

tician
04-15-2012, 03:08 AM
Not sure if the even more severe sleep deprivation is making me miss sarcasm or not.



current_rotational_rate [degrees/s] = sensor_value [digits] * 8.25 [millidegrees/s/digit] * (1/1000) [degree/millidegree]

// Raw Integration of rotational rate to produce current angle
// omega = dtheta/dt -> omega*dt = dtheta -> theta1 - theta0 = omega*(t1-t0) -> theta1 = theta0 + omega*(t1-t0)
current_angle [deg] = last_angle [deg] + { (current_rotational_rate) [deg/s] * (time_current - time_last) [s] }
// The Averaged/Smoothed Integration imlemented in your code
current_angle [deg] = last_angle [deg] + { ((last_rotational_rate+current_rotational_rate)/2) [deg/s] * (time_current - time_last) [s] }

so this should work


unsigned long pastMicros = 0;
unsigned long currMicros = 0;
float dt = 0.0;

void loop()
{
pastMillis = millis();
getGyroValues(); // This will update x, y, and z with new values
pastMicros = currMicros;
currMicros = micros();

// if we are considering a certain range of z to be noise, should we even bother doing any calculations or updating any values when z is considered noise? Seriously asking because of aforementioned sleep deprivation.
if(z >= gyroHigh || z <= gyroLow)
{
// determine time interval between sensor readings with a bit more accuracy than available just using millis()
// of course, micros() only updates in intervals of 4 [us], but still far better resolution/sig-figs than millis()
if (currMicros>pastMicros) //micros() overflows/resets every ~70 minutes
dt = (float) (currMicros-pastMicros)/1000000.0;
else
dt = (float) ((4294967295-pastMicros)+currMicros)/1000000.0;

degreesPerSecond = (((float)z * 8.75))/100;

// here we do an averaged/smoothed integration of rotational rate to produce current angle.
currentAngle += ((p_z + degreesPerSecond)/2) * dt;

// Should p_z be updated if z is insufficient to change the current angle? Again, seriously asking because I trust my mind even less than usual.
p_z = degreesPerSecond;
}

Serial.println(currentAngle);

// wait ~10 milliseconds
while ( (millis()-pastMillis)<10 ) // millis() overflows once every ~50 days, so less inclined to worry
{
delayMicroseconds(50);
}
}

ROBOTMAN
04-17-2012, 11:22 PM
Lol it was not sarcasm :) I have been devoting every hour to getting our FTC bot up and running and I was pretty burned out :) Totally missed a basic concept, but everything is working now. There was a bit of left over error but I tuned it out manually.

Because I got a few pm's about the final code here it is for refrence.

3960

Thanks again!

tician
04-18-2012, 12:33 AM
Went back to a fixed 10 [ms] delay? If so, you could remove the "pastMillis = millis();" line in loop(), since it is just wasting cycles with an invariant delay.

puller
08-06-2012, 04:21 AM
I've had a look at your code, thanks for sharing it.

However, I have a question.

I read values from my L3G4200D using the SPI bus.
Angular rate values when the sensor is in steady state are not 0. Typically I have a value around 100 when the sensor is steady around the z axys. Let's consider this as an example.

I use a calibration method similar to yours, detecting a max and min threshold inside which I consider samples to be noise around the steady state condition.

Now, if the z-sample is 130, I want to integrate it, however, when calculating the DegreesPerSecond I think I should consider only the delta from the steady state value (100) to the sampled valid value (130), that is 30.

Basically I've had expected to see a line like this in your code:


z -= steadyStateValue

Resulting in something like this:


if(z >= gyroHigh || z <= gyroLow)
{
z -= steadyStateValue
degreesPerSecond = (float)z * 0.00000908; //this value was 0.0000085 but was manually calibrated to its current value
if (currMicros>pastMicros) //micros() overflows/resets every ~70 minutes
dt = (float) (currMicros-pastMicros)/1000000.0;
else
dt = (float) ((4294967295-pastMicros)+currMicros)/1000000.0;
currentAngle += ((p_z + degreesPerSecond)/2) * dt;
p_z = degreesPerSecond;
}
else
{
p_z = 0;
}

It sounds starnge to me that this has not been pointed out during the discussion.

Am I missing something?

Thanks.

tician
08-06-2012, 10:42 AM
I am pretty sure there was the assumption that the sensors was assembled and functioning well and that the steady state value was essentially zero with noise varying between gyroHigh and gyroLow. I do not recall the steady state value from the gyro in any of the CM-730's in the lab being that far from a zero value, but I may be mistaken. Is the sensor on a custom board you assembled or is it a breakout board from a manufacturer/re-seller? The L3G4200D datasheet does mention improper soldering and/or mechanical stress introducing error into the zero/steady-state value*. That said, if you do not have any trouble with the sensor other than the steady state value, then just compensate for it. If there are other problems, like improper scaling or non-responsive axes, then you might want to contact the manufacturer/seller of the board.

* Under the heading 2.6.2 "Zero-rate Level":
Zero-rate level describes the actual output signal if there is no angular rate present. The
zero-rate level of precise MEMS sensors is, to some extent, a result of stress to the sensor
and, therefore, the zero-rate level can slightly change after mounting the sensor onto a
printed circuit board or after exposing it to extensive mechanical stress. This value changes
very little over temperature and time.

jwatte
08-07-2012, 03:16 AM
You don't want to use millis() at all if you want accurate timing. You want the microsecond function, whose name escapes me (micros()?)
Also, to get consistent sample rates, you probably want to set a timer to fire for each time you should sample, and sample from the interrupt handler.

tician
08-07-2012, 08:08 AM
You don't want to use millis() at all if you want accurate timing. You want the microsecond function, whose name escapes me (micros()?)
Also, to get consistent sample rates, you probably want to set a timer to fire for each time you should sample, and sample from the interrupt handler.
The code posted above uses only micros() for math and uses millis() with a 50 [us] delay interval for a roughly consistent sample rate, although the final code ROBOTMAN uploaded dumped that for a simple fixed delay irrespective of the time taken to process the loop. Ideally one would just use a timer interrupt for sampling sensors, but I'm pretty sure the arduino was dedicated purely to sampling the gyro, calculating the current angle, and pushing that value out over the UART to the PC, so I think that plus frustration/deadline led to a situation of "good enough."

puller
08-07-2012, 09:51 AM
tician, thank you for your reply.

My L3G4200D is mounted on a Sparkfun breakout.
Here below you can find actual samples of my sensor for 1 second interval:


X:95 Y:-10 Z:6
X:91 Y:-17 Z:9
X:96 Y:-10 Z:4
X:94 Y:-7 Z:6
X:93 Y:-16 Z:4
X:97 Y:-15 Z:2
X:101 Y:-16 Z:3
X:96 Y:-8 Z:2
X:92 Y:-10 Z:5

However, no problem to compensate that, maybe ROBOTMAN was luckier with the zero-rate level being closer to 0 :wink:.

Thank you for the datasheet quote.

Jigar4Electronics
03-22-2013, 09:30 AM
I am also using the same gyro L3G4200D.
I am trying to get angle from it...
I've used ADXL 335 accelerometer.

I am trying to get angles with respect to roll, yaw & pitch

Here is my code:


float Rx,Ry,Rz,Yd,Xd,Zd,Rxe,Rye,Rze,Xavg,Yavg,Zavg,T,Rxg ,Ryg,Rzg,c,Re,R,Rx1,Ry1,Rz1;
Th = 5;
c = 180/3.14;
T = 0.0155; //15.5 ms (verified by using counter)
while(1)
{
get_accl_data(); // Accl data from ADC
accelerometer(); // Converting ADC values in "g" - Rx, Ry, Rz
inclination(); // Normalizing Rx, Ry, Rz
gyro(); // Computing Xavg, Yavg, Zavg by using two consecutive readings

if((Xavg >= Th)||(Xavg <= -Th))
{
Xd = atan2(Rye, Rze);
Xd =(Xd + (Xavg*T));
Xd *= c;
ftoal(Xd,str);
UARTprintf("Xd : %s \n\r",str);
Xd /= c;
}
if((Yavg >= Th)||(Yavg <= -Th))
{
Yd = atan2(Rxe, Rze);
Yd =(Yd + (Yavg*T));
Yd *= c;
ftoal(Yd,str1);
UARTprintf(" Yd : %s \n\r",str1);
Yd /= c;
}
if((Zavg >= Th)||(Zavg <= -Th))
{
Zd = atan2(Rye, Rxe);
Zd =Zd + (Zavg*T);
Zd *= c;
ftoal(Zd,str2);
UARTprintf(" Zd : %s\n\r",str2);
Zd /= c;
}

Rxg = sin(Yd) / (sqrt(1 + ( (cos(Yd)*cos(Yd) )*(tan(Xd) * tan(Xd) ))));
Ryg = sin(Xd) / (sqrt(1 + ( (cos(Xd) * cos(Xd) ) *( tan(Yd) * tan(Yd) ))));
Rzg = ((Rzg > 0) ? 1 : ((Rzg < 0) ? -1 : 1))*(sqrt(1 - (Rxg*Rxg)-(Ryg*Ryg)));

Rxe = (Rx*0.02)+(Rxg*0.98);
Rye = (Ry*0.02)+(Ryg*0.98);
Rze = (Rz*0.02)+(Rzg*0.98);

Re = sqrt((Rxe*Rxe) + (Rye*Rye) + (Rze*Rze) );

Rxe = Rxe/Re;
Rye = Rye/Re;
Rze = Rze/Re;

SysCtlDelay(800000); // 10 ms delay
}


I am not getting the accurate angles...
I am getting some angles proportional to acceleration at a time when I move it.
And after moving it from angle 0 to angle 45 (for example) it stabilizes after a 1-2 sec. and giving stable values of Xd, Yd, Zd again!!

What I am missing??

Please help me