Posted 17 November 2021,
This post describes my efforts, once again, to figure out how to get a Teensy running with Jeff Rowberg’s I2CDev library for the purpose of interfacing to a MPU6050 6-axis IMU with on-board DMP.
After some initial teething problems, I have been using the MPU6050 for years on my various autonomous wall-following robots using Arduino Mega 2560 controllers with great success. However, when I tried switching over from Arduino UNO/Mega controllers to the Teensy 3.x controller, Jeff Rowberg’s library wouldn’t compile at all. At the time I got the library to compile by replacing #include Wire.h with #include i2c_t3.h in i2cdev.h/cpp. See this post from almost two years (December 2019) ago for the gory details.
Unfortunately, two years later when I tried to change out the Mega 2560 for a Teensy 3.5 in Wall-E3 my latest autonomous wall-following robot, I ran into compile problems again – rats! Looking at the code, I saw that the maintainers had accepted my pull request to add a ‘I2CDEV_TEENSY_3X_WIRE’ identifier to I2Cdev.h to switch from <Wire.h> to <i2c_t3.h> as I had done previously, but I still couldn’t get the library to compile with a Teensy 3.x target.
So, back to basics. Fortunately, I had kept all the old versions of the required i2cdevlib files, so my two-year old project (Teensy_MPU6050_DMP6_V2.ino) still compiled and ran properly – whew. So now to figure out why it doesn’t compile with the newest version of the libraries.
So I created a new VS2019 Arduino project called Tensy_MPU6050_DMP6_V3, copied my code and other files from two years ago, and trimmed the program down to just the code required to print out MPU6050 headings every 200 mSec.
|
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 |
/* Name: Teensy_MPU6050_DMP6_V3.ino Created: 11/17/2021 */ #include "MPU6050_6Axis_MotionApps_V6_12.h" elapsedMillis sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec MPU6050 mpu; //uses default I2C address 0x68 #pragma region MPU6050_SUPPORT uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine 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 int GetPacketLoopCount = 0; int OuterGetPacketLoopCount = 0; //RTC/FRAM/MPU6050 status flags bool bMPU6050Ready = true; bool dmpReady = false; // set true if DMP init was successful volatile float global_yawval = 0; //updated by GetIMUHeadingDeg() const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket() uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param bool bFirstTime = true; #pragma endregion MPU6050 Support const int NAV_UPDATE_INTERVAL_MSEC = 200; void setup() { Serial.begin(115200); delay(3000); Serial.println("In Setup"); Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000); Wire.setDefaultTimeout(200000); // 200ms // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); Serial.println(F("Initializing DMP...")); devStatus = mpu.dmpInitialize(); // supply your own gyro offsets here, scaled for min sensitivity //12/03/19 settings from calibration routines mpu.setXGyroOffset(58); mpu.setYGyroOffset(20); mpu.setZGyroOffset(12); mpu.setXAccelOffset(1618); mpu.setYAccelOffset(3046); mpu.setZAccelOffset(4554); // make sure it worked (returns 0 if successful) if (devStatus == 0) { // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); // set our DMP Ready flag so the main loop() function knows it's okay to use it Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle...")); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); bMPU6050Ready = true; Serial.printf("\nMPU6050 Ready at %2.2f Sec\n", millis() / 1000.f); } 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(")")); bMPU6050Ready = false; } sinceLastNavUpdateMsec = 0; } void loop() { if (sinceLastNavUpdateMsec >= NAV_UPDATE_INTERVAL_MSEC) { sinceLastNavUpdateMsec -= NAV_UPDATE_INTERVAL_MSEC; GetIMUHeadingDeg(); //updates global_yawval if successful Serial.printf("%lu\t%3.2f\n", millis(), global_yawval); } } bool GetIMUHeadingDeg() { //Purpose: Get latest yaw (heading) value from IMU //Inputs: None. This function should only be called after mpu.dmpPacketAvailable() returns TRUE //Outputs: // returns true if successful, otherwise false // global_yawval updated on success //Plan: //Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet //Step2: read all available packets to get to latest data //Step3: update global_yawval with latest value //Notes: // 10/08/19 changed return type to boolean // 10/08/19 no longer need mpuIntStatus // 10/21/19 completely rewritten to use Homer's algorithm bool retval = false; int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow { // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //compute the yaw value global_yawval = ypr[0] * 180 / M_PI; retval = true; } return retval; } uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops) { max_loops = max(10, max_loops); //added 12/04/19 to ensure we loop at least 10 times. This really IS a hard-coded number! fifoCount = mpu.getFIFOCount(); GetPacketLoopCount = 0; //11/10/19 I have seen this fail with fifoC > 28 on occasion, so now I loop max three times OuterGetPacketLoopCount = 0; while (fifoCount != packetSize && OuterGetPacketLoopCount <= 3) { mpu.resetFIFO(); delay(1); fifoCount = mpu.getFIFOCount(); GetPacketLoopCount = 0; while (fifoCount < packetSize && GetPacketLoopCount < max_loops) { GetPacketLoopCount++; fifoCount = mpu.getFIFOCount(); delay(2); } //if we get to here, there should be exactly one packet in the FIFO OuterGetPacketLoopCount++; } if (OuterGetPacketLoopCount < 3) { mpu.getFIFOBytes(data, packetSize); return 1; } return 0; //failed to get a good packet } |
And here is a sample of the output:
|
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 |
Testing device connections... getDeviceID() returns 0X34 MPU6050 connection successful Initializing DMP... Enabling DMP... DMP ready! Waiting for MPU6050 drift rate to settle... MPU6050 Ready at 3.77 Sec 3972 -0.71 4172 -1.87 4372 -3.10 4572 -4.24 4772 -5.21 4972 -6.03 5172 -6.72 5372 -7.30 5572 -7.81 5772 -8.28 5972 -8.70 6172 -9.09 6372 -9.48 6572 -9.86 6775 -10.23 6975 -10.59 7175 -10.94 7375 -11.29 7575 -11.66 7775 -12.01 |
This program, using the i2cdevlib files from two years ago, compiles and runs fine. The things that have changed in i2cdevlib since then. Comparing i2cdev.h as used in this program with the current i2dev.h, I see
And the difference in the i2Cdev.cpp files:
In the i2Cdev class methods that actually talk to the MPU6050 over the i2c bus (readBytes, readWords, writeBytes, writeWords) the new version uses a pointer to a ‘TwoWire’ object called ‘useWire’ instead of Wire. The ‘useWire’ object is defined at the top of each of these methods with the lines
|
1 2 3 4 |
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE TwoWire *useWire = &Wire; if (wireObj) useWire = (TwoWire *)wireObj; #endif |
but the declaration (invocation?) of the ‘TwoWire’ class is guarded by an #ifdef as shown
Which means it isn’t included if ‘I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE’ is used. In my old version of I2Cdev.cpp, this didn’t matter, because all the class method declarations & definitions use the ‘Wire.’ object technique for invoking a ‘i2c_t3’ class method. However, the new code uses the ‘useWire->’ pointer technique which does require the ‘TwoWire’ declaration, only it’s not available for ‘I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE’ – ouch!
Comparing the old and new MPU6050.h files, the only changes of significance are:
- The name of the class was changed from MPU6050 in the old file to MPU6050_Base in the new one, with ‘, void * wireObj = 0’ added as the last parameter in the constructor
- GetCurrentFIFOPacket, setFIFOTimeout, and getFIFOTimeout methods were added
- The entire #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 section was removed
- void *wireObj and uint_32t fifoTimeout objects were defined
The new GetCurrentFIFOPacket method added by Homer Creutz replaces the inline method I used in my older code, and does a nicer job to boot, and the wireObj declaration makes possible changing all the method calls from ‘Wire.’ to ‘useWire_>’ style.
Comparing the old and new MPU6050.cpp files, the only changes of significance are:
- The names for all method implementations are changed from MPU6050:: to MPU6050_Base::, with a ‘,wireObj(wireObj)’ parameter added at the end of the constructor method and most other methods as well
- The implementation code for GetCurrentFIFOPacket, setFIFOTimeout, and getFIFOTimeout methods were added
Comparing MPU6050_6Axis_MotionApps_V6_12.h (the old version) to MPU6050_6Axis_MotionApps612.h (the new version), there were a lot of changes:
- The implementation code was split out from the header file into a new MPU6050_6Axis_MotionApps612.cpp file
- A new MPU6050_6Axis_MotionApps612 : public MPU6050_Base class is declared and all the dmpxxx methods are now part of this class.
- All the hard-coded MPU6050 DMP firmware image is gone (in the cpp file?)
- At the very bottom is the line ‘typedef MPU6050_6Axis_MotionApps612 MPU6050;’ declaring ‘MPU6050’ to be a object of type MPU6050_6Axis_MotionApps612
The new MPU6050_6Axis_MotionApps612.cpp file now contains the DMP firmware image and the definitions for all the methods declared in the .h file.
So, it looks like the whole problem with changing over from the old setup to the new one may be just the fact that the declaration (invocation?) of the ‘TwoWire’ class is guarded by an #ifdef that disables the line for ‘I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE’. So tomorrow (neeeeeeedddddd, sleeeeepppp!) I’ll try again with the new setup, with the additional condition added to the #ifdef line.
So, I created yet another VS2019 Arduino project called ‘Teensy_MPU6050_DMP6_V3’, identical in every way to ‘Teensy_MPU6050_DMP6_V2’ except this project includes MPU6050_6Axis_MotionApps612.h instead of MPU6050_6Axis_MotionApps_V6_12.h. To make things simpler, I copied all the reference files from the i2cdevlib folder into the project’s local folder, as shown below:
After editing I2Cdev.h as shown below
|
1 2 3 4 5 6 |
#ifndef I2CDEV_IMPLEMENTATION //#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE #define I2CDEV_IMPLEMENTATION I2CDEV_TEENSY_3X_WIRE //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE #endif // I2CDEV_IMPLEMENTATION |
to utilize the i2c_t3.h version of the Wire library, I compiled it for Teensy 3.2 and got the following output:
|
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 |
Compiling 'Teensy_MPU6050_DMP6_V3' for 'Teensy 3.2 / 3.1' I2Cdev.cpp: In static member function static int8_t I2Cdev::readBytes(uint8_t, uint8_t, uint8_t, uint8_t*, uint16_t, void*) I2Cdev.cpp: 224:9: error: 'TwoWire' was not declared in this scope TwoWire *useWire = &Wire Error compiling project sources Build failed for project 'Teensy_MPU6050_DMP6_V3' I2Cdev.cpp: 224:18: error: 'useWire' was not declared in this scope TwoWire *useWire = &Wire I2Cdev.cpp: 225:42: error: expected primary-expression before ')' token if (wireObj) useWire = (TwoWire *)wireObj I2Cdev.cpp: In static member function static int8_t I2Cdev::readWords(uint8_t, uint8_t, uint8_t, uint16_t*, uint16_t, void*) I2Cdev.cpp: 346:9: error: 'TwoWire' was not declared in this scope TwoWire *useWire = &Wire I2Cdev.cpp: 346:18: error: 'useWire' was not declared in this scope TwoWire *useWire = &Wire I2Cdev.cpp: 347:42: error: expected primary-expression before ')' token if (wireObj) useWire = (TwoWire *)wireObj I2Cdev.cpp: In static member function static bool I2Cdev::writeBytes(uint8_t, uint8_t, uint8_t, uint8_t*, void*) I2Cdev.cpp: 603:5: error: 'TwoWire' was not declared in this scope TwoWire *useWire = &Wire I2Cdev.cpp: 603:14: error: 'useWire' was not declared in this scope TwoWire *useWire = &Wire I2Cdev.cpp: 604:38: error: expected primary-expression before ')' token if (wireObj) useWire = (TwoWire *)wireObj I2Cdev.cpp: In static member function static bool I2Cdev::writeWords(uint8_t, uint8_t, uint8_t, uint16_t*, void*) I2Cdev.cpp: 670:5: error: 'TwoWire' was not declared in this scope TwoWire *useWire = &Wire I2Cdev.cpp: 670:14: error: 'useWire' was not declared in this scope TwoWire *useWire = &Wire I2Cdev.cpp: 671:38: error: expected primary-expression before ')' token if (wireObj) useWire = (TwoWire *)wireObj |
As shown, all the compile errors reference a missing declaration for TwoWire, which I hope means that adding the ‘I2CDEV_TEENSY_3X_WIRE’ identifier to the guard around the code that declares TwoWire in i2Cdev.cpp.
I edited i2Cdev.cpp to add the ‘I2CDEV_TEENSY_3X_WIRE’ identifier to the guard around the code that declares TwoWire in I2Cdev.cpp, and edited I2CDev.h to do the same thing for the guard around the ‘class TwoWire’ declaration, and recompiled. This time the output was:
|
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 |
Compiling 'Teensy_MPU6050_DMP6_V3' for 'Teensy 3.2 / 3.1' MPU6050.h:41: In file included from MPU6050_6Axis_MotionApps612.h:44: from Teensy_MPU6050_DMP6_V3.ino:12: from I2Cdev.h: 308:20: error: conflicting declaration 'TwoWire Wire extern TwoWire Wire I2Cdev.h:97: In file included from MPU6050.h:41: from MPU6050_6Axis_MotionApps612.h:44: from Teensy_MPU6050_DMP6_V3.ino:12: from i2c_t3.h:998: note previous declaration as i2c_t3 Wire extern i2c_t3 Wire I2Cdev.cpp: 82:10: warning: #warning Using I2CDEV_BUILTIN_NBWIRE implementation may adversely affect interrupt detection. [-Wcpp] #warning Using I2CDEV_BUILTIN_NBWIRE implementation may adversely affect interrupt detection I2Cdev.cpp: 83:10: warning: #warning This I2Cdev implementation does not support: [-Wcpp] #warning This I2Cdev implementation does not support I2Cdev.cpp: 84:10: warning: #warning - Repeated starts conditions [-Wcpp] #warning - Repeated starts conditions I2Cdev.cpp:49: In file included from I2Cdev.h: 308:20: error: conflicting declaration 'TwoWire Wire extern TwoWire Wire I2Cdev.h:97: In file included from I2Cdev.cpp:49: from i2c_t3.h:998: note previous declaration as i2c_t3 Wire Error compiling project sources extern i2c_t3 Wire Build failed for project 'Teensy_MPU6050_DMP6_V3' I2Cdev.cpp: 90:13: error: conflicting declaration 'TwoWire Wire TwoWire Wire I2Cdev.h:97: In file included from I2Cdev.cpp:49: from i2c_t3.h:998: note previous declaration as i2c_t3 Wire extern i2c_t3 Wire I2Cdev.cpp: In static member function static int8_t I2Cdev::readBytes(uint8_t, uint8_t, uint8_t, uint8_t*, uint16_t, void*) I2Cdev.cpp: 225:29: error: cannot convert 'i2c_t3*' to 'TwoWire*' in initialization TwoWire *useWire = &Wire I2Cdev.cpp: 284:26: error: 'class TwoWire' has no member named 'write useWire->write(regAddr) I2Cdev.cpp: 290:44: error: 'class TwoWire' has no member named 'read data[count] = useWire->read() I2Cdev.cpp: In static member function static int8_t I2Cdev::readWords(uint8_t, uint8_t, uint8_t, uint16_t*, uint16_t, void*) I2Cdev.cpp: 347:29: error: cannot convert 'i2c_t3*' to 'TwoWire*' in initialization TwoWire *useWire = &Wire I2Cdev.cpp: 424:26: error: 'class TwoWire' has no member named 'write useWire->write(regAddr) I2Cdev.cpp: 433:48: error: 'class TwoWire' has no member named 'read data[count] = useWire->read() << 8 I2Cdev.cpp: 436:49: error: 'class TwoWire' has no member named 'read data[count] |= useWire->read() I2Cdev.cpp: In static member function static bool I2Cdev::writeBytes(uint8_t, uint8_t, uint8_t, uint8_t*, void*) I2Cdev.cpp: 604:25: error: cannot convert 'i2c_t3*' to 'TwoWire*' in initialization TwoWire *useWire = &Wire I2Cdev.cpp: 615:18: error: 'class TwoWire' has no member named 'write useWire->write((uint8_t) regAddr); \\ send address I2Cdev.cpp: 630:22: error: 'class TwoWire' has no member named 'write useWire->write((uint8_t) data[i]) I2Cdev.cpp: In static member function static bool I2Cdev::writeWords(uint8_t, uint8_t, uint8_t, uint16_t*, void*) I2Cdev.cpp: 671:25: error: cannot convert 'i2c_t3*' to 'TwoWire*' in initialization TwoWire *useWire = &Wire I2Cdev.cpp: 682:18: error: 'class TwoWire' has no member named 'write useWire->write(regAddr); \\ send address I2Cdev.cpp: 698:22: error: 'class TwoWire' has no member named 'write useWire->write((uint8_t)(data[i] >> 8)); \\ send MSB I2Cdev.cpp: 699:22: error: 'class TwoWire' has no member named 'write useWire->write((uint8_t)data[i]); \\ send LSB |
So I commented out the ‘
|
1 |
//extern TwoWire Wire; |
line and recompiled, still getting way too many errors – time to punt.
20 November 2021 Update:
As mentioned above, I decided to punt on the idea of incorporating the newest I2CDevlib files, as I keep getting lost trying to sort out the ‘Wire/TwoWire/useWire’ mess.
However, I did discover that the new library versions work just fine with an Arduino UNO or MEGA2560 as the target. So, I created an Arduino project called ‘Arduino_MPU6050_6Axis_MotionApps612’ and used it to tune the rate-controlled turn PID for the new robot. After the requisite number of false-starts and screwups, I was able to get pretty decent 45º/sec and 90º/sec turns with:
|
1 2 3 4 5 |
//11/16/21 almost perfect for 45 deg/sec, 100mSec //11/17/21 and pretty good for 90 deg/sec too! double TurnRate_Kp = 1.8; double TurnRate_Ki = 1.0; double TurnRate_Kd = 0.4; |
So, at this point I have an Arduino program ( Arduino_MPU6050_6Axis_MotionApps612) targeting the Arduino UNO that works fine with the new versions of the i2cdevlib libraries, and an older Arduino program (Teensy_MPU6050_DMP6_V3) targeting the Teensy 3.x that works fine with the 2-year old versions of the i2cdevlib libraries. The next step I think is to move ahead with the Teensy-targeted program using old libraries on the assumption that eventually Jeff Rowberg and company will either fix the current libraries to work properly with Teensy, or show me the error of my ways so that I can fix the Teensy compilation problem.
Pingback: Wall-E3 – Different Form-factor for Better Turn Performance | Paynter's Palace