Technical Concepts
-
Interfacing with devices on I2C bus
-
Adding and utilizing external Arduino libraries
-
Some embedded C/C++ concepts
-
Cloning code repositories with Git
-
Basic dynamics concepts like inertial reference frames and pose
Introduction
What's do quadcopters, cell phones, nuclear warheads, and the Segway all have in common? If you guessed "the potential to destroy the world," you would be close but the answer I was looking for was "inertial measurement units", or IMU for short. While all these devices have IMU's made with varying levels of accuracy and precision, each of them uses an IMU in some form for the purposes of guidance, navigation, and orientation. A very common application of inertial measurement is to form an attitude and heading indication system, otherwise known as an AHRS (pronounced AY-harz). On aircraft, for example, the AHRS is integrated with an "artificial horizon" screen so that the pilot can determine his current pitch, roll, and yaw relative to straight and level flight.
An example AHRS with added altitude and ground speed indication
This project aims to create a basic AHRS which uses colored lights on a WS2812 RGB LED strip to indicate how an MPU-6050 IMU is oriented in three-dimensional space.
What You Need for This Project
- Hardware
- Arduino Uno
- WS2812 Addressible RGB LED strip
- MPU-6050 breakout board
- Breadboard and jumper wires
- Software
- Arduino IDE
- Git
- GitHub Repositories
- yaaa (project files)
- i2cdevlib (included in yaaa -> tools-and-resources)
- Adafruit_NeoPixel (included in yaaa -> tools-and-resources)
Hello, MPU. How Do You Do?
The MPU-6050 from Invensense (full disclosure: I'm not affiliated with Invensense in any way) contains a three-axis accelerometer and three-axis gyroscope which respectively measure acceleration on and rate of rotation about three orthogonal axes. This brings your total variables in the system (or "degrees of freedom", if you prefer) to six. More degrees can be added with an additional GPS, real-time clock, barometer, magnetometer, or other spacial/inertial sensing device. Each additional degree of freedom increases the potential accuracy of the IMU, assuming the algorithms that read the sensors utilize the data effectively. Both the accelerometer and gyroscope in the MPU-6050 are made with MEMS technology which means they rely on the physical deflection of microscopic structures made out of silicon. An example of the beautiful intricacies of a MEMS gyroscope can be seen below:
MEMS gyroscope picture from www.geekmomprojects.com
Cast Away Your Inhibitions and Boogy with Some Crazy Rhythms... It's Time for Filter Algorithms!
Generally speaking, getting the raw data from an IMU is fairly trivial. The real challenge (and why they pay guidance and nav. engineers the big bucks) is figuring out ways of filtering and fusing the data such that they quickly, accurately, and precisely predict the orientation and/or "dead reckoned" position of the craft or device. When dealing with accel and gyro data, two major issues present themselves. The first is that accelerometers are inherently very noisy and will only provide an accurate picture of acceleration after many samples have been averaged over a period of time. The gyroscope, on the other hand, suffers from the opposite problem: at large time scales, the measurement is susceptible to "drift". There's a bunch of different factors that cause this phenomenon but the primary one being that a gyroscope only measures angular rate (d[theta]/dt) and not angular position (theta). When you take the indefinite integral of the rate to get the position, it creates a rogue constant term. That constant creates a bias, and since the bias is time varying, causes the drift.Well isn't that just a pickle? How can one possibly overcome this limitation? Filter, baby, filter! Several very well known algorithms are used in this field like the complimentary filter, the Kalman Filter / Extended Kalman Filter (EKF), and particularly in hobbyist circles, the Mahony / Madgwick filter. OlliW does a nice comparison of the filter types on his blog and Mouser Electronics has a decent explanation on their website. The A Comparison of Complementary and Kalman Filtering by Walter T. Higgins Jr. (PDF linked below) also presents a very good description of Kalman and complimentary filters and how they differ.
With all that said, however, this project won't go into much depth about filtering the raw data... that's your homework for this evening. For this project however, the MPU-6050 provides us a very convenient shortcut. From the datasheet:
"Internal Digital Motion Processing™ (DMP™) engine supports 3D MotionProcessing and gesture recognition algorithms. The MPU-60X0 collects gyroscope and accelerometer data while synchronizing data sampling at a user defined rate. The total dataset obtained by the MPU-60X0 includes 3-Axis gyroscope data, 3- Axis accelerometer data, and temperature data. The MPU’s calculated output to the system processor can also include heading data from a digital 3-axis third party magnetometer."
What does all this gobbledygook mean? It means someone was paid the big bucks to do the hard work for us! The DMP engine filters and combines raw sensor data (even data from an external magnetometer) into a 3D representation and provides a very convenient interface for playing with the data. In addition, Jeff Rowberg's I2Cdev device library includes the algorithms to get that fused and filtered DMP data into useful quantites for navigation and orientation like Euler angles, quaternions, and pitch, roll, and yaw angles in an inertial reference frame, which is what we will be utilizing. Since this project only uses six degrees of freedom, the heading (yaw angle) is mostly just an "estimate." You'll see that it's a bit more prone to inaccuracy and drift in the video at the end. An external magnetometer or a 9-DOF IMU like the MPU-9250 would greatly mitigate that yaw error.
Hardware
The hardware is pretty straightforward. The MPU-6050 connects to the Arduino's I2C bus through the SDA and SCL pins located just beyond AREF and GND on the top header row. One newer models, SCL and SDA are labeled on the side of the pin header itself. It also uses the external interrupt on Pin 2 in order to specify immediately when the data is ready for processing. The way the I2C network is set up, it requires pull-up resistors between VCC and the bus pins. I used 10K resistors and everything seemed hunky dory. Interestingly, while technically being specified as a 3.3V device in the datasheet, the MPU-6050 is more than happy to tolerate 5V signals and power without blowing up (or even getting damaged). Supply voltage can handle VDD = -0.5V to +6V and logic levels of -0.5V to VDD + 0.5V. The RGB LED strip only has a single pin controlling the chain of LED's which is connected to Pin 4 in this case.A Fritzing diagram can be seen below:
Here is my actual hardware setup. I just soldered on some extension wire to my light strip to make it a bit easier to keep in place while I jostled my breadboard around.
Library Installation
After installing Git on your computer, find a nice project directory somewhere and in your terminal or command line type:git clone --recursive https://github.com/swedishhat/yaaaThis will download the project files as well as the other repositories listed in the "What You Need" section. In order to get the sketch in yaaa/mpu6050-led-ahrs/ to compile, you need to copy the following directories to your "libraries" folder inside of your Arduino sketchbook directory. This can be found through the Arduino IDE by selecting File>Preferences.
-
yaaa/tools-and-resources/Adafruit_NeoPixel/
-
yaaa/tools-and-resources/i2cdevlib/Arduino/MPU6050/
Code
The following code is a quick and dirty visualization of 3D space using a one dimensional light strip and various colors / intensities. A large chunk of the code for this project was lovingly borrowed from the "MPU6050_DMP6 / "Teapot Demo" included in the examples for Jeff Rowberg's I2Cdevlib Arduino library. The demo code was stripped down to it's bare necessities and used to get yaw, pitch, and roll angle information. I then used Adafruit's NeoPixel library to generate colors and intensity values for the pitch and roll quadrants. You can think of it in the context of the image below where roll, pitch, and yaw axes correspond to the X, Y, and Z axes respectively:
Oooh... shiny. But what does it mean!?
As the IMU rotates about the yaw (Z) axis, a white indicator will point to the current heading. At straight and level operation (pitch and roll values are approximately zero) a "Great Circle" can be drawn around the green equator of the sphere, concentric to the line traced by the yaw heading indicator. If the roll (X) or pitch (Y) values go negative over their respective axes, the LED's corresponding to the "nose" and port (left) side will go from green to red while the "tail" and starboard (right) side LED's will go from green to blue. The opposite will happen if the pitch or roll values go positive. That Great Circle can then be visualized as tilting around the sphere, and wherever the circle touches are the colors of the LED's on the strip.
The full code can be found in the project repository on GitHub but my "value added" code snippet can be seen below:
// ================================================================
// === NEOPIXEL AHRS ROUTINE ===
// ================================================================
// When we setup the NeoPixel library, we tell it how many pixels, and which pin to use to send signals.
// Note that for older NeoPixel strips you might need to change the third parameter--see the strandtest
// example in the lbrary folder for more information on possible values.
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIXELPIN, NEO_GRB + NEO_KHZ800);
#include // NeoPixel library from Adafruit
#define PIXELPIN 4 // Arduino pin connected to strip
#define NUMPIXELS 60 // Total number of RGB LEDs on strip
void update_led_ahrs(float yaw, float pitch, float roll) {
// Note: the YPR values are in DEGREES! not radians
// Clean slate.
pixels.clear();
// Determine the "nose" of the AHRS from yaw angle indication
int yaw_index = int(NUMPIXELS * (180.0 + yaw) / 360.0);
// Light intensity for pitch and roll quadrants
float roll_brightness = 255 * roll / 180.0;
float pitch_brightness = 255 * pitch / 180.0;
// Bread and butter: Counts through a quadrant's worth of NeoPixel indecies
// and determines the appropriate color for all four quadrants. The pitch and
// roll brightness values then scale how red, blue, or green each pixel. This
// emulates in admittedly simplistic fashion a Great Circle around an RGB
// sphere with red at the "South Pole", green at the "Equator", and blue at
// the "North Pole". Green values are inversely proportional to pitch and roll
int i;
for (i = 0; i < (NUMPIXELS / 4); i++){
if (pitch >= 0) {
pixels.setPixelColor((yaw_index - (NUMPIXELS/8) + i) % NUMPIXELS, pixels.Color(pitch_brightness, 255-2*pitch_brightness, 0));
pixels.setPixelColor((yaw_index + (3*NUMPIXELS/8) + i) % NUMPIXELS, pixels.Color(0, 255-2*pitch_brightness, pitch_brightness));
} else {
pixels.setPixelColor((yaw_index - (NUMPIXELS/8) + i) % NUMPIXELS, pixels.Color(0, 255 + 2*pitch_brightness, -1 * pitch_brightness));
pixels.setPixelColor((yaw_index + (3*NUMPIXELS/8) + i) % NUMPIXELS, pixels.Color(-1 * pitch_brightness, 255 + 2*pitch_brightness, 0));
}
if (roll >= 0) {
pixels.setPixelColor((yaw_index - (3*NUMPIXELS/8) + i) % NUMPIXELS, pixels.Color(roll_brightness, 255-roll_brightness, 0));
pixels.setPixelColor((yaw_index + (NUMPIXELS/8) + i) % NUMPIXELS, pixels.Color(0, 255-roll_brightness, roll_brightness));
} else {
pixels.setPixelColor((yaw_index - (3*NUMPIXELS/8) + i) % NUMPIXELS, pixels.Color(0, 255 + 2*roll_brightness, -1 * roll_brightness));
pixels.setPixelColor((yaw_index + (NUMPIXELS/8) + i) % NUMPIXELS, pixels.Color(-1 * roll_brightness, 255 + 2*roll_brightness, 0));
}
}
// Set the "nose indicator" and turn the device on
pixels.setPixelColor(yaw_index, pixels.Color(255,255,255)); // White as can be
pixels.show();
}
No comments:
Post a Comment