Chris Seto
/
MPU6050-DMP-fixpossible
Resolve STM issues
Fork of MPU6050 by
Revision 6:b272bd888e98, committed 2014-07-02
- Comitter:
- chris1seto
- Date:
- Wed Jul 02 19:38:47 2014 +0000
- Parent:
- 5:7d1bf3ce0053
- Commit message:
- inital, still having issues starting DMP
Changed in this revision
--- a/I2Cdev.cpp Sat Nov 23 16:47:00 2013 +0000 +++ b/I2Cdev.cpp Wed Jul 02 19:38:47 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) { } @@ -132,8 +132,8 @@ char command[1]; command[0] = regAddr; char *redData = (char*)malloc(length); - i2c.write(devAddr<<1, command, 1, true); - i2c.read(devAddr<<1, redData, length); + i2c.write(devAddr*2, command, 1, true); + i2c.read(devAddr*2, redData, length); for(int i =0; i < length; i++) { data[i] = redData[i]; } @@ -255,7 +255,7 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) { i2c.start(); - i2c.write(devAddr<<1); + i2c.write(devAddr*2); i2c.write(regAddr); for(int i = 0; i < length; i++) { i2c.write(data[i]);
--- a/I2Cdev.h Sat Nov 23 16:47:00 2013 +0000 +++ b/I2Cdev.h Wed Jul 02 19:38:47 2014 +0000 @@ -9,13 +9,9 @@ #include "mbed.h" -#define I2C_SDA p28 -#define I2C_SCL p27 - class I2Cdev { private: I2C i2c; - Serial debugSerial; public: I2Cdev(); I2Cdev(PinName i2cSda, PinName i2cScl);
--- a/MPU6050.cpp Sat Nov 23 16:47:00 2013 +0000 +++ b/MPU6050.cpp Wed Jul 02 19:38:47 2014 +0000 @@ -41,8 +41,9 @@ */ #include "MPU6050.h" +#include "shared.h" -#define useDebugSerial +//#define useDebugSerial //instead of using pgmspace.h typedef const unsigned char prog_uchar; @@ -52,7 +53,7 @@ /** Default constructor, uses default I2C address. * @see MPU6050_DEFAULT_ADDRESS */ -MPU6050::MPU6050() : debugSerial(USBTX, USBRX) +MPU6050::MPU6050() : i2Cdev(I2C_SDA, I2C_SCL) { devAddr = MPU6050_DEFAULT_ADDRESS; } @@ -63,7 +64,7 @@ * @see MPU6050_ADDRESS_AD0_LOW * @see MPU6050_ADDRESS_AD0_HIGH */ -MPU6050::MPU6050(uint8_t address) : debugSerial(USBTX, USBRX) +MPU6050::MPU6050(uint8_t address) : i2Cdev(I2C_SDA, I2C_SCL) { devAddr = address; } @@ -3263,27 +3264,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"); + } + + if (useProgMem) + { + progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + pc.printf("Prog mem"); + } + + 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; + + 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; } - i2Cdev.writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer); + pc.printf("ERRO!: %d\r\n", i2Cdev.writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer)); // verify data if needed if (verify && verifyBuffer) {
--- a/MPU6050.h Sat Nov 23 16:47:00 2013 +0000 +++ b/MPU6050.h Wed Jul 02 19:38:47 2014 +0000 @@ -417,7 +417,6 @@ class MPU6050 { private: I2Cdev i2Cdev; - Serial debugSerial; public: MPU6050(); MPU6050(uint8_t address);
--- a/MPU6050_6Axis_MotionApps20.h Sat Nov 23 16:47:00 2013 +0000 +++ b/MPU6050_6Axis_MotionApps20.h Wed Jul 02 19:38:47 2014 +0000 @@ -33,10 +33,9 @@ #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ #define _MPU6050_6AXIS_MOTIONAPPS20_H_ -#include <iostream> - #include "I2Cdev.h" #include "helper_3dmath.h" +#include "shared.h" // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board #define MPU6050_INCLUDE_DMP_MOTIONAPPS20 @@ -102,18 +101,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[] @@ -350,7 +349,7 @@ DEBUG_PRINTLN(F("Checking hardware revision...")); uint8_t hwRevision = readMemoryByte(); DEBUG_PRINT(F("Revision @ user[16][6] = ")); - DEBUG_PRINTLNF(hwRevision, HEX); + pc.printf("%04x\r\n", hwRevision); DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); setMemoryBank(0, false, false);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jul 02 19:38:47 2014 +0000 @@ -0,0 +1,124 @@ +#include "mbed.h" +#include "I2Cdev.h" +#include "MPU6050_6Axis_MotionApps20.h" +#include "shared.h" + +#ifndef M_PI +#define M_PI 3.1415 +#endif + +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 +int count = 0; + +int main() +{ + pc.baud(115200); + pc.printf("MPU6050 initialize \r\n"); + + mpu.initialize(); + pc.printf("MPU6050 testConnection \r\n"); + + if(mpu.testConnection()) + { + pc.printf("MPU6050 test passed \r\n"); + } + else + { + pc.printf("MPU6050 test failed \r\n"); + } + + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + mpu.setXGyroOffset(-61); + mpu.setYGyroOffset(-127); + mpu.setZGyroOffset(19); + mpu.setZAccelOffset(16282); // 1688 factory default for my test chip + + // make sure it worked (returns 0 if so) + if (devStatus == 0) + { + // turn on the DMP, now that it's ready + pc.printf("Enabling DMP...\r\n"); + mpu.setDMPEnabled(true); + + mpuIntStatus = mpu.getIntStatus(); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + pc.printf("DMP ready!\r\n"); + 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) + pc.printf("DMP Initialization failed (code %d )\r\n", devStatus); + while (1); + } + + while (1); + + while (1) + { + mpuIntStatus = mpu.getIntStatus(); + fifoCount = mpu.getFIFOCount(); + + // Check overflow + if ((mpuIntStatus & 0x10) || fifoCount == 1024) + { + // reset so we can continue cleanly + mpu.resetFIFO(); + pc.printf("FIFO overflow!\r\n"); + } + + if (mpuIntStatus & 0x02) + { + // wait for correct available data length, should be a VERY short wait + while (fifoCount < packetSize) + { + fifoCount = mpu.getFIFOCount(); + } + + mpu.getFIFOBytes(fifoBuffer, packetSize); + + fifoCount -= packetSize; + } + + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetEuler(euler, &q); + + for (int i = 0; i < 3; i++) + { + euler[i] *= 180/M_PI; + } + + if (count++ > 20) + { + pc.printf("euler\t%d\t%d\t%d\r\n", euler[0], euler[1], euler[2]); + count = 0; + } + } + +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Jul 02 19:38:47 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/04dd9b1680ae \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/shared.cpp Wed Jul 02 19:38:47 2014 +0000 @@ -0,0 +1,3 @@ +#include "shared.h" + +Serial pc(SERIAL_TX, SERIAL_RX); \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/shared.h Wed Jul 02 19:38:47 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