Code for our FYDP -only one IMU works right now -RTOS is working
Diff: DMP/DMP.cpp
- Revision:
- 0:964eb6a2ef00
diff -r 000000000000 -r 964eb6a2ef00 DMP/DMP.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/DMP/DMP.cpp Wed Mar 18 22:23:48 2015 +0000 @@ -0,0 +1,366 @@ +//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 "DMP.h" + + +//******* MPU DECLARATION THINGS *****************// +int dmp_test = 1; //if this is 0, get raw MPU values + //if this is 1, get DMP values + +int acc_offset[3]; //this saves the offset for acceleration + +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 +bool dmpReady2 = false; // set true if DMP init was succesfful +uint8_t mpuIntStatus2; // holds interrupt status byte from MPU +uint8_t devStatus2; // return status after each device operation (0 = success) +uint16_t packetSize2; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount2; // count of all bytes currently in FIFO +uint8_t fifoBuffer2[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 +InterruptIn checkpin2(PTD5); //PTD5 +//*** Interrupt Detection Routine ***// +volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high +void dmpDataReady(){ + mpuInterrupt = true; +} +volatile bool mpuInterrupt2 = false; //indicates whether interrupt pin has gone high +void dmpDataReady2(){ + mpuInterrupt2 = true; +} + +//****** END MPU DECLARATION ********************// + + +int test_dmp(); //this wraps up the code req'd to start the DMP +void start_dmp(MPU6050); //this will get the DMP ready +void update_dmp(); //call this frequently + +//*********************FUNCTIONS***************************************************************// +int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem + imuSwitch = 1; + LED_OFF; + Thread::wait(100); + + while(mpu.testConnection()==0) + { + imuSwitch = 0; + Thread::wait(100); + imuSwitch = 1; + Thread::wait(100); + bt.lock(); + bt.printf("Stuck on IMU1"); + bt.unlock(); + } + imuSwitch = 1; + LED_ON //turn ON LED + return 1; +} +int test_dmp2(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem + imu2Switch = 1; + LED_OFF; + Thread::wait(100); + + while(mpu2.testConnection()==0) + { + imu2Switch = 0; + Thread::wait(100); + imu2Switch = 1; + Thread::wait(100); + bt.lock(); + bt.printf("Stuck on IMU2"); + bt.unlock(); + } + imu2Switch = 1; + LED_ON //turn ON LED + return 1; +} + +void start_dmp(MPU6050 mpu1){ //this will get the DMP ready + //initialize device + mpu1.reset(); + DMP_DBG_MSG("\n\rInitializing I2C devices...") + devStatus = mpu1.dmpInitialize(); + //insert gyro offsets here// Write a calibration algorithm in the future + mpu1.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available + mpu1.setYGyroOffset(0); + mpu1.setZGyroOffset(0); + mpu1.setXAccelOffset(0); //the accel offsets don't do anything + mpu1.setYAccelOffset(0); + mpu1.setZAccelOffset(0); + + mpu1.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); + + if(devStatus == 0){ //this means initialize was successful + //turn on DMP + DMP_DBG_MSG("\n\rEnabling DMP") + mpu1.setDMPEnabled(true); + + // enable interrupt detection + DMP_DBG_MSG("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 + DMP_DBG_MSG("DMP ready! Waiting for first interrupt") + dmpReady = true; + + //get expected DMP packet size + packetSize = mpu1.dmpGetFIFOPacketSize(); + }else{ + DMP_DBG_MSG("\n\rDMP Initialization failed") + DMP_DBG_MSG("\t%d",devStatus) + Thread::wait(1000); + } + acc_offset[0]=0; + acc_offset[1]=0; + acc_offset[2]=0; + mpu.setDLPFMode(MPU6050_DLPF_BW_42); +} +void start_dmp2(MPU6051 mpu1){ //this will get the DMP ready + //initialize device + mpu2.reset(); + DMP_DBG_MSG("\n\rInitializing I2C devices...") + devStatus2 = mpu2.dmpInitialize(); + //insert gyro offsets here// Write a calibration algorithm in the future + mpu2.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available + mpu2.setYGyroOffset(0); + mpu2.setZGyroOffset(0); + mpu2.setXAccelOffset(0); //the accel offsets don't do anything + mpu2.setYAccelOffset(0); + mpu2.setZAccelOffset(0); + + mpu2.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); + + if(devStatus2 == 0){ //this means initialize was successful + //turn on DMP + DMP_DBG_MSG("\n\rEnabling DMP") + mpu2.setDMPEnabled(true); + + // enable interrupt detection + DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n") + checkpin2.rise(&dmpDataReady2); + + mpuIntStatus2 = mpu2.getIntStatus(); + + //set the DMP ready flag so main loop knows when it is ok to use + DMP_DBG_MSG("DMP ready! Waiting for first interrupt") + dmpReady2 = true; + + //get expected DMP packet size + packetSize2 = mpu2.dmpGetFIFOPacketSize(); + }else{ + DMP_DBG_MSG("\n\rDMP Initialization failed") + DMP_DBG_MSG("\t%d",devStatus2) + Thread::wait(1000); + } + acc_offset[0]=0; + acc_offset[1]=0; + acc_offset[2]=0; + mpu2.setDLPFMode(MPU6050_DLPF_BW_42); +} + +void update_dmp(){ + 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 = mpu.getIntStatus(); + + //get current FIFO count; + fifoCount = mpu.getFIFOCount(); + + //check for overflow (only happens if your code sucks) + if((mpuIntStatus & 0x10) || fifoCount == 1024){ + //reset so we can continue cleanly + mpu.resetFIFO(); + DMP_DBG_MSG("\n\rFIFO overflow") + } else if (mpuIntStatus & 0x02){ + int16_t x,y,z; + //wait for correct available data length (should be very short) + while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + + //read a packet from FIFO + mpu.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; + + mpu.dmpGetQuaternion(&q,fifoBuffer); + imu_data.quat[0]=q.w; + imu_data.quat[1]=q.x; + imu_data.quat[2]=q.y; + imu_data.quat[3]=q.z; + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr,&q,&gravity); + imu_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it + imu_data.ypr[1] = ypr[1]*180/M_PI; + imu_data.ypr[2] = ypr[2]*180/M_PI; + + mpu.getAcceleration(&x,&y,&z); + imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384; + imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384; + imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384; + #ifdef PRINT_GYR + DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI) + #endif + + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + #ifdef PRINT_ACC + DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z) + #endif + //get rotations + mpu.getMotion6(&x,&y,&z,&gx,&gy,&gz); + float scale = 250/16384; + imu_data.gyr[0] = gx*scale; + imu_data.gyr[1] = gy*scale; + imu_data.gyr[2] = gz*scale; + + } +} +void update_dmp2(){ + if (!dmpReady2) return; + while (!mpuInterrupt2 && fifoCount2 < packetSize2) { + // 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 + mpuInterrupt2 = false; //this resets the interrupt flag + mpuIntStatus2 = mpu2.getIntStatus(); + + //get current FIFO count; + fifoCount2 = mpu2.getFIFOCount(); + + //check for overflow (only happens if your code sucks) + if((mpuIntStatus2 & 0x10) || fifoCount2 == 1024){ + //reset so we can continue cleanly + mpu2.resetFIFO(); + DMP_DBG_MSG("\n\rFIFO overflow") + } else if (mpuIntStatus2 & 0x02){ + int16_t x,y,z; + //wait for correct available data length (should be very short) + while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount(); + + //read a packet from FIFO + mpu2.getFIFOBytes(fifoBuffer2, packetSize2); + + //track FIFO count here in the case there is > 1 packet available + //(allows us to immediately read more w/o waiting for interrupt) + fifoCount2 -= packetSize2; + + mpu2.dmpGetQuaternion(&q,fifoBuffer2); + imu2_data.quat[0]=q.w; + imu2_data.quat[1]=q.x; + imu2_data.quat[2]=q.y; + imu2_data.quat[3]=q.z; + mpu2.dmpGetGravity(&gravity, &q); + mpu2.dmpGetYawPitchRoll(ypr,&q,&gravity); + imu2_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it + imu2_data.ypr[1] = ypr[1]*180/M_PI; + imu2_data.ypr[2] = ypr[2]*180/M_PI; + + mpu2.getAcceleration(&x,&y,&z); + imu2_data.acc[0] = (x-acc_offset[0]) *9.81/16384; + imu2_data.acc[1] = (y-acc_offset[1]) *9.81/16384; + imu2_data.acc[2] = (z-acc_offset[2]) *9.81/16384; + #ifdef PRINT_GYR + DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI) + #endif + + mpu2.dmpGetAccel(&aa, fifoBuffer2); + mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity); + #ifdef PRINT_ACC + DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z) + #endif + //get rotations + mpu2.getMotion6(&x,&y,&z,&gx,&gy,&gz); + float scale = 250/16384; + imu2_data.gyr[0] = gx*scale; + imu2_data.gyr[1] = gy*scale; + imu2_data.gyr[2] = gz*scale; + + } +} +void update_acc() +{ + int16_t x,y,z; + mpu.getAcceleration(&x,&y,&z); + imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384; + imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384; + imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384; +} +void calibrate_1() //call this to calibrate the accelerometer +{ //in the future, modify the OFFSET REGISTERS to make the DMP converge faster + //right now, I am only calculating the offset for the accelerometers + int sum[3] = {0,0,0}; + int i; + int16_t x,y,z; + + int count=0; + int t_now = t.read_ms(); + while((t.read_ms()-t_now) < 1000) //loop for one second + { + mpu.getAcceleration(&x,&y,&z); + sum[0] += x; + sum[1] += y; + sum[2] += z; + Thread::wait(5); + count++; + } + for(i = 0; i<3; i++) + acc_offset[i] = (sum[i]/count); + //bt.printf("ACC_OFF:%d;%d;%d;",acc_offset[0],acc_offset[1],acc_offset[2]);//print the offset used by accelerometer +}