Posted 29 September 2019,
In my quest to figure out WTF happened to my ability to acquire real-time relative heading information on both my 2-wheel and 4-wheel robots, I have been trying to start from scratch with very simple controller/IMU hardware configurations. After succeeding with a basic functionality demonstration using a Teensy 3.2 and a Sparkfun MPU6250 IMU breakout board, I decided the next step would be to do the same thing with an Arduino Mega controller and a GY-521 )MPU6050 clone) to more closely replicate the hardware configuration on my 2-wheel and 4-wheel robots.
As usual I started this project with a web search for basic MPU6050/Arduino examples, and I found this YouTube video showing just what I was after. After going through the video several times to make sure I understood what was going on, I decided to try and duplicate it so I could compare my (hopefully) working demo code with my (currently non-working) robot code.
In my past efforts with the MPU6050, I had struggled with the complexities of using Jeff Rowberg’s wonderful (but quite massive and convoluted) I2CDevLib GitHub repository. There was always something that didn’t quite fit the situation, and making it fit invariably required a trip down the rabbit hole into Alice’s wonderland. Getting the right combination of files in the right places seemed to be more a matter of luck than skill. However, this particular video does a nice job of explicitly demonstrating what has to go where. Essentially the magic steps are:
- Download Jeff Rowberg’s IC2DevLib repository from GitHub into a ZIP file.
- UnZip the repository files into a temporary folder
- Copy the Arduino/I2CDev and Arduino/MPU6050 folders into the Arduino/Libraries folder. This makes them available to the Arduino IDE (and the VS2017/Visual Micro setup I use).
- Open a new sketch in the Arduino IDE (or a new project in the VS/VisMicro environment) and then:
- In the Arduino IDE select ‘File-Examples, and scroll down to the ‘Examples from Custom Libraries’ section. Then select ‘MPU6050->MPU6050_DMP6’ This will load the example code into the sketch.
- In the VS/VM environment, select the Visual Micro Explorer (under the vMicro tab). Then click on the Examples tab, expand the ‘MPU6050’ section and then select the MPU6050_DMP6 example. This will load the code into the edit window.
Assuming you have the wiring setup correct, the example should run ‘out of the box’ with no required modifications. However, after verifying that everything was working, I made the following changes:
- The unmodified MPU6050_6Axis_MotionApps20.h file configures the MPU6050 DMP to send data packets to the controller at a fairly high rate – like 100Hz. This is way too high for my robot application, so I changed the configuration to send packets at a 10Hz rate, by changing the MPU6050_DMP_FIFO_RATE_DIVISOR constant from 0x01 to 0x09 (lines 271-274) as shown below
-
1234#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR//#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x09 //09/29/19 reduce FIFO rate to 20Hz vs 100Hz#endif
- The Arduino I2C library (Wire.h) has a well-known and documented flaw that causes the I2C bus to hang up on an intermittent basis, so I modified I2CDev.h lines 50-57 to use the SBWIRE library that contains timeouts to prevent this problem from happening
|
1 2 3 4 5 6 7 8 |
// ----------------------------------------------------------------------------- // I2C interface implementation setting // ----------------------------------------------------------------------------- #ifndef I2CDEV_IMPLEMENTATION //#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE #define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE //09/29/19 gfp enabled this to avoid I2C hangups //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE #endif // I2CDEV_IMPLEMENTATION |
And the last change I made was to disable the interrupt service routine (ISR) and use a polling technique. Instead of waiting for an interrupt, I simply poll the DMP register with
‘mpuIntStatus = mpu.getIntStatus();’
every time through the loop. If the return value indicates that a data packet is ready, it is read; otherwise it does nothing. This appears to be entirely equivalent to the interrupt technique as long as the loop is fast enough service the DMP’s FIFO.
30 September Update:
Well, something’s not equivalent, as the yaw values are fine for a few minutes, but then start showing up as ‘179.000’. From my previous work I know this means that the line
mpu.getFIFOBytes(fifoBuffer, packetSize);
is getting out of sync with the DMP and isn’t reading a complete packet. When I then changed the code back to the original interrupt-driven model, the yaw values stay valid forever.
03 October Update:
I modified the code to break the ‘put other programming stuff here’ block out of the ‘if()’ within a ‘while()’ within a ‘loop()’ structure for two reasons:
- It gave me a headache every time I tried to figure out how it worked
- I wanted to do ‘the programming stuff’ only once every K Msec where K was something like 100 or 200. With the above nested structure, that would never work.
After removing extraneous comments and unused code, the resulting program is shown below:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 |
/* Name: Arduino_GY521_Test.ino Created: 9/28/2019 6:38:12 PM Author: FRANKWIN10\Frank */ #include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #include "elapsedMillis.h" // class default I2C address is 0x68 // specific I2C addresses may be passed as a parameter here // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) // AD0 high = 0x69 MPU6050 mpu; //MPU6050 mpu(0x69); // <-- use for AD0 high /* ========================================================================= NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch depends on the MPU-6050's INT pin being connected to the Arduino's external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is digital I/O pin 2. * ========================================================================= */ #define OUTPUT_READABLE_YAWPITCHROLL #define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) bool blinkState = false; // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector //extra stuff const int GET_INT_STATUS_PIN = 53; const int READ_BYTES_SECTION_PIN = 51; int IMU_CHECK_INTERVAL_MSEC = 100; elapsedMillis sinceLastIMUCheck; float global_yawval = 0.0; //contains most recent yaw value from IMU int global_fifo_count = 0; // ================================================================ // === INITIAL SETUP === // ================================================================ void setup() { // I2C bus init done in SBWIRE.h Serial.begin(115200); while (!Serial); // wait for Leonardo enumeration, others continue immediately // initialize device Serial.println(F("Initializing MPU6050...")); mpu.initialize(); pinMode(INTERRUPT_PIN, INPUT); // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); // load and configure the DMP Serial.println(F("Initializing DMP...")); devStatus = mpu.dmpInitialize(); // supply your own gyro offsets here, scaled for min sensitivity mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset(1788); // 1688 factory default for my test chip // make sure it worked (returns 0 if so) if (devStatus == 0) { // Calibration Time: generate offsets and calibrate our MPU6050 mpu.CalibrateAccel(6); mpu.CalibrateGyro(6); mpu.PrintActiveOffsets(); // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); // enable Arduino interrupt detection Serial.print(F("Enabling interrupt detection (Arduino external interrupt ")); Serial.print(digitalPinToInterrupt(INTERRUPT_PIN)); Serial.println(F(")...")); attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) Serial.print(F("DMP Initialization failed (code ")); Serial.print(devStatus); Serial.println(F(")")); } // configure LED for output pinMode(LED_PIN, OUTPUT); sinceLastIMUCheck = 0; //this manages 'other program stuff' cycle //diagnostic pins - not needed for normal operation pinMode(GET_INT_STATUS_PIN, OUTPUT); pinMode(READ_BYTES_SECTION_PIN, OUTPUT); //print out column headers Serial.println("MSec\tYawDeg\tGlFifoCnt\tFifoCnt"); } // ================================================================ // === INTERRUPT DETECTION ROUTINE === // ================================================================ volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high void dmpDataReady() { mpuInterrupt = true; } // ================================================================ // === MAIN PROGRAM LOOP === // ================================================================ void loop() { // if programming failed, don't try to do anything if (!dmpReady) return; //if (mpuInterrupt && fifoCount < packetSize) if (mpuInterrupt) { mpuInterrupt = false;//reset interrupt flag global_yawval = GetIMUHeadingDeg(); //retreive the most current yaw value from IMU // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } //other program stuff block - executes every IMU_CHECK_INTERVAL_MSEC Msec //for this test program, there's nothing here except diagnostics printouts if (sinceLastIMUCheck >= IMU_CHECK_INTERVAL_MSEC) { sinceLastIMUCheck -= IMU_CHECK_INTERVAL_MSEC; Serial.print(millis()); Serial.print("\t"); Serial.print(global_yawval); Serial.print("\t"); Serial.print(global_fifo_count); Serial.print("\t"); Serial.println(fifoCount); if (global_fifo_count != 0) { Serial.print("Program halted!"); while (1) { } } } } float GetIMUHeadingDeg() { // An interrupt has occurred. get INT_STATUS byte //pulse an output pin to measure getIntStatus() duration digitalWrite(GET_INT_STATUS_PIN, HIGH); mpuIntStatus = mpu.getIntStatus(); digitalWrite(GET_INT_STATUS_PIN, LOW); // get current FIFO count fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) { // reset so we can continue cleanly mpu.resetFIFO(); // fifoCount = mpu.getFIFOCount(); // will be zero after reset no need to ask Serial.println(F("FIFO overflow!")); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) { // read all available packets from FIFO while (fifoCount >= packetSize) // Lets catch up to NOW, in case someone is using the dreaded delay()! { mpu.getFIFOBytes(fifoBuffer, packetSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize; //digitalWrite(READ_BYTES_SECTION_PIN, !digitalRead(READ_BYTES_SECTION_PIN)); //toggle the pin //pulse the pin digitalWrite(READ_BYTES_SECTION_PIN, HIGH); delay(1); digitalWrite(READ_BYTES_SECTION_PIN, LOW); } global_fifo_count = mpu.getFIFOCount(); //should be zero here #ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); #endif } float yawval = ypr[0] * 180 / M_PI; return yawval; } |
Notes about the above program:
- I used the SBWIRE library vs the normal Arduino WIRE library to avoid the well-known and well documented infinite blocking problems in the WIRE code. This was accomplished by editing the I2C interface implementation section in I2Cdev.h as follows
-
12345678// -----------------------------------------------------------------------------// I2C interface implementation setting// -----------------------------------------------------------------------------#ifndef I2CDEV_IMPLEMENTATION//#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE //09/29/19 gfp enabled this to avoid I2C hangups//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE#endif // I2CDEV_IMPLEMENTATION
- I lowered the MPU6050 interrupt rate to 20Hz (I don’t need anything faster for my wall-following robot by modifying MPU6050_6AxisMotionApps20.h as follows:
-
1234ifndef MPU6050_DMP_FIFO_RATE_DIVISOR//#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x09 //09/29/19 reduce FIFO rate to 20Hz vs 100Hz#endif
- The loop() function has just three blocks
- if (!dmpReady) return; this bypasses everything else if the MPU6050 didn’t init correctly
-
123456789if (mpuInterrupt){mpuInterrupt = false;//reset interrupt flagglobal_yawval = GetIMUHeadingDeg(); //retreive the most current yaw value from IMU// blink LED to indicate activityblinkState = !blinkState;digitalWrite(LED_PIN, blinkState);}
All this section does is call GetIMUHeadingDeg() whenever an interrupt has been processed in the ISR -
12345678910111213141516171819202122//other program stuff block - executes every IMU_CHECK_INTERVAL_MSEC Msec//for this test program, there's nothing here except diagnostics printoutsif (sinceLastIMUCheck >= IMU_CHECK_INTERVAL_MSEC){sinceLastIMUCheck -= IMU_CHECK_INTERVAL_MSEC;Serial.print(millis());Serial.print("\t");Serial.print(global_yawval);Serial.print("\t");Serial.print(global_fifo_count);Serial.print("\t");Serial.println(fifoCount);if (global_fifo_count != 0){Serial.print("Program halted!");while (1){}}}
This section is the ‘everything else’ block. In my robot programs, this section runs the robot, using the yaw value output from the MPU6050 as appropriate.
- I discovered that the local variable ‘fifoCount’ can become desynchronized from the actual FIFO count resulting in a situation where the line:
if (mpuInterrupt && fifoCount < packetSize)
in the loop() function fails with fifoCount == packetSize. The fix for this was to remove the fifoCount comparison from the if() statement, making it just ‘if (mpuInterrupt)’. This means the if() block will execute every time the interrupt occurs, whether or not there is data in the FIFO.
With the above modifications, the program has run for many hours with no problems, so I’m convinced I have most, if not all, the problems licked. I’m still using the interrupt-driven version rather than the polling version I would prefer, but that’s a small price to pay for the demonstrated stability of the interrupt-driven version.
Future Work:
Next I plan to try the new MotionDriver 6.12 version of the MPU6050 DMP firmware, which is reputed to be faster, better, and more stable than the present 2.0 version.
04 October Update.
As it happens, the only thing that was required to change from MotionApps V2 to MotionApps V6.12 was to change #include “MPU6050_6Axis_MotionApps20.h” to #include “MPU6050_6Axis_MotionApps_V6_12.h” in little test program. This compiled and ran fine, and the only difference I could see is that V6.12 has a fixed interrupt rate of about 200Hz, whereas V2.0 could be adjusted down to about 20Hz. According to some Invensense documentation, the newer version has better/faster calibration capabilities and (maybe?) lower drift rates??
Stay Tuned
Frank
Pingback: Polling vs Interrupt with MPU6050 (GY-521) and Arduino - Paynter's Palace
Pingback: Heading-based Turns Using MPU6050 and Polling vs Interrupts - Paynter's Palace