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

--- 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;

--- 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);        

--- 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
     }
 

--- 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);

--- 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

--- /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

--- /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

--- /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

--- /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