![]() |
Got Gyro drift? Try Sensor Fusion !
1 Attachment(s)
The RoboBees release a simple Inertial Measurement Unit (IMU) based Attitude and Heading Reference System (AHRS), written in LabVIEW.
There are several commercial, automotive grade devices available that are IMU-based and maintain good quality orientation (Roll, Pitch, and Yaw) information over time. Some are within reach of well-financed FRC teams. Here’s some free code that lowers the entry cost for teams that would like to eliminate the drift associated with KoP Rate Gyroscope sensors and try their hand at integrating a basic AHRS into their next robot design. This code is compatible with the AdaFruit.com 9-DoF IMU, product ID 1714, which runs about $20 (no affiliation). This breakout board contains a triaxial Accelerometer, Rate Gyroscope, and Magnetometer, powered by 5 VDC and interfaced over the I2C bus (cRIO and RoboRIO compatible). The Magnetometer sensor is likely to be new to many teams, and challenges remain in successfully integrating it on an FRC bot, but its contribution is critical to stabilize what is arguably a robot’s most important rotation - Yaw. The simple Complementary Filter is applied to fuse the three sensors into a single composite sensor which provides accurate and stable rotation indications in all 3 dimensions (Roll, Pitch, and Yaw). This sensor fusion approach effectively combines the individual sensor's best respective properties while mitigating their individual shortfalls. Code runs on the cRIO with last year’s version of LabVIEW, and should easily port once FIRST releases this season’s LabVIEW version for the new RoboRIO. IMU calibration can be involved and a bit esoteric … the code guides you through a simplified calibration process to help you get the most out of your sensor. The various IMU code modules are patterned after the familiar WPI sensor VIs. There’s no coprocessor to integrate or soldering/assembly. The concept behind the released demonstration project is to have you up and running by simply plugging in the IMU and supplying your FRC Team number to the LabVIEW Project file. The files have a bunch of comments for how the code works, equation references, and implementation considerations to promote learning about this powerful technology. Bug reports are welcome! Best wishes to all FRC teams this season, the RoboBees are excited about seeing you out on the field! The RoboBees FRC Team 836 [Edit:Code updated 1 Jan 15 to include all required dependencies.] |
Re: Got Gyro drift? Try Sensor Fusion !
1 Attachment(s)
For those who want quick access to some additional detail ...
Description: This series of Inertial Measurement Unit (IMU) subVIs is patterned after the WPI series for reading sensors, and includes several additional IMU Application subVIs. These application subVIs do the math required for sensor fusion, forming a basic Attitude and Heading Reference System (AHRS), with drift-free Roll, Pitch and Yaw outputs. An IMU typically consists of a 3-axis Accelerometer, Rate Gyroscope, and Magnetometer (sometimes the Mag is absent, resulting in reduced capability). Although technically incorrect, these devices are often referred to as 9 Degrees-of-Freedom (DoF) sensors (3 sensors with each 3 axes in XYZ = 9 DoF). The RoboBees IMU Series subVIs are: OPEN - Performs IMU initialization, setup, and calibration of each axis of Rate Gyroscope, Accelerometer, and Magnetometer. IMU RefNum Registry Set / Get - These are standard WPI-type functional globals used to pass IMU configuration data. READ - Reads IMU sensors, generates and applies calibration values. Gyro output provides XYZ rates as well as integrating these rates for degree displacements. Accelerometer output provides XYZ acceleration and indication of motion and freefall. RESET - Resets Gyro degree displacement to zero for all axes. COMPLE FILTER - Performs sensor fusion for basic AHRS using a complementary filter design. TILT - Calculate Roll and Pitch from gravity vector and implements an optionally tilt-compensated Earth compass for Yaw. These IMU subVIs are designed to be used with the Adafruit 9-DoF IMU Breakout - containing L3GD20 (Gyro) + LSM303 (Accelerometer and Magnetometer) chips (Product ID: 1714), or the Adafruit 10-DoF IMU Breakout - which adds the BMP180 chip (Product ID: 1604). The 10-DoF IMU adds a Barometer for air temperature and pressure measurements (for indicating altitude, not attitude), and is not currently implemented. The two IMU Application subVIs, COMPLE FILTER and TILT, are agnostic of IMU model, and compatible with separate sensors, as long as all sensor axes are aligned properly. The motivation to fusing these sensors is to combine their best properties and mitigate the worst properties of each. The overall sensor fusion design is provided in block diagram form (attached). A major benefit is effective elimination of the drift inherent in the Rate Gyro when used alone. Design: The IMU OPEN subVI sets up each sensor based on the respective Set Cluster settings. This includes setting the sensor range, output data rate, enabling the High Pass Filter (Gyro and Accelerometer only), and selecting a High Pass Cut off Frequency. Each sensor can also be selectively enabled/disabled. All sensors (except the Barometer), need to be enabled for a fully functional AHRS. The IMU breakout board from Adafruit.com places all sensors on a common I2C bus for communication. The sensor chip's I2C Addresses (in hex) and respective data sheets for I2C register references are: Chip - Sensor - I2C Address - Data Sheet Reference LSM303DLHC - Accelerometer - 0x19 - http://www.adafruit.com/datasheets/LSM303DLHC.PDF LSM303DLHC - Magnetometer - 0x1E - see above L3GD20(H) - Rate Gyroscope - 0x6B - http://www.st.com/st-web-ui/static/a...DM00036465.pdf BMP180 - Barometer - 0x77 - http://www.adafruit.com/datasheets/B...0-DS000-09.pdf Note: Adafruit modified both breakout board configurations in Oct of 2014 to upgrade the Gyro sensor from L3GD20 to L3GD20H (improved performance specifications). This code should be compatible with each Gyro variant. Guidance on the Gyro startup sequence is given in section 3.1 of ST MicroElectronics App Note AN4506 for L3GD20H : http://www.st.com/web/en/resource/te...DM00119037.pdf After the sensors are set up, enabled sensors are turned on, IMU configuration data is loaded into an IMU Cluster, and sensor calibration begins. Calibration: Raw readings from an IMU are not very useful until they are conditioned by carefully determined calibration parameters. Since reading the digital sensor values is a bit complicated, the IMU READ subVI is simply called by IMU OPEN to produce the raw measurement values used to characterize the sensor. The calibration is only good for the range selected during the cal process. The calibration process used here is determining the values 'm' and 'b' for the assumed linear sensor model Y=mX+b for each parameter/axis pair, where 'X' is the raw sensor reading, 'm' and 'b' are cal parameters, and 'Y' is the corrected sensor reading. The sensor is 'exercised' eight times to allow the Gyro readings to reach steady-state, which improves the calibration. With the default cal sample size of 30, the Gyro/Accelerometer cal process takes about 2.5 seconds, which is about 0.5 seconds longer than the default cal time for the OPEN subVI for an analog Gyro. The Gyro and Accelerometer cal is performed first, and the robot needs to be completely still during this time (as is required with an analog Gyro). Since the cal is conducted within the OPEN subVI, and OPEN is generally placed in Begin (in FIRST Robotics) before the robot can move itself, this requirement is easily fulfilled. Care, however, must still be taken to ensure no external bumps occur during this cal. This code can detect if a physical disturbance occurred, and recalibrate automatically as mitigation (no extra charge). Recalling the model Y=mX+b, for the IMU Gyro, 'm' is taken from the spec sheet as the gain (sensitivity) for each axis. Value 'b', which is bias from zero, is determined by taking the statistical mean of a sample of Gyro rate measurements while the sensor is still. For the Accelerometer, 'm' should be determined off-robot, as it requires the sensor to be moved in all 3 dimensions. These values are either determined experimentally off-robot (to improve upon the standard sensitivity values given in the sensor spec sheet), or assumed to be a value of one for each axis. To determine experimentally, find the maximum and minimum values of each axis as they are rotated (slowly) through vertical. The axis sensitivity is half of the signed difference between max and min. Value 'b' is determined by taking the statistical mean of a sample of Accelerometer measurements while the sensor is still. The fun really begins with the Magnetometer calibration. Proper Mag cal requires the sensor to be rotated in all 3 dimensions, but unlike the Accelerometer, the Mag must be rotated with the rest of the robot to account for how the robot itself disturbs the Earth's magnetic field (this is a lot easier when the Mag sensor is installed in a smart phone.). Since one typically would not have available a 3-axis non-ferrous robot gimbal, the cal process is designed to support a 2-D calibration in the X-Y (horizontal) plane only. In another departure from the Gyro/Accel cal, sample data from the Mag must be taken as the robot is moving. This process is optional to allow the 'm' and 'b' Mag cal parameters to be determined during final testing, and then loaded into the Mag Set Cluster for operations (i.e. competition). If selected, the Mag cal process first instructs you to begin rotating the robot (manually) with a pop-up dialog box. Selecting OK initiates data collection, and provides a second dialog box to allow you to signal when you've completed a 360 degree rotation. Once complete, the X and Y axis 'm' and 'b' parameters are calculated and displayed on the IMU OPEN Front Panel as 'scale' and 'zero', respectively. An XY graph of the data is also provided to allow visual inspection of the cal result - look for a nicely shaped circle (see example at left), if not, repeat the cal. If you like the plot, load the X and Y axis scale and zero parameters into the Mag Set Cluster, retain the default scale (1) and zero (0) values for the Mag Z axis, and turn off Mag cal by deasserting MagCal. Magnetometer measurement error is generally classified into two categories, Hard Iron and Soft Iron effects. Only Hard Iron effects are compensated here, as they appear to dominate. Soft Iron effect compensation is a future improvement. Performance is adequate as long as sources of Soft Iron distortion (non-magnetized Iron, Nickle, and Cobalt) are kept away from sensor (or vice versa). If an ellipse is displayed, there is Soft Iron magnetic field interference. Ensure the cal plot has equally sized X and Y axis ranges prior to visually inspecting the cal plot and making an ellipse determination. If Soft Iron is detected, either relocate the IMU or discover and remove the offending material. Run all motors and other actuators to verify that the Yaw output of the Complementary Filter is not significantly affected. If so, it may be possible to relocate the IMU or shield the motor with mu-metal or similar. Do not shield the IMU! Future Improvements: Barometer, move freefall into interrupt/DIO, DCM/Kalman filter, Soft Iron Calibration, 3D cal, Auto ranging, FPGA integration, Fuse Mag-based Yaw with differential encoders to correct for external-to-robot ferrous sources, SPI interface for faster read response. Implementation: Code as per example on upper left. Experimentally determine values for Accelerometer scale as described in above section on calibration, and load into the Accel Set Cluster (or default to one). Set appropriate ranges, fastest data rates (if polling), and enable each sensor. Install sensor on robot and initially set MagCal to True. Determine Mag cal parameters as described above and load these values into Mag Set Cluster. Set MagCal to False to skip future Mag cals. The Mag cal values are strongly a function of where the sensor is located on the robot, and what is installed nearby, so it's important to recalibrate fairly often, certainly after relocating the sensor, and again after the robot is completely assembled. Be sure you actuate all motors and mechanisms to test for interaction with the Yaw indication, which uses the Mag. Be aware that another robot that comes close to the Magnetometer might disturb the local magnetic field enough to cause error in Yaw – the solution to this difficulty is left to the enterprising student of sensor fusion. Enabling the Gyro and Accelerometer High Pass Filters (HPF) can have a deleterious effect on dynamic response, as they can introduce lag. HPFs default to off, but advanced control system designers may find them useful. Experiment cautiously. The Accelerometer senses both gravitation and movement (study Einstein's principle of Equivalence). Roll and Pitch will be affected by movement, as will Yaw should Tilt-Compensation be enabled (this is selected with the Comple Filter subVI). Configuring these IMU subVIs per the example creates a simple Attitude and Heading Reference System (AHRS). The range of Pitch/Roll/Yaw values are constrained in range, however, as follows: Pitch range is -90<P<+90, Roll range is -180<R<+180, and Yaw range is -180<Y<+180. Although over-rotating Pitch and Yaw beyond their range will result in erroneous readings, this should be an adequate operating range for most robot body applications. These range constraints may or may not be adequate for an actuator or mechanism. Yaw is a bit different ... North is 0 degrees, and East/West is -90/+90 deg, respectively. South is both -180 and +180 deg, as these values are congruent. It is ok to over-rotate Yaw, as this portion of the Complementary Filter is designed with logic to smoothly transition across the +/-180 degree discontinuity (auto-wrap). It is important to note that while the R/P/Y values are constrained, the Gyro displacement values in X/Y/Z are not. Gyro deg outputs from the IMU READ subVI will wind (not wrap) - i.e. they will continue to count above/below 360 degrees. Use the LabVIEW Quotient & Remainder (modulus) function to convert these outputs to a wrapping behavior, if desired. Implementation considerations for IMU Read: This design performs numerical integration off-FPGA for the Digital Gyro (unlike the Analog Gyro subVIs, which typically utilize the FPGA to perform accumulation for superior speed resulting in less integration error). Testing indicates this off-FPGA approach may be adequate for FRC application, however, it is important to ensure the code runs without any timing issues. This is why a Timed Loop is recommended for the IMU Read within Periodic Tasks (executes at a higher priority). Non-uniformity in read iteration is mitigated by design, as each dt is uniquely calculated by time stamping each read event. This means that while some variation in timing can be tolerated (the occasional late iteration), excessively long durations (periods) between successive reads will fail to capture the actual rate profile as it changes resulting in integration error. In short, use a Timed Loop structure to iterate IMU READs, and set the loop period for a relatively fast iteration (<75 ms). Shorter periods provide improved accuracy of degree displacements, however setting this too short can impact the execution of your remaining code (especially when using Timed Loops). One way of determining an optimized period is to set it while monitoring the "Finished Late?" Loop parameter, ensuring this value stays False. It helps if the CPU Utilization remains low (<75%), which can be monitored with the RT Get CPU Loads subVI (Real Time menu). Study LabVIEW determinism for additional insight. ----- The RoboBees FRC Team 836! ----- You too can conquer inertia!! ... consider a career in STEM! |
Re: Got Gyro drift? Try Sensor Fusion !
I was wondering if this will work with the new roborio. I was looking at the code and it looks like you have the open VI setup for the old system. How would you go about setting this up for the new system
|
Re: Got Gyro drift? Try Sensor Fusion !
The posted code was developed with last year's 2013 LabVIEW (LV FRC 2014) and will not work with the new roboRIO until ported. I've had a chance to take a quick look at the LV FRC 2015 copy that we all received, and have noted that the WPI Library for I2C communication has been changed enough to require a bit of tinkering:
(a) The I2C addressing on the WPI I2C open VI now appears to be 7-bit based ... this means you no longer have to left shift the device address by one bit. (Reliable source : Joe Ross / Beachbots, thanx Joe) (b) The I2C DevRef includes additional parameters to account for things like identifying which of the two roboRIO I2C buses the device is connected to, is now a TypeDef, and re-sequences the parameter order. This make it initially incompatible with my IMU READ Bundle Constants - but is an easy fix. (c) The code would need to be moved into a project with a roboRIO Target, as the current Project File is cRIO-targeted. Note the I2C VIs change configuration internally as they are target-sensitive ... for example, you'll see the I2C compatibility mode connection on the I2C read VI disappear since I believe roboRIO I2C communication now auto-detects clock stretching. (d) Also note that the on-line documentation for the I2C read VI appears to have some incorrect information (and is not updated). It claims to limit the maximum number of bytes read in one read operation to four, but I don't think this is correct. This IMU/AHRS code is designed to read six bytes per read operation for efficiency, and the current WPI library still seems to support this. If you're handy with code tinkering, start with with issues (a)-(c) and you can probably port the code yourself. I will release a roboRIO version myself, on this thread, sometime soon. If anyone already has the IMU sensor and needs to get this working now, let me know so I can help out - I'll re-prioritize my build season to-do list! Richard |
Re: Got Gyro drift? Try Sensor Fusion !
i do have this sensor and have been trying to get it to work for the past couple days. Thanks for your reply now that i have a better idea of what needs to change i might be able to figure it out. if i do i will post it here. Thanks for your help
|
Re: Got Gyro drift? Try Sensor Fusion !
1 Attachment(s)
Here is the RoboBees IMU-based Attitude and Heading Reference System (AHRS), designed for the roboRIO, and written in LabVIEW.
Porting to the roboRIO is complete, and issues with the WPI I2C library changes (mentioned above) have been resolved. To try it out, load the Project file, open the IMU Demo VI, and follow the instructions listed on the front panel. Some additional update details: (a) The calibration time has been optimized to take advantage of the roboRIO’s processing speed improvement over the cRIO. A new set of cycle-time benchmarks were established for the IMU READ operation: __________________________________________________ ______________ - IMU Read benchmarks, with roboRIO or 4-slot cRIO - For enabled Accelerometer (A), Rate Gyro (G), and Magnetometer (M), the time to execute IMU Read in milliseconds (ms): Code:
Sensor ... roboRIO Slow / Fast 4-slot cRIO Slow / Fast selected__________________________________________________ ______________ (b) The Complementary Filter uses a value called ‘Alpha’ to set the fusion point between sensors. There are now suggested Alpha value starting points on the IMU Demo Front Panel to lower the learning curve on filter tuning for optimal fusion. (c) This code has been tested with both AdaFruit’s 9 DoF IMU Breakout Board (Product ID: 1714) and their 10 DoF IMU Breakout Board (Product ID: 1604), and is compatible with either Rate Gyroscope chip version (STMicroelectronics L3GD20 or L3GD20H). (d) Includes a ‘Bad Cal’ indication for the Magnetometer calibration process, completing the set (A,G,M). These detect when excessive variation exists in the calibration data set produced by IMU OPEN, which can result from some of the common calibration failure modes (like bumping the Rate Gyro when is should be at rest, and attempting to cal the Mag near a field disturbance). (e) Includes basic Magnetic Anomaly Detection. This might be used for knowing when to stop trusting the Complementary Filter’s Yaw output or knowing when another robot gets near yours. Calibrated magnetic data should be consistent with data available at NOAA’s National Geophysical Data Center at : http://www.ngdc.noaa.gov/geomag-web/...e=true#igrfwmm (at least it was in Southern Maryland). (f) The code has a known issue in interfacing with the MXP’s I2C bus, using the Rev Robotics MXP Expansion Board. Works fine with the roboRIO’s On-Board I2C bus. This is probably something I’m missing in software configuration, and is under review. Enjoy and good luck this season ! |
Re: Got Gyro drift? Try Sensor Fusion !
You just made my day. I have been working the past week on getting this into labview 2015 but i have had no luck. I was just about to give up on this idea when i saw this. I am going to try it right now. Thanks you so much
|
Re: Got Gyro drift? Try Sensor Fusion !
I just tested the code. It works great. Very impressive. Thanks again for putting this code up
|
Re: Got Gyro drift? Try Sensor Fusion !
Quote:
|
Re: Got Gyro drift? Try Sensor Fusion !
Have confirmed via testing that issue is restricted to reading multiple devices on MXP I2C bus only. Reading one I2C device on MXP appears fine.
Have reported issue to WPI Library bug tracker for disposition. |
Re: Got Gyro drift? Try Sensor Fusion !
Thanks for sharing! I think this raises the entry point for autonomous localization and navigation, which is great.
Quote:
The strength of Earth's magnetic field is about 40-50 microtesla [wikipedia]. The magnetic field due to current in a wire is 2e-7 * I / r, where I is current in amps, and r is the distance in meters. Say you're within 33cm of a wire. That's 0.6 microtesla/amp, and if you've got a drivetrain that pulls 60+ amps (forward and backwards), you've suddenly got a 70+ microtesla swing that will overwhelm the field you're getting from the earth. The thing that scares me about this is that it will kinda-mostly work when you're doing simple testing (without a real robot, or under light load), and start going bizerk when you try it on the field. |
Re: Got Gyro drift? Try Sensor Fusion !
Can it be done with only a gyro and accelerometer?
|
Re: Got Gyro drift? Try Sensor Fusion !
Steven - You need to worry about both. I actually developed the code with the sensor installed on our last year's robot (I do this to ensure I'm accounting for all the other code we run, as the rate gyro numerical integration is particularly time sensitive). In limited testing, I found no real issue installing/operating the IMU on my robot until the drive system was engaged. I also found that pushing (an off) robot alongside my IMU-capable robot produced a similar field disturbance (which makes an interesting robot proximity detector).
Of course you would want to locate the IMU as far away as possible from both the magnets in the DC motors and the current supply wiring - 33 cm is too close. I also experimented briefly with shielding the CIMS with mu-metal, with some encouraging but admittedly incomplete results. As I mentioned, 'challenges remain in successfully integrating the Magnetometer on an FRC bot', and 'be sure you actuate all motors and mechanisms to test for interaction with the Yaw indication, which uses the Mag'. Hint: You don't have to use the Mag all the time ... |
Re: Got Gyro drift? Try Sensor Fusion !
"Can it be done with only a gyro and accelerometer?"
You cannot get a Yaw indication with an accelerometer, since Yaw is rotation about the Gravity vector. You need something, like a Mag, to generate a Yaw-direction as a reference vector. This disturbance of the Earth field, however, will have no effect on the complimentary filter's Pitch and Roll indications. |
Re: Got Gyro drift? Try Sensor Fusion !
Update on MXP I2C issue: FIRST reports that they have located the bug causing multiple device read issues on the MXP I2C bus, and has committed the fix into their source code.
I would think this means that the next code update we receive would resolve the issue, but I have no concrete information on this or when to expect it. |
Re: Got Gyro drift? Try Sensor Fusion !
Hi
On your DEMO Block Diagram you show connecting the VIN pin on the Adafruit board to the +V of a DIO channel. I'm assuming this is because the power pin on the RoboRio's I2C port is rated at 3.3V and the 10DOF board inputs are VIn or 3.0 V. So you opted to power the raw VIN with 5.0V However, this means splitting the cable into two connector areas. Reading the spec on the Adafruit page, https://learn.adafruit.com/adafruit-...nnecting-it-up .. it looks like we should be able to power the board from the I2C power pin. Is there another reason for not taking this simpler wiring approach? It says: ---------------------------------------------------------------------------- 3V3 Setup If you are using an MCU or board with 3V3 logic (instead of the 5V logic used by the Arduino Uno), you can still power the 10-DOF with the VIN pin or you can use the 3Vo pin, which will bypass the on-board 3V3 regulator and level shifting: Connect Vin or 3Vo on the breakout to the 3.3V supply on your target MCU Connect GND on the breakout to GND on the target MCU Like other breakouts on Adafruit, the 10 DOF Breakout is fully level shifted, and you can safely use it on 3V3 or 5V systems. ---------------------------------------------------------------------------- |
Re: Got Gyro drift? Try Sensor Fusion !
Phil - I did opt to power the board with 5 volts. I have not conducted any testing with the 3.3 volt I2C supply, although I suspect it will work fine and does present a simpler wiring situation.
|
Re: Got Gyro drift? Try Sensor Fusion !
:confused:
I am a bit lost and in need of help. I have been helping my team with their programming for a few years now and I can get all of our needs met and then some. We are trying to step it up and do field oriented drive for our mecanum wheels this year. I have read this thread 3 times over and looked at the download for the roboRIO more than once. Can someone please explain (or provide an example) how to integrate the download into our team code so that we can use the output of the 9DOF chip as our gyro input. everyone I have asked says to look up this thread. I may just be not seeing a forest through the trees here. Any help you can provide would be greatly appreciated in this last build week. |
Re: Got Gyro drift? Try Sensor Fusion !
Eric - I would recommend running the IMU Demo vi first, to get familiar with the settings, operation, and calibration. You will want to clearly understand how to set the loop timing, how it affects both the IMU readings as well as your other code. You will also want to understand how to set the two 'tau' values to tune the complementary filter performance.
Once you understand how it works, there is an example on how to integrate it into your robot code within the IMUOpen vi. Look at the LabVIEW Block Diagram of the IMUOpen vi ... scroll down to where there is a large note section describing how it works. To the left of the note section is the example on how to implement these IMU vi's into your code. There is one piece of code that goes into Begin, and another that goes into Periodic Tasks (AKA Timed Tasks). [They both have a Disabled Structure wrapped around them so they won't execute, as they are only there to illustrate implementation.] Copy this example code into your Robot Project Begin and Periodic Tasks, and remove the Disabled Structures. If the IMU Demo worked on your roboRIO, this should also work. Take either the X/Y/Z Gyro output of the IMURead vi, or the Roll/Pitch/Yaw output of the Comple Filter vi as your sensed values, depending on what you're trying to achieve (sensor fusion or just using this code to read the Gyro.) Understand that this sensor fusion uses the Magnetometer to stabilize the drift associated with the Gyro's Yaw - which is what you'll use for a field-oriented drive. If you haven't much experience with a Magnetometer, I would also recommend you initially start by simply getting your field-oriented drive working with the Z-output of the Gyro (pre-sensor fusion). Then ensure you actuate all robot mechanisms (especially your drive system) to verify the Magnetometer readings are not affected (significantly). Any significant field disturbance will otherwise directly affect your field-oriented drive performance, until resolved. It can be challenging to maintain constantly clean readings of the Earth's field on an FRC-bot ... your issue mitigation options are IMU relocation, magnetic shielding, or clever use of when you take a Mag reading. (Most teams that report on CD, indicate that they only use the Mag to get an initial bearing during Disabled Mode, when the bot is largely powered down, and then live with Gyro drift during the match). From the code documentation: 'The Mag cal values are strongly a function of where the sensor is located on the robot, and what is installed nearby, so it's important to recalibrate fairly often, certainly after relocating the sensor, and again after the robot is completely assembled. Be sure you actuate all motors and mechanisms to test for interaction with the Yaw indication, which uses the Mag. Be aware that another robot that comes close to the Magnetometer might disturb the local magnetic field enough to cause error in Yaw.' Bottom line - the Mag is not a slam-dunk sensor for an FRC-bot, it requires (very) careful integration. The referenced code can be used to produce Gyro Yaw readings that are independent of the Mag, but will have some drift. Pitch and Roll can be drift-free with sensor fusion, and are independent of the Mag. |
Re: Got Gyro drift? Try Sensor Fusion !
Thank You for the very quick response. I must have missed these notes it in my reviews of the code. I will read the notes in IMU Open vi and try again.
|
Re: Got Gyro drift? Try Sensor Fusion !
2 Attachment(s)
This just in ... Here's an update to the roboRIO IMU code to fix a bug in the IMU Open vi. The bug affects the Rate Gyro operation by causing it to be set up incorrectly. This bug ONLY affects the newer AdaFruit 9 and 10 DoF IMUs, that have the L3GD20H Rate Gyro chip (purchased after Oct 2014).
I'm attaching the entire Project update, but if you like you can just make the change, (highlighted in the attached PNG Image file), in the code you may have already downloaded. |
Re: Got Gyro drift? Try Sensor Fusion !
Thank you for all the help. I am 99% of the way there.
I got the Demo to work, and i integrated it in my code with no issues when i run code via fast deploy (run Robot main) it makes me do the calibration on the first run. However i cant get it to work when i do a build and deploy to run via WiFi or FMS. Any suggestions? |
Re: Got Gyro drift? Try Sensor Fusion !
Eric - The IMU Open vi (in your Begin) has a Boolean input labeled "MagCal" ... set it to False.
This tells the IMU Open to skip the Magnetometer calibration process, which uses dialog boxes to gate the data collection sequence. I suspect that the dialog box is hanging your code (since there's no way to answer it when you deploy code). If you're using the complementary filter for fusion, you'll need to calibrate the Mag, and then transfer the cal values into Mag Set cluster for the IMU Open. Once this is completed, you would turn off the MagCal input (it's not automatic, sorry) - this would be the competition mode, when you are using deployed code. If you're not using the fusion filter, just leave MagCal false. Hopefully that will fix it ... |
Re: Got Gyro drift? Try Sensor Fusion !
The issue previously reported that the roboRIO can not read multiple I2C devices (like an IMU) when using the MXP I2C bus has been resolved in the mandatory FRC update 2015.1.0 (LabVIEW WPILib changes: Fixed artf4003: multiple I2C devices don’t work on MXP).
Download and install the update (from the link on the LabVIEW FRC 2015 Getting Started page). The released RoboBees IMU demonstration code has been tested against this update and will now work correctly when the IMU is connected to the I2C bus on the MXP. You can use either I2C bus now, On-Board or MXP. This provides a means to employ two IMU's, if required. |
Re: Got Gyro drift? Try Sensor Fusion !
I'm experimenting with this code and the Adafruit 10DOF board.
I have it all wired up and I'm watching the displays on the demo VI. On the pitch/roll/yaw dials I'm seeing proper action on the RED arrows, but little activity on the grey arrows. From the comments I expected both color dials to more or less follow each other with variations from drift. But the grey dials never get far from zero. Is this expected with an "out of the box" VI configuration. Phil. |
Re: Got Gyro drift? Try Sensor Fusion !
Phil - The issue you're describing can be caused by the programming error I identified in post #21 of this thread - if you're using one of the newer IMUs (purchased after last Oct). You can either download the corrected project file, or simply make the change illustrated in the code image attached to that post.
Another cause of deviation between the rate gyro and it's fusion (the grey & red needles, respectively) can be over-ranging the rate gyro by rotating it faster than the range it is set to. You won't damage the sensor but will miss some angular displacement reporting while beyond the set range. This is easy to do when the IMU is off the robot (say, rotating it in your hand). I suspect that the problem is the the first thing, though, since you describe the grey needles as not moving at all. Both needles should track out of the box, within a tolerance affected by the quality of the calibration process, and assuming the sensors are not over ranged. If you haven't already, try the code update and let us know if that resolves it. Let us know if the symptom persists and I'll look around for another cause. |
Re: Got Gyro drift? Try Sensor Fusion !
Quote:
I thought I had downloaded the latest version... but clearly I hadn't. Question... Does resetting the gyro during operation upset the sensor fusion math? It doesn't seem to, but I didn't want to assume. Phil. |
Re: Got Gyro drift? Try Sensor Fusion !
Operationally re-setting the rate gyro does not upset any of the fusion performance. The reset simply zeros the degree displacement accumulator (in software) ... so it works just like the analog rate gyro reset you're probably accustomed to. Reset at will.
The fusion algorithm uses the gyro rate (in degs/sec), not the gyro displacement (in degrees), to do it's magic, which is why there's no operational impact to the reset. Zeroing (resetting) the gyro displacement is, of course, different from zeroing the actual gyro rate (resetting the zero rate bias) - the latter only occurring during the calibration process embedded in the IMU Open vi execution. I'm assumming you're referring to former 'type' of reset, using the IMU Reset vi. |
Re: Got Gyro drift? Try Sensor Fusion !
So.. Loving the ADAIMU code. Thanks.
Have a problem. If the robot is being moved during startup, the Gyro gets a bad cal. I see where you make two attempts, but the problem could last longer then that, and I'd realy prefer not to hold up the begin code too long. I put a filter on the angular rate and check it in Disabled.vi If it get too large (more than 0.1) I want to do a recal. Easy with the analog gyro.... Can't figure how to do it with your IMU code I tried just redoing the open... it failed, looks like coms doesn't like two opens. There doesn't seem to be an IMUclose.vi What is the best way to initiate a re-cal, or restart of the IMU? Don't need a recal on the magnetics, just the gyro. Phil. |
Re: Got Gyro drift? Try Sensor Fusion !
1 Attachment(s)
While waiting for an educated reply, I generated a patch that seems to work. I created a IMUrecal.vi that I can call when I see a large drift while disabled.
It's taken from the original OPEN vi. I'm not sure if it's correct, or if I'm accessing the devref correctly, but it seems to be doing the job. I keep the devref in a global, and just re-write it with the updated cal values. Any good? Phil |
Re: Got Gyro drift? Try Sensor Fusion !
1 Attachment(s)
Phil -
You might like this ... I developed a new vi to constantly re-calibrate the gyro while the robot is in Disabled Mode. It is integrated into this updated IMU Demo Project, see the True case within the IMU Read Loop for the new vi "IMU ZeroBias". Alternatively, you could use this vi within the Disabled.vi if you like, I just implemented it all within Periodic tasks. Here, you would simply use the existing "Robot Mode" variable to drive the case structure within the IMU Read Loop. IMU ZeroBias will read a sample set of measurements from each axis for Gyro calibration. Samples are placed into a circular buffer to maintain the most recent period of samples, used to compute the most current zero rate bias. The intent is to constantly update the Gyro rate bias (during Disabled Mode, which is assumed to satisfy the condition that the sensor is not moving). This ensures we have the best possible calibration available immediately prior to the termination of Disabled Mode (DM), effectively dealing with the Gyro's thermal stabilization and the uncertainty in the variation of how long DM may persist from one match to the next - this way you always start with a 'fresh' cal even if it takes 10 minutes for a field start. If excessive variation in the circular buffer's dataset is detected, the gyro calibration is not updated, and automatically retains the last good calibration until the situation improves. In this way the code will logically adapt to both the delay before match start and any errant movement while Disabled that would otherwise spoil the cal - and your 32 point Autonomous. You can watch the IMU ZeroBias Front Panel to observe this behavior ... watch the constant calibration updates during DM, then bump it to see it freeze on the last good cal until both the physical disturbance is over and the bad data is removed from the circular buffer. So ... this is the best way I can think of, for the moment. A repeating IMU Open is probably no-go since the I2C bus needs to be closed first. Only caveat is I haven't been able to test this as much as I normally would like ... see it if works for you and let me know if you see any issues. See you at Chesapeake? |
Re: Got Gyro drift? Try Sensor Fusion !
Phil - Your IMU ReCal looks good to me. The code you have will also recalibrate the accelerometer bias. The DevRef access seems fine. This is a valid alternative to the IMU ZeroBias for a quick Gyro re-cal.
|
Re: Got Gyro drift? Try Sensor Fusion !
Hi Richard
Your new Zero Bias looks outstanding. I tore out my code and implemented your new system. (I kept a backup just in case of course ;) I'll get a chance to try it out tomorrow. I'll let you know how it goes. I see the code that rejects the bad calibration. How much movement is required to trigger this rejection? ie: Is it pretty strong in rejecting bumps etc? Yes, definitely see you at Chesapeake. Thanks again for this great code. The digital gyro I/F is really so much better than the Analog one. Phil. |
Re: Got Gyro drift? Try Sensor Fusion !
Quote:
Sensitivity is set by the constant (0.5) which is the threshold used to evaluate the data set standard deviation. The tradeoff involved in selecting the threshold is that while lower values will reject smaller movements, it will also increase false positives as the threshold nears the noise floor of the sensor. I arrived at the currently coded value experimentally to balance this tradeoff. You can modify the constant to a control and explore while monitoring the IMU ZeroBias Front Panel as you trigger it with movement. |
Re: Got Gyro drift? Try Sensor Fusion !
I am starting to experiment with this code and IMU in preparation for next season. First of all, thanks Richard for the code and thorough documentation/example.
I am assuming both Robobees and Gaco used this during competition. How did it work out (particularly the use of the magnetometer)? Thanks, -Steve Raque 2614 MARS |
Re: Got Gyro drift? Try Sensor Fusion !
Steve - We had the code installed but the capability ended up being overcome by events ... something to do with cutting our robot in half with a sawzall at Chesapeake to produce an entirely new bi-robot design (all within carry-in weight limits, which was the real challenge!). Turned out well but changed our focus, you might say, and having a AHRS wasn't key at the time.
|
Re: Got Gyro drift? Try Sensor Fusion !
Richard,
What is the calibration Use Case when the IMU is on the robot. Do you run the Demo project live and get the values, or do you run the robot code with the MagCal in IMUOpen set to TRUE? |
Re: Got Gyro drift? Try Sensor Fusion !
Steve - To conduct the calibration, you want to run 'Live', i.e. using a run-button deploy / debug mode, rather than compiling and deploying code to the roboRIO. This simply allows you to interact with the Dialog boxes that I use to prompt the calibration process (if you try to run the code as deployed when MagCal is TRUE then the program will appear to hang while it waits for you to respond to a Dialog box that, well, isn't displayed anywhere). Yes, you request magnetometer calibration by setting MagCal in IMUOpen to TRUE. Whether you use the Demo Project, or move the IMU code within Demo into your robot code, doesn't matter.
To use the IMU on the robot, it is also important that the calibration be performed with the sensor installed, to ensure the robot hard iron effects are compensated for during the calibration process. If you move the sensor, or change the metallic configuration of the robot, a re-calibration is strongly recommended. The general idea is to set MagCal to TRUE to perform the cal, run the code, execute the cal, practicing the process until you get a good result (referring to the Example Magnetometer Cal Plot), then stop the code. Copy the four resulting magnetic calibration values (Scale & Zero for both X & Y) listed under Magnetometer Cal Results on the IMUOpen Front Panel, to the corresponding Mag Set control input for the same VI. Save this and reconfigure MagCal to FALSE. Now when you run the code (which doesn't need to be 'Live' anymore) the IMU is fully calibrated and you can use the sensor fusion results from the Complementary Filter VI. Adjust tau mag and tau acc to taste, to balance the fusion. Some teams (I believe GaCo was one) only used the IMU and this software for it's gyro readings, which is fine if you don't need the benefit of sensor fusion. This still gives you a three-axis digital gyro. In this case, the magnetometer calibration is not required, and you would read your gyro results directly from the IMURead VI. The IMUZeroBias VI provides the circular buffer to constantly calibrate the Gyro while the robot is disabled, which can be useful in mitigating measurement drift. |
Re: Got Gyro drift? Try Sensor Fusion !
Quote:
Thanks! |
Re: Got Gyro drift? Try Sensor Fusion !
"Rotate robot 360 deg in *horizontal plane*. Best data results from rotating *about actual sensor axis*". (Dialog box instruction, * for emphasis.)
Try not to stray from the horizontal plane. Once the IMU is on robot, this is probably easier to ensure, since you're spinning the entire bot on the ground (and the robot wants to stay on the ground). Don't make a big wandering loop, spin the sensor about it's vertical axis. If the sensor is in your hand (off robot), you would place it flat on a (non-metal) table (and away from metal fasteners - i.e. look under your table!), and simply rotate the sensor in place without 'orbiting' anything. I usually mount the board on a small block of wood with squared-up sides which facilitates experimentation. If sensor is on-robot, stare at the sensor while you manually spin the robot, adjusting it's motion to maintain the sensor over the same spot on the ground, as much as you can - it won't be perfect. Make a complete 360 degree rotation, it's ok to go over some. I usually take about 20 seconds to complete the rotation when the sensor is on-robot, the software is collecting data samples during this period. IOW, don't try to go too fast. It's ok to partially rotate and stop long enough to readjust your grip, then start rotating again. The data analysis process is insensitive to this - you'll just see some data concentration on some locations of the data plot. If you receive an ellipse on the plot, instead of a circle, your sensor is experiencing soft iron effects for which the code does not compensate. Ensure the graph axes are scaled the same (equally sized ranges), prior to deciding that you see an ellipse, because the graph is set to auto range and might visually distort even though it's really a circle. Follow the guidance under the XY Mag Plot on the IMUOpen Front Panel should you get an ellipse. After trying all these tricks, if you still can't achieve a fairly noise-free circle, try moving your operation to a completely different location. You may be attempting the cal in an Earth field that is disturbed. I had no trouble with the calibration in my stick-framed workshop, but a metal-sided building might disturb the Earth field. (If you have access to an airport, you could perform the cal on their compass rose, as that area is certified to be magnetically clean ... joking - that's definitely going overboard). I don't expect this to be a real issue, just a possibility. Bottom line is to use the example plot for reference. This gives you a simple way to show you what you're trying to produce. The reference is typical of what I achieve after some practice. Like many things in metrology, technique can influence the result. |
Re: Got Gyro drift? Try Sensor Fusion !
1 Attachment(s)
Quote:
thank you for the programm. I am using an Adafruit 10-DOF IMU Breakout - L3GD20H + LSM303 + BMP180 with an ARDUINO UNO. The problem is: in your programm IMU demo.VI I receive some errors. For instance one is: "The master copy for this type definition could not be found or the master copy has errors. You must find and fix the master copy, or right-click this type definition and select Disconnect." Is it possible to work with your programm but with ARDUINO UNO? best regards |
Re: Got Gyro drift? Try Sensor Fusion !
tetris - It's not clear from your post that you are using the FIRST FRC version of LabVIEW and a National Instruments roboRIO. This is a FIRST Robotics forum and the assumed context is use of FIRST hardware/software. In either case, you're of course welcome to the software.
It seems the DIO Type Def error is coming up due a code change. The grayed out "Digital Module" Type Def is supposed to be a Type Def for selecting which I2C port is being used to connect the IMU to an NI roboRIO. If you have different hardware you may be able to adapt the code. The other two errors can be resolved by removing the two vi's with '?' - they also are not part of the original code release. If you are using FIRST software/hardware, try re-downloading the demonstration code, or re-installing LabVIEW. If you're not using the FRC release of LabVIEW or roboRIO, you'll have to adapt to what you're using. This LabVIEW code will not run on an Arduino. You might try the code library that AdaFruit maintains for their breakout board. You can download from their site : https://learn.adafruit.com/adafruit-...mp180/software Their code will provide you basic access to the readings from each sensor, but it does not perform sensor fusion - you would add that code yourself. If you can read LabVIEW, you can follow the logic used here to develop C code for the Arduino. |
Re: Got Gyro drift? Try Sensor Fusion !
Hello,
I am using the National instrument NI USB-8451. Do you think your programm can work with this device? When I open the programm in Labview, it looks for library " ROCK ROBOTICS" but it doesn't find it. Do you know where I could download this library? ( then i will maybe not have these "?"). I don't understand what is "FIRST FRC version of LabVIEW ". ( I am using LABVIEW 2014 STUDENT EDITION). What is FIRST software/hardware? Best regards |
Re: Got Gyro drift? Try Sensor Fusion !
Unfortunately, tetris, this is a forum for a specific set of hardware and software only available to FIRST robotics competition teams (google it to find out). We probably won't be able to help with using other NI hardware and software.
I suggest posting to one of the forums that NI sponsors. -Steve |
Re: Got Gyro drift? Try Sensor Fusion !
HI,
where can I download lobrary "ROCK ROBOTICS"? in order not to have these "?" Best regards |
Re: Got Gyro drift? Try Sensor Fusion !
The Rock Robotics library is designed to work with the hardware used by FIRST Robotics Competition teams, and the special FPGA image that goes along with it. If you don't have a RoboRIO or FRC-cRIO system, you won't be able to use them. And even if you do have that hardware, you will need the special Driver Station program running on a network-connected Windows laptop in order to enable the hardware outputs.
While some of us here are able to give general advice, you're not likely to get the specific help you're looking for in this forum. |
Re: Got Gyro drift? Try Sensor Fusion !
1 Attachment(s)
Thank you for your answers.
Is it possible to make 3 screenshots of the "context help" in Labview for these 3 Icons?: "WPI_I2COpen"; "WPI_I2CRead"; WPI_I2CWrite" I need the name and descriptions of inputs and outputs of each icon (example with picture attached). Thanks a lot, sorry to bother you Regards |
Re: Got Gyro drift? Try Sensor Fusion !
Quote:
Can we take a few steps back and get a more general view of what you're trying to do? If you're using the NI USB-8451, nothing you read here is applicable. |
Re: Got Gyro drift? Try Sensor Fusion !
Hello thank you for your answers....
It is for a school project (engineering master). I need the screenshot: i have installed labview 2014 and FIRST ( because I have student licence of my school). Best regards and Mery christmas |
Re: Got Gyro drift? Try Sensor Fusion !
Hello
yes exactly I need the WPI library functions (in the Rock Robotics directory) Can you send it to me ? Where can I find it? I need to understand the blockdiagramms in order to adapt the programm with my USB 8451 device. regards |
Re: Got Gyro drift? Try Sensor Fusion !
Quote:
I would recommend installing the "FRC Update". |
Re: Got Gyro drift? Try Sensor Fusion !
Thanks a lot yes I did it and it works.
Nice that you help people and thanks a lot jhersh..... because other user on this forum was very unhelpful and not friendly with me! Happy new year Jhersh and all the best in the new year 2016!!!! Denis from GENEVA |
Re: Got Gyro drift? Try Sensor Fusion !
Hi everyone! I hate to bring up an old post, but I want to post a shout-out to the RoboBees for providing the FIRST community with an elegant AHRS implementation!
As some of you may know, I've been working to improve the software libraries available for the ADIS16448 IMU. I ran across the RoboBees implementation and managed to integrate it into the main IMU code. Once again, thank you very much for all of your hard work! If you would like to take a look at the code, you can find it on GitHub here. |
Re: Got Gyro drift? Try Sensor Fusion !
Has anyone tried to use this code this year. I used it last year with great success. However i started to add this into my 2016 code and it keeps asking for a FPGA_TypedefDigitalModule.ctl does anyone know where to find this
|
Re: Got Gyro drift? Try Sensor Fusion !
Quote:
There's probably a faster way, but if you just drill down through the VIs and locate all the ones with errors, you can just delete the "module number" constant that is giving the problem. It's not needed any more. Phil. |
Re: Got Gyro drift? Try Sensor Fusion !
Bpk - If I remember correctly, you can just delete the reference to the .control in the code and the rest should work ok.
See this for a much more advanced IMU with LabVIEW code The RoboBees are using this one for StrongHold - you might like it. |
Re: Got Gyro drift? Try Sensor Fusion !
Hey good day,
I very much like the work you have done on the AHRS and I am thinking about manipulating the code to use with an Arduino. I am currently making a DIY flight controller for a quadcopter as a project for my BSc in Electrical and Computer Engineering and I am having difficulty in properly fusing my sensors. I am able to get raw data from them(I have an AltIMU 10 V5 https://www.pololu.com/product/2739) but my attempts to properly fuse the data has been futile thus far. I wanted to know if you think it is possible to build an Arduino version of the AHRS based on your code for LabVIEW platform? Regards. |
Re: Got Gyro drift? Try Sensor Fusion !
Quote:
|
Re: Got Gyro drift? Try Sensor Fusion !
To answer your question, yes, you should be able to write a simple fusion algo on the Arduino. The Complementary Filter (used here) is computationaly simple.
As Austin points out there appears to be Arduino code available ://github.com/pololu/minimu-9-ahrs-arduino/blob/master/README.textile Couldn't tell which fusion algo they use from the read-me. Other ideas: The IMU used in this thread is from AdaFruit ... they have interface code for the Arduino, but no fusion code if I recall. There are other IMUs that have built-in fusion algos, and can be interfaced with the Arduino ... There's another thread, https://www.chiefdelphi.com/forums/s...d.php?t=141100 that discusses this. If you use an IMU with built-in fusion it will likely out perform what you might develop and get you there faster. If you write your own, you'll learn more. |
| All times are GMT -5. The time now is 22:26. |
Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi