Chris Seto
/
MPU6050-DMP-4
MPU6050 issues
Fork of MPU6050 by
Revision 6:6300d9561dfd, committed 2014-07-01
- Comitter:
- chris1seto
- Date:
- Tue Jul 01 16:56:00 2014 +0000
- Parent:
- 5:7d1bf3ce0053
- Commit message:
- Initial MPU6050 issues
Changed in this revision
diff -r 7d1bf3ce0053 -r 6300d9561dfd I2Cdev.cpp --- a/I2Cdev.cpp Sat Nov 23 16:47:00 2013 +0000 +++ b/I2Cdev.cpp Tue Jul 01 16:56:00 2014 +0000 @@ -7,12 +7,12 @@ #define useDebugSerial -I2Cdev::I2Cdev(): i2c(I2C_SDA,I2C_SCL), debugSerial(USBTX, USBRX) +I2Cdev::I2Cdev(): i2c(I2C_SDA,I2C_SCL) { } -I2Cdev::I2Cdev(PinName i2cSda, PinName i2cScl): i2c(i2cSda,i2cScl), debugSerial(USBTX, USBRX) +I2Cdev::I2Cdev(PinName i2cSda, PinName i2cScl): i2c(i2cSda,i2cScl) { } @@ -40,8 +40,9 @@ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ -int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { - uint16_t b; +int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) +{ + uint16_t b = 0; uint8_t count = readWord(devAddr, regAddr, &b, timeout); *data = b & (1 << bitNum); return count;
diff -r 7d1bf3ce0053 -r 6300d9561dfd I2Cdev.h --- a/I2Cdev.h Sat Nov 23 16:47:00 2013 +0000 +++ b/I2Cdev.h Tue Jul 01 16:56:00 2014 +0000 @@ -9,13 +9,12 @@ #include "mbed.h" -#define I2C_SDA p28 -#define I2C_SCL p27 +#define I2C_SDA PB_9 +#define I2C_SCL PB_8 class I2Cdev { private: I2C i2c; - Serial debugSerial; public: I2Cdev(); I2Cdev(PinName i2cSda, PinName i2cScl);
diff -r 7d1bf3ce0053 -r 6300d9561dfd MPU6050.cpp --- a/MPU6050.cpp Sat Nov 23 16:47:00 2013 +0000 +++ b/MPU6050.cpp Tue Jul 01 16:56:00 2014 +0000 @@ -41,9 +41,11 @@ */ #include "MPU6050.h" +#include "shared.h" #define useDebugSerial + //instead of using pgmspace.h typedef const unsigned char prog_uchar; #define pgm_read_byte_near(x) (*(prog_uchar*)(x))//<- I modified here @@ -52,7 +54,7 @@ /** Default constructor, uses default I2C address. * @see MPU6050_DEFAULT_ADDRESS */ -MPU6050::MPU6050() : debugSerial(USBTX, USBRX) +MPU6050::MPU6050() { devAddr = MPU6050_DEFAULT_ADDRESS; } @@ -63,7 +65,7 @@ * @see MPU6050_ADDRESS_AD0_LOW * @see MPU6050_ADDRESS_AD0_HIGH */ -MPU6050::MPU6050(uint8_t address) : debugSerial(USBTX, USBRX) +MPU6050::MPU6050(uint8_t address) { devAddr = address; } @@ -79,7 +81,7 @@ { #ifdef useDebugSerial - debugSerial.printf("MPU6050::initialize start\n"); + pc.printf("MPU6050::initialize start\r\n"); #endif setClockSource(MPU6050_CLOCK_PLL_XGYRO); @@ -88,7 +90,7 @@ setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! #ifdef useDebugSerial - debugSerial.printf("MPU6050::initialize end\n"); + pc.printf("MPU6050::initialize end\r\n"); #endif } @@ -99,11 +101,11 @@ bool MPU6050::testConnection() { #ifdef useDebugSerial - debugSerial.printf("MPU6050::testConnection start\n"); + pc.printf("MPU6050::testConnection start\r\n"); #endif uint8_t deviceId = getDeviceID(); #ifdef useDebugSerial - debugSerial.printf("DeviceId = %d\n",deviceId); + pc.printf("DeviceId = %d\r\n",deviceId); #endif return deviceId == 0x34; } @@ -3263,22 +3265,49 @@ uint8_t *progBuffer = NULL; uint16_t i; uint8_t j; - if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); - if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); - for (i = 0; i < dataSize;) { + + if (verify) + { + verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + pc.printf("Verify\r\n"); + } + + if (useProgMem) + { + progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + pc.printf("Prog mem\r\n"); + } + + for (i = 0; i < dataSize;) + { // determine correct chunk size according to bank position and data size chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; // make sure we don't go past the data size - if (i + chunkSize > dataSize) chunkSize = dataSize - i; + if (i + chunkSize > dataSize) + { + chunkSize = dataSize - i; + } // make sure this chunk doesn't go past the bank boundary (256 bytes) - if (chunkSize > 256 - address) chunkSize = 256 - address; + if (chunkSize > 256 - address) + { + chunkSize = 256 - address; + } + + pc.printf("Chunksize: %d\r\n", chunkSize); - if (useProgMem) { + if (useProgMem) + { // write the chunk of data as specified - for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j); - } else { + for (j = 0; j < chunkSize; j++) + { + progBuffer[j] = pgm_read_byte(data + i + j); + pc.printf("%d, ", progBuffer[j]); + } + } + else + { // write the chunk of data as specified progBuffer = (uint8_t *)data + i; } @@ -3286,11 +3315,14 @@ i2Cdev.writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer); // verify data if needed - if (verify && verifyBuffer) { + if (verify && verifyBuffer) + { setMemoryBank(bank); setMemoryStartAddress(address); i2Cdev.readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer); - if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) { + + if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) + { /*Serial.print("Block write verification error, bank "); Serial.print(bank, DEC); Serial.print(", address "); @@ -3340,7 +3372,9 @@ uint8_t success, special; uint8_t *progBuffer = NULL; uint16_t i, j; - if (useProgMem) { + + if (useProgMem) + { progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary }
diff -r 7d1bf3ce0053 -r 6300d9561dfd MPU6050.h --- a/MPU6050.h Sat Nov 23 16:47:00 2013 +0000 +++ b/MPU6050.h Tue Jul 01 16:56:00 2014 +0000 @@ -45,6 +45,7 @@ #include "I2Cdev.h" #include "helper_3dmath.h" +//#include "shared.h" #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) @@ -417,7 +418,6 @@ class MPU6050 { private: I2Cdev i2Cdev; - Serial debugSerial; public: MPU6050(); MPU6050(uint8_t address);
diff -r 7d1bf3ce0053 -r 6300d9561dfd MPU6050_6Axis_MotionApps20.h --- a/MPU6050_6Axis_MotionApps20.h Sat Nov 23 16:47:00 2013 +0000 +++ b/MPU6050_6Axis_MotionApps20.h Tue Jul 01 16:56:00 2014 +0000 @@ -33,7 +33,7 @@ #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ #define _MPU6050_6AXIS_MOTIONAPPS20_H_ -#include <iostream> +#include "shared.h" #include "I2Cdev.h" #include "helper_3dmath.h" @@ -102,18 +102,18 @@ // after moving string constants to flash memory storage using the F() // compiler macro (Arduino IDE 1.0+ required). -//#define DEBUG +#define DEBUG #ifdef DEBUG - #define DEBUG_PRINT(x) std::cout << x //Serial.print(x) - #define DEBUG_PRINTF(x, y) std::cout << x //Serial.print(x, y) - #define DEBUG_PRINTLN(x) std::cout << x << std::endl //Serial.println(x) - #define DEBUG_PRINTLNF(x, y) std::cout << x << std::endl //Serial.println(x, y) + #define DEBUG_PRINT(x) pc.printf(" %s \r\n", x); + #define DEBUG_PRINTF(x, y) pc.printf(" %s \r\n", x); + #define DEBUG_PRINTLN(x) pc.printf(" %s \r\n", x); + #define DEBUG_PRINTLNF(x, y) pc.printf(" %s \r\n", x); #define F(x) x #else - #define DEBUG_PRINT(x) - #define DEBUG_PRINTF(x, y) - #define DEBUG_PRINTLN(x) - #define DEBUG_PRINTLNF(x, y) + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) #endif #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] @@ -349,8 +349,7 @@ setMemoryStartAddress(0x06); DEBUG_PRINTLN(F("Checking hardware revision...")); uint8_t hwRevision = readMemoryByte(); - DEBUG_PRINT(F("Revision @ user[16][6] = ")); - DEBUG_PRINTLNF(hwRevision, HEX); + pc.printf("Revision @ user[16][6] = %d\r\n", hwRevision); DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); setMemoryBank(0, false, false); @@ -386,9 +385,10 @@ // load DMP code into memory banks DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); - DEBUG_PRINTLN(F(" bytes)")); - if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { + pc.printf("%d bytes)\r\n", MPU6050_DMP_CODE_SIZE); + + if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) + { DEBUG_PRINTLN(F("Success! DMP code written and verified.")); // write DMP configuration
diff -r 7d1bf3ce0053 -r 6300d9561dfd main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Jul 01 16:56:00 2014 +0000 @@ -0,0 +1,123 @@ +#include "mbed.h" +#include "I2Cdev.h" +#include "MPU6050_6Axis_MotionApps20.h" +#include "shared.h" + +MPU6050 mpu; + +// 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 + +volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high +void dmpDataReady() +{ + mpuInterrupt = true; +} + +#ifndef M_PI +#define M_PI 3.1415 +#endif + +I2C i2c(); + +int main() +{ + pc.baud(115200); + + mpu.initialize(); + + // verify connection + pc.printf("Testing device connections...\r\n"); + + bool mpu6050TestResult = mpu.testConnection(); + if(mpu6050TestResult) + { + pc.printf("MPU6050 test passed \r\n"); + } + else + { + pc.printf("MPU6050 test failed \r\n"); + } + + // load and configure the DMP + pc.printf("Initializing DMP...\n"); + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + mpu.setXGyroOffset(0); + mpu.setYGyroOffset(0); + mpu.setZGyroOffset(0); + mpu.setZAccelOffset(16282); // 1688 factory default for my test chip + + if (devStatus==0) + { + pc.printf("OK!\r\n"); + mpu.setDMPEnabled(true); + + mpuIntStatus = mpu.getIntStatus(); + + packetSize = mpu.dmpGetFIFOPacketSize(); + } + else + { + pc.printf("Failed with code %d\r\n", devStatus); + while(1); + } + + // Main prog loop + while (1) + { + wait_us(500); + + mpuIntStatus = mpu.getIntStatus(); + + // get current FIFO count + fifoCount = mpu.getFIFOCount(); + + // check for overflow (this should never happen unless our code is too inefficient) + if ((mpuIntStatus & 0x10) || fifoCount == 1024) + { + // reset so we can continue cleanly + mpu.resetFIFO(); + //pc.printf("FIFO overflow!"); + } + else if (mpuIntStatus & 0x02) + { + // wait for correct available data length, should be a VERY short wait + while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + + // read a packet from FIFO + 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; + + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + /*pc.printf("ypr\t"); + pc.printf("%f3.2",ypr[0] * 180/M_PI); + pc.printf("\t"); + pc.printf("%f3.2",ypr[1] * 180/M_PI); + pc.printf("\t"); + pc.printf("%f3.2\n",ypr[2] * 180/M_PI);*/ + + } + } +} \ No newline at end of file
diff -r 7d1bf3ce0053 -r 6300d9561dfd mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Jul 01 16:56:00 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/024bf7f99721 \ No newline at end of file
diff -r 7d1bf3ce0053 -r 6300d9561dfd shared.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/shared.cpp Tue Jul 01 16:56:00 2014 +0000 @@ -0,0 +1,3 @@ +#include "shared.h" + +Serial pc(SERIAL_TX, SERIAL_RX); \ No newline at end of file
diff -r 7d1bf3ce0053 -r 6300d9561dfd shared.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/shared.h Tue Jul 01 16:56:00 2014 +0000 @@ -0,0 +1,8 @@ +#ifndef SHARED_H +#define SHARED_H + +#include "mbed.h" + +extern Serial pc; + +#endif \ No newline at end of file