Mark Vandermeulen
Code for our FYDP -only one IMU works right now -RTOS is working
Diff: DMP/DMP.cpp
- Revision:
- 0:964eb6a2ef00
--- /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
+}