![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
asdfsaf
Dependencies: MPU6050-DMP-Ian mbed-rtos mbed
Revision 0:dceb852b19f3, committed 2014-05-27
- Comitter:
- majik
- Date:
- Tue May 27 09:23:23 2014 +0000
- Commit message:
- dfsasf
Changed in this revision
diff -r 000000000000 -r dceb852b19f3 DMP.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/DMP.cpp Tue May 27 09:23:23 2014 +0000 @@ -0,0 +1,205 @@ +//Copied MPU6050.h and MPU6050.cpp from another library (otherwise MPU gets stuck randomly) + +/*** THIS IS THE BEST VERSION OF DMP CODE + This code demonstrates how to access the MPU6050 IMU and fix the + connection errors that occur, using a hardware hack. + It requires two hardware fixes: A transistor (PTE5) and an extra GPIO pin (PTE23) connected + to the SDA line. + Error 1: The SDA line gets stuck on ground. + Solution 1: Use the transistor to raise the SDA voltage off of ground. This resets the I2C bus. (line 71) + Error 2: The SDA line gets stuck on 3.3V + Solution 2: Use the GPIO line to pull the SDA voltage down a bit to reset the I2C bus. (line 96) + (I am currently trying to find a software solution for the above hack) + + This code works for using the DMP. (See "DMP" for implementation that can do raw accel data) + It appears all connection issues have been fixed. + Next step: make code neater (in progress) + + LEARNED: Do not need the PTE23 line. Only fix necessary is the transistor (PTE5) +***/ + +#include "mbed.h" +#include "I2Cdev.h" +#include "MPU6050_6Axis_MotionApps20.h" +#include "robot.h" + + +//******* MPU DECLARATION THINGS *****************// +int dmp_test =1; //if this is 0, get raw MPU values + //if this is 1, get DMP values + +int16_t ax, ay, az; +int16_t gx, gy, gz; + +//******* DMP things ****************// +// MPU control/status vars +bool dmpReady = false; // set true if DMP init was succesfful +uint8_t mpuIntStatus; // holds interrupt status byte from MPU +uint8_t devStatus; // return status after each device operation (0 = success) +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 + + +uint8_t MPU_CONNECTION; + +// 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 [rad]. Multiply by 180, divide by PI for [deg] +InterruptIn checkpin(PTD7); //interrupt +//*** Interrupt Detection Routine ***// +volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high +void dmpDataReady(){ + mpuInterrupt = true; +} + +//****** END MPU DECLARATION ********************// + + +int test_dmp(); //this wraps up the code req'd to start the DMP +void start_dmp(MPU6050 mpu); //this will get the DMP ready +void update_dmp(MPU6050 mpu); //call this frequently + +//*********************FUNCTIONS***************************************************************// +int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem + /// This function tests the I2C port for the MPU6050 and resets it if it is not working + //NOTE: when the I2C connection is not successful, calling initialize() will cause the board to freeze + DigitalOut reset_pin(PTE5); //Transistor to pull up SDA line + reset_pin = 0; //start the transistor in OFF mode + //int count = 0; + DigitalIn sample_1(PTE0); //(this pin will be used as the SDA pin when an MPU6050 object is declared + int temp = sample_1; //Sample what is happening on the SDA line to guess what the solution is. + myled = 1; //turn OFF LED. This will turn on again when the MPU is connected. + //bt.baud(baudRate); + DigitalOut reset_1(PTE0);//change the SDA pin to output. + reset_1 = 0; + if(temp == 1){ //if SDA line starts high, unstick by pulling it low + bt.printf("\n\rSetting up I2C connection..."); + reset_1 = 0; //SDA pin = low + wait_ms(100); + MPU6050 *mpu2 = new MPU6050; //create a temporary MPU connection for testing + mpu2->reset(); + //bt.baud(baudRate); + while(mpu2->testConnection() == 0){ + delete mpu2; //delete mpu2 so I can use PTE0 as a GPIO again + bt.baud(baudRate); //you must reset baud rate whenever a new MPU6050 is declared + DigitalOut sda_pinx(PTE0); + sda_pinx = 0; //set this to 0 to pull the SDA line down from 3.3V + reset_pin = 0; //make sure transistor = OFF + bt.printf("\n\rRetry"); + wait_ms(50); + MPU6050 *mpu2 = new MPU6050; + wait_ms(500); + if(mpu2->testConnection() == 0) + { + reset_pin = 1; + wait_ms(50); + reset_pin = 0; + } + } + delete mpu2; //free the memory allocated to mpu2 (this is important) + }else{ + bt.printf("\n\rSetting up I2C connection..."); + reset_1 = 1; + reset_pin = 1; + wait_ms(20); + reset_pin = 0; + reset_1 = 0; + wait_ms(20); + } + reset_pin = 0; + DigitalIn not_used(PTE23); + myled = 0; //turn ON LED + return 1; +} + + +void start_dmp(MPU6050 mpu1){ //this will get the DMP ready + //initialize device + mpu1.reset(); + bt.printf("\n\rInitializing I2C devices..."); + devStatus = mpu1.dmpInitialize(); + //insert gyro offsets here// Write a calibration algorithm in the future + mpu1.setXGyroOffset(-31); //800/25=32 //-31 is the largest offset available + mpu1.setYGyroOffset(-183/25); + mpu1.setZGyroOffset(80/25); + mpu1.setXAccelOffset(0); //the accel offsets don't do anything + mpu1.setYAccelOffset(0); + mpu1.setZAccelOffset(0); + + + if(devStatus == 0){ //this means initialize was successful + //turn on DMP + bt.printf("\n\rEnabling DMP"); + mpu1.setDMPEnabled(true); + + // enable interrupt detection + bt.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n"); + checkpin.rise(&dmpDataReady); + + mpuIntStatus = mpu1.getIntStatus(); + + //set the DMP ready flag so main loop knows when it is ok to use + bt.printf("DMP ready! Waiting for first interrupt"); + dmpReady = true; + + //get expected DMP packet size + packetSize = mpu1.dmpGetFIFOPacketSize(); + }else{ + bt.printf("\n\rDMP Initialization failed"); + bt.printf("\t%d",devStatus); + wait_ms(1000); + } +} +void update_dmp(MPU6050 mpu1){ + + if (!dmpReady) return; + //while (!mpuInterrupt && fifoCount < packetSize) { + // other program behavior stuff here + // if you are really paranoid you can frequently test in between other + // stuff to see if mpuInterrupt is true, and if so, "break;" from the + // while() loop to immediately process the MPU data + // . + //} + // reset interrupt flag and get INT_STATUS byte + //mpuInterrupt = false; //this resets the interrupt flag + mpuIntStatus = mpu1.getIntStatus(); + + //get current FIFO count; + fifoCount = mpu1.getFIFOCount(); + + //check for overflow (only happens if your code sucks) + if((mpuIntStatus & 0x10) || fifoCount == 1024){ + //reset so we can continue cleanly + mpu1.resetFIFO(); + bt.printf("\n\rFIFO overflow"); + } else if (mpuIntStatus & 0x02){ + //wait for correct available data length (should be very short) + while (fifoCount < packetSize) fifoCount = mpu1.getFIFOCount(); + + //read a packet from FIFO + mpu1.getFIFOBytes(fifoBuffer, packetSize); + + //track FIFO count here in the case there is > 1 packet available + //(allows us to immediately read more w/o waiting for interrupt) + fifoCount -= packetSize; + + mpu1.dmpGetQuaternion(&q,fifoBuffer); + mpu1.dmpGetGravity(&gravity, &q); + mpu1.dmpGetYawPitchRoll(ypr,&q,&gravity); + #ifdef PRINT_GYR + bt.printf("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI); + #endif + + mpu1.dmpGetAccel(&aa, fifoBuffer); + mpu1.dmpGetLinearAccel(&aaReal, &aa, &gravity); + #ifdef PRINT_ACC + bt.printf("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z); + #endif + } +} \ No newline at end of file
diff -r 000000000000 -r dceb852b19f3 DMP.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/DMP.h Tue May 27 09:23:23 2014 +0000 @@ -0,0 +1,12 @@ +#ifndef DMP_H +#define DMP_H + +extern volatile bool mpuInterrupt; //indicates whether interrupt pin has gone high +extern uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +extern uint16_t fifoCount; // count of all bytes currently in FIFO + +int test_dmp(); +void start_dmp(MPU6050); //this will get the DMP ready +void update_dmp(MPU6050); //call this frequently + +#endif \ No newline at end of file
diff -r 000000000000 -r dceb852b19f3 MPU6050-DMP-Ian.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050-DMP-Ian.lib Tue May 27 09:23:23 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/majik/code/MPU6050-DMP-Ian/#3f0bc381e1dd
diff -r 000000000000 -r dceb852b19f3 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue May 27 09:23:23 2014 +0000 @@ -0,0 +1,44 @@ +/* + This code is in working condition. + DMP works in RTOS environment +*/ + +#include "mbed.h" +#include "rtos.h" +#include "robot.h" +#include "MPU6050_6Axis_MotionApps20.h" +#include "DMP.h" + + + +//int main() { +void led2_thread(void const *args){ + DigitalOut reset_pin(PTE5); + test_dmp(); + MPU6050 mpu; + + myled = 0; //turn on LED + + btSwitch = 1; + bt.baud(baudRate); //set baud rate for bluetooth device + + start_dmp(mpu); + + while(1){ + //bt.baud(baudRate); + //reset_pin = 0; + if(!mpuInterrupt && fifoCount < packetSize); //interrupt not ready + else{ //mpu interrupt is ready + update_dmp(mpu); + mpuInterrupt = false; //this resets the interrupt flag + } + } +} + +int main() { + Thread thread(led2_thread); + + while (true) { + Thread::wait(500); + } +} \ No newline at end of file
diff -r 000000000000 -r dceb852b19f3 mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Tue May 27 09:23:23 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#3761f69dbbb2
diff -r 000000000000 -r dceb852b19f3 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue May 27 09:23:23 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b \ No newline at end of file
diff -r 000000000000 -r dceb852b19f3 robot.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robot.cpp Tue May 27 09:23:23 2014 +0000 @@ -0,0 +1,5 @@ +#include "mbed.h" + +DigitalOut myled(PTE3); +Serial bt(PTA2,PTA1); +DigitalOut btSwitch(PTE25); \ No newline at end of file
diff -r 000000000000 -r dceb852b19f3 robot.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robot.h Tue May 27 09:23:23 2014 +0000 @@ -0,0 +1,13 @@ +#ifndef ROBOT_H +#define ROBOT_H + +#define baudRate 115200 //this is the baud rate of the bluetooth device (you just have to know this) +#define PI 3.1415926 +#define PRINT_GYR //uncomment this to print gyro data +//#define PRINT_ACC //uncomment this to print accel data + +extern DigitalOut myled; +extern Serial bt; +extern DigitalOut btSwitch; + +#endif \ No newline at end of file