I don’t think you need two gyros or non-kit gyros. We have seen similar problems with the WPI gyro code, which is why we do our own bias calculation and integration - see my above post. If you need more details, let me know.
Hmm, that’s interesting.
Yeah, I’d love to know the details of how you guys calculate that.
This is going to be a long post. I’m trying to target this post at relative newbies, so if any of this is old news feel free to skip to the good parts.
(Since this got long, I’ll break it into two posts: the first will be explanation (the "why), and the second will be the “how” part.)
The vast majority of gyro “drift” is usually due to a poor voltage bias calculation.
What is this “bias” that you speak of? The sensor outputs a voltage relative to the angular rate (these devices are angular rate (speed) sensors, not heading sensors). The voltage when still (angular rate = 0) is typically around 2.5V for a 5V sensor, but in reality that can vary for many reasons. Therefore, the software always needs to calculate the voltage bias before using the angular rate to compute a heading. The bias is used by subtracting it from each sample before computing the heading. Thus, after the “bias subtraction”, the sensor output is transformed from 0 - 5V to approximately -2.5 - +2.5V.
How is the bias typically calculated? The bias is calculated by averaging a number of samples when the software thinks the gyro is still. The WPI gyro software performs this calculation as soon as the software begins to run. Any movement of the robot just after power-on until the bias calculation is finished will cause your bias to be bad, and you will get a large gyro drift. “When is the WPI code finished calculating it’s bias so we know it’s safe to move the robot?” you ask. Good question. I don’t really know, which is one of the reasons we don’t use it. Another reason we don’t use it is because we find it nearly impossible to keep the robot perfectly still immediately after power on.
Can the bias of the gyro drift? The short answer is yes. If you calculate a good bias, and then the bias changes, your heading will drift over time. That’s not good.
What causes the bias to drift I don’t want to repeat a good post: see Jared Russell’s post above. One important I’ll repeat: the bias is likely to drift as the gyro warms up. Therefore, in an ideal world you would like gyro to be at its steady-state before calculating the bias. The WPI code does this immediately after power-on so the likelihood of bias drift is much higher than if you could wait.
Ok, then in your opinion when should the bias be calculated There are 3 main criteria for me: 1) The robot MUST be PERFECTLY STILL; 2) The gyro should be at steady state; and 3) The bias calculation should be performed as close as possible to the time your are going to start using it (in other words, a bias calculation from 5 minutes ago is much less likely to be accurate than a bias calculated 1 second ago).
So when do YOU calculate the bias? Simple answer: the instant before the match begins. This ensures it satisfies all three criteria: 1) the robot will be perfectly still since all humans have to be off the field or someone is getting DQ’d; 2) The gyro should be at steady state since it has been a few minutes since power-on; and 3) the instant before the match starts is as close as you can get to when you start using the bias.
What are the disadvantages of your method? The WPI gyro code can’t be used, and you have to write all of your own software.
So, how do you actually do it?
First, we throw out the WPI gyro library and just use the analog input library. You’ll need to know the sensitivity of your gyro (in deg/s/V), but if you don’t know that you can make a guess and figure it out with some testing. We always do the tests anyway to characterize the sensor we’re using.
You need a few things in your code:
-
An array of gyro voltage readings so you can calculate a moving average. This moving average will be stopped once the match begins and this average will become your gyro bias.
-
A variable to indicate that your robot has become enabled once during this power cycle. Why is this important? Because you don’t want to resume calculating the bias in that short disabled period between auto and teleop. You may say, “but you said a bias calculated as close to the the current time possible is better, so wouldn’t it be good to calculate it again just before teleop?” The answer is no, because then you have a good chance of violating the most important criteria: the robot MUST be still. There is a decent chance that either your robot was still in motion as power was cut to end autonomous, or some other robot runs into you at the end of autonomous. Either way, the safe bet is to just use the bias from just before auto mode starts.
-
An integral to calculate the heading from the angular rate samples. (If you don’t know what that is, it’s a fancy term for area under a curve, which is a big sum. Look it up on wikipedia.)
So here it goes with the code:
(note: caveats apply. a) I’m doing this by the seat of my pants, so there may be a bug or two; b) this is more intended to be pseudo-code rather than copy-and-paste C code; c) this is intended to provide a general idea - not a final solution - see (a) and (b))
(note 2: I’m going to assume you have a typedef.h file and use descriptive types. f32 = 32-bit float, u8 = unsigned 8-bit integer, s16 = signed 16-bit integer, etc.)
defines and global variables
#define GYRO_BIAS_SIZE 32
#define GYRO_ANALOG_CHAN 1
#define GYRO_SENSITIVITY 0.025 /* in deg/s/V */
f64 gyroBiasArray[GYRO_BIAS_SIZE];
f64 gyroBiasSum;
u8 gyroBiasIdx;
bool visitedEnabled;
f64 gyroHeading;
f64 currentTime;
f64 previousTime;
In your “init” function
gyroBiasSum = 0;
gyroBiasIdx = 0;
visitedEnabled = FALSE;
gyroHeading = 0;
for (u8 i = 0; i < GYRO_BIAS_SIZE; i++)
{
gyroBiasArray* = 0;
}
In your fastest periodic loop
f64 gyroV;
f64 gyroDPS; /* in deg/sec */
static f64 gyroDPS_prev = 0; /* deg/sec previous sample - used for trapezoidal integration */
if ((getFMSMode() == AUTO) || (getFMSMode() == TELEOP))
{
visitedEnabled = TRUE;
}
previousTime = currentTime;
currentTime = getTickCnt_ms() / 1000;
/* sample the gyro voltage */
gyroV = getAnalogVoltage(GYRO_ANALOG_CHAN);
/* calculate the bias if we have not yet been enabled */
if (FALSE == visitedEnabled)
{
/* compute the moving average by first calculating a moving sum
the moving sum is computed by first subtracting the oldest
sample in the bias array, then add the current sample. Then
replace the oldest sample in the bias array with the newest sample. */
gyroBiasSum -= gyroBiasArray[gyroBiasIdx];
gyroBiasSum += gyroV;
gyroBiasArray[gyroBiasIdx] = gyroV;
gyroBiasIdx++;
if (gyroBiasIdx >= GYRO_BIAS_SIZE)
{
gyroBiasIdx = 0;
}
}
else
{
/* if we're here, it means we've been enabled. Therefore, we need to
be calculating the heading instead of the bias. */
/* first, subtract bias voltage and convert to degrees/sec */
gyroDPS = (gyroV - (gyroBiasSum / GYRO_BIAS_SIZE)) * GYRO_SENSITIVITY;
/* Now, compute heading using trapezoidal integration */
gyroHeading += ((gyroDPS + gyroDPS_prev)/2) * (currentTime - previousTime);
gyroDPS_prev = gyroDPS;
}
And there you have it.
I would highly recommend characterizing the sensor you’re using rather than just using the sensitivity from the data sheet. Not only can the sensitivity vary from part to part, but the sensitivity can be reduced if the gyro isn’t mounted perfectly flat. Here’s how we do it:
-
make your best guess at the gyro sensitivity (use the nominal sensitivity in the data sheet if you have it).
-
Place the robot flat up against a wall and start your code.
-
After sufficient time to allow a good bias calculation, enable your robot into teleop mode.
-
Rotate your robot 180 degrees and place the robot flat against the wall again (so you ensure your robot rotated as close to exactly 180 degrees as possible).
-
Write down the computed heading.
-
adjust your gyro sensitivity in your code using the following calculation: NewGyroSensitivity = OldGyroSensitivity * ActualRecordedHeading / 180
-
Repeat the procedure until your recorded heading is VERY close to 180 deg.
Last note: we actually use LabVIEW. I can post that as well if you don’t do textual programming.
If there are any questions, let me know.*
Wow, thanks for posting this!
I’ll share this it my team.
What exactly do you mean by the “init function”? Do you mean to put that in Begin.vi? (We use LabVIEW too)
How fast do you run that periodic loop?
And you just reference the gyro heading in tele/auto by using a global variable, right?
Correct. However, for LabVIEW it’s much easier than the C version I posted. You don’t need much in Begin.vi other than wire a FALSE into the visitedEnabled global (if you choose to make it a global - we make it global because we use it in a few places).
Download “The Secret Book of Labview v1.0” here and go to page 25 (page numbered 20, page 25 of the PDF) and try to implement the “boxcar filter” - that will be your moving average that you need for your bias calculation.
How fast do you run that periodic loop?
I don’t have the code in front of me, but it’s either 5 or 10 ms. We’ve been doing most of our control code in a 25 ms loop, but we have one really fast loop just for this.
And you just reference the gyro heading in tele/auto by using a global variable, right?
Correct
Although I have never done this before, I would imagine this only holds true for LabVIEW users. One of the benefits of Java and C++ is that you can change pretty much any of the high level code in the WPILIB. In this case, just extend the gyro class and rewrite whichever part of it calculates the bias.
Edit: There is a convenient int in the Gyro class (Java) called m_center. All you would have to set that variable to voltage of the analog channel at the start of the match. Unfortunately it is set at the default access level without a modifier method, so you would have to extend the gyro class (or just make a slight modification to your version of the WPILIB).
It’s not quite so easy (in Java at least). The method that calculates the gyro bias is private, so it can’t be extended. You have to patch WPILib.
As I said in my edit, you don’t have to modify that code. Just overwrite the m_center variable at the start of the match. Good catch though.
Kind of, but that’s not really how the gyro works under the hood. The FPGA onboard the cRIO has a hardware accumulator which can integrate a signal coming from the ADC at a specific sample rate. This is used for the gyro as the timing is really precise and fast. In software, there is a memory mapped interface to the FPGA used to configure and view state on the various subsystems (accumulators, counters, etc.). The code in initGyro does the following:
- Reset and init an accumulator on the channel the gyro is plugged in to.
- Wait 1 second
- Turn on the accumulator
- Wait 5 seconds
- View the ‘value’ of the accumulator (a 64b long) and the count of samples taken to get there.
- Find the average voltage per sample, set that as the new ‘center’ for the accumulator.
- Reset the accumulator
When you ask for an angle reading, it just reads the current ‘value’ out of the accumulator and scales it to a human understandable value.
m_center holds a local copy of what got written into the FPGA’s accumulator as the ‘center’. It is just a cached value used to convert the last few average samples on the ADC to a measurement with correct units in getRate. (Remember, the accumulator is doing the heavy lifting for measuring position). Writing to m_center won’t really help here.
I don’t know why WPILib chose to wait for 5 seconds to denoise the sensor. It seems like this can be done in WAY less samples (like, 10ms).
So long as you put your subclass in the edu.wpi.first.wpilibj package you should be able to poke around at the accumulator object, so you can just build a new init method. Which is dumb.
I don’t think drift is all of the problem. This year we used the KOP gyro for autonomous. Drift is no problem for this short period. Back in 2012 we never got an acceptable field centric working. 2 weeks ago the programming team developed field centric code again. Put it on the robot with the kop gyro and with some adjustments in code it works fairly well. Most of the testing was done at low velocity checking rotations and stuff. Yes, there was drift but, not excessive. Would be fine for a 2 minute match. Last wed. The drivers tried the field centric code and when the robot is driven hard the the way swerve should be, The gyro goes nuts. Large 10 15 20 30 40 degree swings in 10 to 15 seconds in both rotational directions. The drivers where practicing figure eights. Had to put robo centric back on for the rest of the night. This has to be some thing other than drift. Acceleration affecting the gyro rate? We have some other digital gyros to test. Just have to figure out and program them into the c-rio. We need a rock solid orientation to use field centric in competition. Is this a swerve issue, how do other teams do it.
Could the gyro have been saturated?
Just to pile onto a thread that was once about mecanum robots.
The LabVIEW WPILib code is just as open as C++ and Java, and is in fact even easier to modify or use as a template to write your own stuff.
By the way, it is highly likely that the WPILib code will be modified to incorporate better calibration for 2015. The implementation in WPILib is simple, but not entirely incorrect. If your team relies heavily on the gyro, you can better characterize your sensor and customize the calibration to your setup.
Greg McKaskle
Yes, that’s a real thing. I remember seeing a value of 0.2 degrees per second per g of acceleration. That doesn’t sound like much, and a properly mounted yaw rate sensor with good mechanical isolation shouldn’t give you significant problems. But remember that vibration is also detected as acceleration, and it’s possible that having the sensor rigidly mounted to the robot frame can yield false readings when the robot is in motion.
Consider also the possibility that you have an electrical issue. Signal noise picked up from motor wiring on the gyro analog signal will be accumulated as errors in the gyro angle. Make sure your sensor wiring is not running alongside power wiring.
Is there an easy way to do this in LabVIEW?
Or do I have to put something in auto and tele that wires true to the visitedEnabled global?
Like this:
Actually, we usually just wire a TRUE constant into visitedEnabled in Teleop and our Auto code. That makes it much easier.
(EDIT: Sorry for the issues. The site is having attachment problems. I should be able to post it in the morning unless the site problems persist.)
