Got Gyro drift? Try Sensor Fusion !

[dhal_sauce]

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.

[AustinSchuh]

Originally Posted by dhal_sauce

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.

The page you linked to has sample code on it. (https://github.com/pololu/minimu-9-ahrs-arduino) which is for an arduino. That should be easier.

[Eric_Puccio]

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.

[Richard100]

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.

[Eric_Puccio]

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.

[Richard100]

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.

[StevenB]

Thanks for sharing! I think this raises the entry point for autonomous localization and navigation, which is great.

Originally Posted by Richard100

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.

I think I'd be more worried about transient magnetic fields due to my own electrical system, rather than other robots.

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.

[GuyM142]

Can it be done with only a gyro and accelerometer?

[Richard100]

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 ...

[Richard100]

"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.

[Eric_Puccio]

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?

[Richard100]

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 ...

[Richard100]

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.

[tetris]

Originally Posted by Richard100

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.]

Hello,
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

[Richard100]

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.