Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

DMP/DMP.cpp@0:964eb6a2ef00, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:23:48 2015 +0000
Revision:
0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:964eb6a2ef00 1 //Copied MPU6050.h and MPU6050.cpp from another library (otherwise MPU gets stuck randomly)
majik 0:964eb6a2ef00 2
majik 0:964eb6a2ef00 3 /*** THIS IS THE BEST VERSION OF DMP CODE
majik 0:964eb6a2ef00 4 This code demonstrates how to access the MPU6050 IMU and fix the
majik 0:964eb6a2ef00 5 connection errors that occur, using a hardware hack.
majik 0:964eb6a2ef00 6 It requires two hardware fixes: A transistor (PTE5) and an extra GPIO pin (PTE23) connected
majik 0:964eb6a2ef00 7 to the SDA line.
majik 0:964eb6a2ef00 8 Error 1: The SDA line gets stuck on ground.
majik 0:964eb6a2ef00 9 Solution 1: Use the transistor to raise the SDA voltage off of ground. This resets the I2C bus. (line 71)
majik 0:964eb6a2ef00 10 Error 2: The SDA line gets stuck on 3.3V
majik 0:964eb6a2ef00 11 Solution 2: Use the GPIO line to pull the SDA voltage down a bit to reset the I2C bus. (line 96)
majik 0:964eb6a2ef00 12 (I am currently trying to find a software solution for the above hack)
majik 0:964eb6a2ef00 13
majik 0:964eb6a2ef00 14 This code works for using the DMP. (See "DMP" for implementation that can do raw accel data)
majik 0:964eb6a2ef00 15 It appears all connection issues have been fixed.
majik 0:964eb6a2ef00 16 Next step: make code neater (in progress)
majik 0:964eb6a2ef00 17
majik 0:964eb6a2ef00 18 LEARNED: Do not need the PTE23 line. Only fix necessary is the transistor (PTE5)
majik 0:964eb6a2ef00 19 ***/
majik 0:964eb6a2ef00 20
majik 0:964eb6a2ef00 21 #include "mbed.h"
majik 0:964eb6a2ef00 22 #include "DMP.h"
majik 0:964eb6a2ef00 23
majik 0:964eb6a2ef00 24
majik 0:964eb6a2ef00 25 //******* MPU DECLARATION THINGS *****************//
majik 0:964eb6a2ef00 26 int dmp_test = 1; //if this is 0, get raw MPU values
majik 0:964eb6a2ef00 27 //if this is 1, get DMP values
majik 0:964eb6a2ef00 28
majik 0:964eb6a2ef00 29 int acc_offset[3]; //this saves the offset for acceleration
majik 0:964eb6a2ef00 30
majik 0:964eb6a2ef00 31 int16_t ax, ay, az;
majik 0:964eb6a2ef00 32 int16_t gx, gy, gz;
majik 0:964eb6a2ef00 33
majik 0:964eb6a2ef00 34 //******* DMP things ****************//
majik 0:964eb6a2ef00 35 // MPU control/status vars
majik 0:964eb6a2ef00 36 bool dmpReady = false; // set true if DMP init was succesfful
majik 0:964eb6a2ef00 37 uint8_t mpuIntStatus; // holds interrupt status byte from MPU
majik 0:964eb6a2ef00 38 uint8_t devStatus; // return status after each device operation (0 = success)
majik 0:964eb6a2ef00 39 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
majik 0:964eb6a2ef00 40 uint16_t fifoCount; // count of all bytes currently in FIFO
majik 0:964eb6a2ef00 41 uint8_t fifoBuffer[64]; // FIFO storage buffer
majik 0:964eb6a2ef00 42 bool dmpReady2 = false; // set true if DMP init was succesfful
majik 0:964eb6a2ef00 43 uint8_t mpuIntStatus2; // holds interrupt status byte from MPU
majik 0:964eb6a2ef00 44 uint8_t devStatus2; // return status after each device operation (0 = success)
majik 0:964eb6a2ef00 45 uint16_t packetSize2; // expected DMP packet size (default is 42 bytes)
majik 0:964eb6a2ef00 46 uint16_t fifoCount2; // count of all bytes currently in FIFO
majik 0:964eb6a2ef00 47 uint8_t fifoBuffer2[64]; // FIFO storage buffer
majik 0:964eb6a2ef00 48
majik 0:964eb6a2ef00 49
majik 0:964eb6a2ef00 50
majik 0:964eb6a2ef00 51 uint8_t MPU_CONNECTION;
majik 0:964eb6a2ef00 52
majik 0:964eb6a2ef00 53 // orientation/motion vars
majik 0:964eb6a2ef00 54 Quaternion q; // [w,x,y,z] quaternion container
majik 0:964eb6a2ef00 55 VectorInt16 aa; // [x,y,z] accel sensor measurements
majik 0:964eb6a2ef00 56 VectorInt16 aaReal; // [x,y,z] gravity-free accel sensor measurements
majik 0:964eb6a2ef00 57 VectorInt16 aaWorld; // [x,y,z] world-frame accel sensor measurements
majik 0:964eb6a2ef00 58 VectorFloat gravity; // [x,y,z] gravity vector
majik 0:964eb6a2ef00 59 float euler[3]; // [psi,theta,phi] Euler angle container
majik 0:964eb6a2ef00 60 float ypr[3]; // [yaw,pitch,roll] yaw/pitch/roll container [rad]. Multiply by 180, divide by PI for [deg]
majik 0:964eb6a2ef00 61 InterruptIn checkpin(PTD7); //interrupt
majik 0:964eb6a2ef00 62 InterruptIn checkpin2(PTD5); //PTD5
majik 0:964eb6a2ef00 63 //*** Interrupt Detection Routine ***//
majik 0:964eb6a2ef00 64 volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high
majik 0:964eb6a2ef00 65 void dmpDataReady(){
majik 0:964eb6a2ef00 66 mpuInterrupt = true;
majik 0:964eb6a2ef00 67 }
majik 0:964eb6a2ef00 68 volatile bool mpuInterrupt2 = false; //indicates whether interrupt pin has gone high
majik 0:964eb6a2ef00 69 void dmpDataReady2(){
majik 0:964eb6a2ef00 70 mpuInterrupt2 = true;
majik 0:964eb6a2ef00 71 }
majik 0:964eb6a2ef00 72
majik 0:964eb6a2ef00 73 //****** END MPU DECLARATION ********************//
majik 0:964eb6a2ef00 74
majik 0:964eb6a2ef00 75
majik 0:964eb6a2ef00 76 int test_dmp(); //this wraps up the code req'd to start the DMP
majik 0:964eb6a2ef00 77 void start_dmp(MPU6050); //this will get the DMP ready
majik 0:964eb6a2ef00 78 void update_dmp(); //call this frequently
majik 0:964eb6a2ef00 79
majik 0:964eb6a2ef00 80 //*********************FUNCTIONS***************************************************************//
majik 0:964eb6a2ef00 81 int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem
majik 0:964eb6a2ef00 82 imuSwitch = 1;
majik 0:964eb6a2ef00 83 LED_OFF;
majik 0:964eb6a2ef00 84 Thread::wait(100);
majik 0:964eb6a2ef00 85
majik 0:964eb6a2ef00 86 while(mpu.testConnection()==0)
majik 0:964eb6a2ef00 87 {
majik 0:964eb6a2ef00 88 imuSwitch = 0;
majik 0:964eb6a2ef00 89 Thread::wait(100);
majik 0:964eb6a2ef00 90 imuSwitch = 1;
majik 0:964eb6a2ef00 91 Thread::wait(100);
majik 0:964eb6a2ef00 92 bt.lock();
majik 0:964eb6a2ef00 93 bt.printf("Stuck on IMU1");
majik 0:964eb6a2ef00 94 bt.unlock();
majik 0:964eb6a2ef00 95 }
majik 0:964eb6a2ef00 96 imuSwitch = 1;
majik 0:964eb6a2ef00 97 LED_ON //turn ON LED
majik 0:964eb6a2ef00 98 return 1;
majik 0:964eb6a2ef00 99 }
majik 0:964eb6a2ef00 100 int test_dmp2(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem
majik 0:964eb6a2ef00 101 imu2Switch = 1;
majik 0:964eb6a2ef00 102 LED_OFF;
majik 0:964eb6a2ef00 103 Thread::wait(100);
majik 0:964eb6a2ef00 104
majik 0:964eb6a2ef00 105 while(mpu2.testConnection()==0)
majik 0:964eb6a2ef00 106 {
majik 0:964eb6a2ef00 107 imu2Switch = 0;
majik 0:964eb6a2ef00 108 Thread::wait(100);
majik 0:964eb6a2ef00 109 imu2Switch = 1;
majik 0:964eb6a2ef00 110 Thread::wait(100);
majik 0:964eb6a2ef00 111 bt.lock();
majik 0:964eb6a2ef00 112 bt.printf("Stuck on IMU2");
majik 0:964eb6a2ef00 113 bt.unlock();
majik 0:964eb6a2ef00 114 }
majik 0:964eb6a2ef00 115 imu2Switch = 1;
majik 0:964eb6a2ef00 116 LED_ON //turn ON LED
majik 0:964eb6a2ef00 117 return 1;
majik 0:964eb6a2ef00 118 }
majik 0:964eb6a2ef00 119
majik 0:964eb6a2ef00 120 void start_dmp(MPU6050 mpu1){ //this will get the DMP ready
majik 0:964eb6a2ef00 121 //initialize device
majik 0:964eb6a2ef00 122 mpu1.reset();
majik 0:964eb6a2ef00 123 DMP_DBG_MSG("\n\rInitializing I2C devices...")
majik 0:964eb6a2ef00 124 devStatus = mpu1.dmpInitialize();
majik 0:964eb6a2ef00 125 //insert gyro offsets here// Write a calibration algorithm in the future
majik 0:964eb6a2ef00 126 mpu1.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available
majik 0:964eb6a2ef00 127 mpu1.setYGyroOffset(0);
majik 0:964eb6a2ef00 128 mpu1.setZGyroOffset(0);
majik 0:964eb6a2ef00 129 mpu1.setXAccelOffset(0); //the accel offsets don't do anything
majik 0:964eb6a2ef00 130 mpu1.setYAccelOffset(0);
majik 0:964eb6a2ef00 131 mpu1.setZAccelOffset(0);
majik 0:964eb6a2ef00 132
majik 0:964eb6a2ef00 133 mpu1.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
majik 0:964eb6a2ef00 134
majik 0:964eb6a2ef00 135 if(devStatus == 0){ //this means initialize was successful
majik 0:964eb6a2ef00 136 //turn on DMP
majik 0:964eb6a2ef00 137 DMP_DBG_MSG("\n\rEnabling DMP")
majik 0:964eb6a2ef00 138 mpu1.setDMPEnabled(true);
majik 0:964eb6a2ef00 139
majik 0:964eb6a2ef00 140 // enable interrupt detection
majik 0:964eb6a2ef00 141 DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n")
majik 0:964eb6a2ef00 142 checkpin.rise(&dmpDataReady);
majik 0:964eb6a2ef00 143
majik 0:964eb6a2ef00 144 mpuIntStatus = mpu1.getIntStatus();
majik 0:964eb6a2ef00 145
majik 0:964eb6a2ef00 146 //set the DMP ready flag so main loop knows when it is ok to use
majik 0:964eb6a2ef00 147 DMP_DBG_MSG("DMP ready! Waiting for first interrupt")
majik 0:964eb6a2ef00 148 dmpReady = true;
majik 0:964eb6a2ef00 149
majik 0:964eb6a2ef00 150 //get expected DMP packet size
majik 0:964eb6a2ef00 151 packetSize = mpu1.dmpGetFIFOPacketSize();
majik 0:964eb6a2ef00 152 }else{
majik 0:964eb6a2ef00 153 DMP_DBG_MSG("\n\rDMP Initialization failed")
majik 0:964eb6a2ef00 154 DMP_DBG_MSG("\t%d",devStatus)
majik 0:964eb6a2ef00 155 Thread::wait(1000);
majik 0:964eb6a2ef00 156 }
majik 0:964eb6a2ef00 157 acc_offset[0]=0;
majik 0:964eb6a2ef00 158 acc_offset[1]=0;
majik 0:964eb6a2ef00 159 acc_offset[2]=0;
majik 0:964eb6a2ef00 160 mpu.setDLPFMode(MPU6050_DLPF_BW_42);
majik 0:964eb6a2ef00 161 }
majik 0:964eb6a2ef00 162 void start_dmp2(MPU6051 mpu1){ //this will get the DMP ready
majik 0:964eb6a2ef00 163 //initialize device
majik 0:964eb6a2ef00 164 mpu2.reset();
majik 0:964eb6a2ef00 165 DMP_DBG_MSG("\n\rInitializing I2C devices...")
majik 0:964eb6a2ef00 166 devStatus2 = mpu2.dmpInitialize();
majik 0:964eb6a2ef00 167 //insert gyro offsets here// Write a calibration algorithm in the future
majik 0:964eb6a2ef00 168 mpu2.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available
majik 0:964eb6a2ef00 169 mpu2.setYGyroOffset(0);
majik 0:964eb6a2ef00 170 mpu2.setZGyroOffset(0);
majik 0:964eb6a2ef00 171 mpu2.setXAccelOffset(0); //the accel offsets don't do anything
majik 0:964eb6a2ef00 172 mpu2.setYAccelOffset(0);
majik 0:964eb6a2ef00 173 mpu2.setZAccelOffset(0);
majik 0:964eb6a2ef00 174
majik 0:964eb6a2ef00 175 mpu2.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
majik 0:964eb6a2ef00 176
majik 0:964eb6a2ef00 177 if(devStatus2 == 0){ //this means initialize was successful
majik 0:964eb6a2ef00 178 //turn on DMP
majik 0:964eb6a2ef00 179 DMP_DBG_MSG("\n\rEnabling DMP")
majik 0:964eb6a2ef00 180 mpu2.setDMPEnabled(true);
majik 0:964eb6a2ef00 181
majik 0:964eb6a2ef00 182 // enable interrupt detection
majik 0:964eb6a2ef00 183 DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n")
majik 0:964eb6a2ef00 184 checkpin2.rise(&dmpDataReady2);
majik 0:964eb6a2ef00 185
majik 0:964eb6a2ef00 186 mpuIntStatus2 = mpu2.getIntStatus();
majik 0:964eb6a2ef00 187
majik 0:964eb6a2ef00 188 //set the DMP ready flag so main loop knows when it is ok to use
majik 0:964eb6a2ef00 189 DMP_DBG_MSG("DMP ready! Waiting for first interrupt")
majik 0:964eb6a2ef00 190 dmpReady2 = true;
majik 0:964eb6a2ef00 191
majik 0:964eb6a2ef00 192 //get expected DMP packet size
majik 0:964eb6a2ef00 193 packetSize2 = mpu2.dmpGetFIFOPacketSize();
majik 0:964eb6a2ef00 194 }else{
majik 0:964eb6a2ef00 195 DMP_DBG_MSG("\n\rDMP Initialization failed")
majik 0:964eb6a2ef00 196 DMP_DBG_MSG("\t%d",devStatus2)
majik 0:964eb6a2ef00 197 Thread::wait(1000);
majik 0:964eb6a2ef00 198 }
majik 0:964eb6a2ef00 199 acc_offset[0]=0;
majik 0:964eb6a2ef00 200 acc_offset[1]=0;
majik 0:964eb6a2ef00 201 acc_offset[2]=0;
majik 0:964eb6a2ef00 202 mpu2.setDLPFMode(MPU6050_DLPF_BW_42);
majik 0:964eb6a2ef00 203 }
majik 0:964eb6a2ef00 204
majik 0:964eb6a2ef00 205 void update_dmp(){
majik 0:964eb6a2ef00 206 if (!dmpReady) return;
majik 0:964eb6a2ef00 207 while (!mpuInterrupt && fifoCount < packetSize) {
majik 0:964eb6a2ef00 208 // other program behavior stuff here
majik 0:964eb6a2ef00 209 // if you are really paranoid you can frequently test in between other
majik 0:964eb6a2ef00 210 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
majik 0:964eb6a2ef00 211 // while() loop to immediately process the MPU data
majik 0:964eb6a2ef00 212 // .
majik 0:964eb6a2ef00 213 }
majik 0:964eb6a2ef00 214 // reset interrupt flag and get INT_STATUS byte
majik 0:964eb6a2ef00 215 mpuInterrupt = false; //this resets the interrupt flag
majik 0:964eb6a2ef00 216 mpuIntStatus = mpu.getIntStatus();
majik 0:964eb6a2ef00 217
majik 0:964eb6a2ef00 218 //get current FIFO count;
majik 0:964eb6a2ef00 219 fifoCount = mpu.getFIFOCount();
majik 0:964eb6a2ef00 220
majik 0:964eb6a2ef00 221 //check for overflow (only happens if your code sucks)
majik 0:964eb6a2ef00 222 if((mpuIntStatus & 0x10) || fifoCount == 1024){
majik 0:964eb6a2ef00 223 //reset so we can continue cleanly
majik 0:964eb6a2ef00 224 mpu.resetFIFO();
majik 0:964eb6a2ef00 225 DMP_DBG_MSG("\n\rFIFO overflow")
majik 0:964eb6a2ef00 226 } else if (mpuIntStatus & 0x02){
majik 0:964eb6a2ef00 227 int16_t x,y,z;
majik 0:964eb6a2ef00 228 //wait for correct available data length (should be very short)
majik 0:964eb6a2ef00 229 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
majik 0:964eb6a2ef00 230
majik 0:964eb6a2ef00 231 //read a packet from FIFO
majik 0:964eb6a2ef00 232 mpu.getFIFOBytes(fifoBuffer, packetSize);
majik 0:964eb6a2ef00 233
majik 0:964eb6a2ef00 234 //track FIFO count here in the case there is > 1 packet available
majik 0:964eb6a2ef00 235 //(allows us to immediately read more w/o waiting for interrupt)
majik 0:964eb6a2ef00 236 fifoCount -= packetSize;
majik 0:964eb6a2ef00 237
majik 0:964eb6a2ef00 238 mpu.dmpGetQuaternion(&q,fifoBuffer);
majik 0:964eb6a2ef00 239 imu_data.quat[0]=q.w;
majik 0:964eb6a2ef00 240 imu_data.quat[1]=q.x;
majik 0:964eb6a2ef00 241 imu_data.quat[2]=q.y;
majik 0:964eb6a2ef00 242 imu_data.quat[3]=q.z;
majik 0:964eb6a2ef00 243 mpu.dmpGetGravity(&gravity, &q);
majik 0:964eb6a2ef00 244 mpu.dmpGetYawPitchRoll(ypr,&q,&gravity);
majik 0:964eb6a2ef00 245 imu_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it
majik 0:964eb6a2ef00 246 imu_data.ypr[1] = ypr[1]*180/M_PI;
majik 0:964eb6a2ef00 247 imu_data.ypr[2] = ypr[2]*180/M_PI;
majik 0:964eb6a2ef00 248
majik 0:964eb6a2ef00 249 mpu.getAcceleration(&x,&y,&z);
majik 0:964eb6a2ef00 250 imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
majik 0:964eb6a2ef00 251 imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
majik 0:964eb6a2ef00 252 imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
majik 0:964eb6a2ef00 253 #ifdef PRINT_GYR
majik 0:964eb6a2ef00 254 DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI)
majik 0:964eb6a2ef00 255 #endif
majik 0:964eb6a2ef00 256
majik 0:964eb6a2ef00 257 mpu.dmpGetAccel(&aa, fifoBuffer);
majik 0:964eb6a2ef00 258 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
majik 0:964eb6a2ef00 259 #ifdef PRINT_ACC
majik 0:964eb6a2ef00 260 DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z)
majik 0:964eb6a2ef00 261 #endif
majik 0:964eb6a2ef00 262 //get rotations
majik 0:964eb6a2ef00 263 mpu.getMotion6(&x,&y,&z,&gx,&gy,&gz);
majik 0:964eb6a2ef00 264 float scale = 250/16384;
majik 0:964eb6a2ef00 265 imu_data.gyr[0] = gx*scale;
majik 0:964eb6a2ef00 266 imu_data.gyr[1] = gy*scale;
majik 0:964eb6a2ef00 267 imu_data.gyr[2] = gz*scale;
majik 0:964eb6a2ef00 268
majik 0:964eb6a2ef00 269 }
majik 0:964eb6a2ef00 270 }
majik 0:964eb6a2ef00 271 void update_dmp2(){
majik 0:964eb6a2ef00 272 if (!dmpReady2) return;
majik 0:964eb6a2ef00 273 while (!mpuInterrupt2 && fifoCount2 < packetSize2) {
majik 0:964eb6a2ef00 274 // other program behavior stuff here
majik 0:964eb6a2ef00 275 // if you are really paranoid you can frequently test in between other
majik 0:964eb6a2ef00 276 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
majik 0:964eb6a2ef00 277 // while() loop to immediately process the MPU data
majik 0:964eb6a2ef00 278 // .
majik 0:964eb6a2ef00 279 }
majik 0:964eb6a2ef00 280 // reset interrupt flag and get INT_STATUS byte
majik 0:964eb6a2ef00 281 mpuInterrupt2 = false; //this resets the interrupt flag
majik 0:964eb6a2ef00 282 mpuIntStatus2 = mpu2.getIntStatus();
majik 0:964eb6a2ef00 283
majik 0:964eb6a2ef00 284 //get current FIFO count;
majik 0:964eb6a2ef00 285 fifoCount2 = mpu2.getFIFOCount();
majik 0:964eb6a2ef00 286
majik 0:964eb6a2ef00 287 //check for overflow (only happens if your code sucks)
majik 0:964eb6a2ef00 288 if((mpuIntStatus2 & 0x10) || fifoCount2 == 1024){
majik 0:964eb6a2ef00 289 //reset so we can continue cleanly
majik 0:964eb6a2ef00 290 mpu2.resetFIFO();
majik 0:964eb6a2ef00 291 DMP_DBG_MSG("\n\rFIFO overflow")
majik 0:964eb6a2ef00 292 } else if (mpuIntStatus2 & 0x02){
majik 0:964eb6a2ef00 293 int16_t x,y,z;
majik 0:964eb6a2ef00 294 //wait for correct available data length (should be very short)
majik 0:964eb6a2ef00 295 while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();
majik 0:964eb6a2ef00 296
majik 0:964eb6a2ef00 297 //read a packet from FIFO
majik 0:964eb6a2ef00 298 mpu2.getFIFOBytes(fifoBuffer2, packetSize2);
majik 0:964eb6a2ef00 299
majik 0:964eb6a2ef00 300 //track FIFO count here in the case there is > 1 packet available
majik 0:964eb6a2ef00 301 //(allows us to immediately read more w/o waiting for interrupt)
majik 0:964eb6a2ef00 302 fifoCount2 -= packetSize2;
majik 0:964eb6a2ef00 303
majik 0:964eb6a2ef00 304 mpu2.dmpGetQuaternion(&q,fifoBuffer2);
majik 0:964eb6a2ef00 305 imu2_data.quat[0]=q.w;
majik 0:964eb6a2ef00 306 imu2_data.quat[1]=q.x;
majik 0:964eb6a2ef00 307 imu2_data.quat[2]=q.y;
majik 0:964eb6a2ef00 308 imu2_data.quat[3]=q.z;
majik 0:964eb6a2ef00 309 mpu2.dmpGetGravity(&gravity, &q);
majik 0:964eb6a2ef00 310 mpu2.dmpGetYawPitchRoll(ypr,&q,&gravity);
majik 0:964eb6a2ef00 311 imu2_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it
majik 0:964eb6a2ef00 312 imu2_data.ypr[1] = ypr[1]*180/M_PI;
majik 0:964eb6a2ef00 313 imu2_data.ypr[2] = ypr[2]*180/M_PI;
majik 0:964eb6a2ef00 314
majik 0:964eb6a2ef00 315 mpu2.getAcceleration(&x,&y,&z);
majik 0:964eb6a2ef00 316 imu2_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
majik 0:964eb6a2ef00 317 imu2_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
majik 0:964eb6a2ef00 318 imu2_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
majik 0:964eb6a2ef00 319 #ifdef PRINT_GYR
majik 0:964eb6a2ef00 320 DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI)
majik 0:964eb6a2ef00 321 #endif
majik 0:964eb6a2ef00 322
majik 0:964eb6a2ef00 323 mpu2.dmpGetAccel(&aa, fifoBuffer2);
majik 0:964eb6a2ef00 324 mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity);
majik 0:964eb6a2ef00 325 #ifdef PRINT_ACC
majik 0:964eb6a2ef00 326 DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z)
majik 0:964eb6a2ef00 327 #endif
majik 0:964eb6a2ef00 328 //get rotations
majik 0:964eb6a2ef00 329 mpu2.getMotion6(&x,&y,&z,&gx,&gy,&gz);
majik 0:964eb6a2ef00 330 float scale = 250/16384;
majik 0:964eb6a2ef00 331 imu2_data.gyr[0] = gx*scale;
majik 0:964eb6a2ef00 332 imu2_data.gyr[1] = gy*scale;
majik 0:964eb6a2ef00 333 imu2_data.gyr[2] = gz*scale;
majik 0:964eb6a2ef00 334
majik 0:964eb6a2ef00 335 }
majik 0:964eb6a2ef00 336 }
majik 0:964eb6a2ef00 337 void update_acc()
majik 0:964eb6a2ef00 338 {
majik 0:964eb6a2ef00 339 int16_t x,y,z;
majik 0:964eb6a2ef00 340 mpu.getAcceleration(&x,&y,&z);
majik 0:964eb6a2ef00 341 imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
majik 0:964eb6a2ef00 342 imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
majik 0:964eb6a2ef00 343 imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
majik 0:964eb6a2ef00 344 }
majik 0:964eb6a2ef00 345 void calibrate_1() //call this to calibrate the accelerometer
majik 0:964eb6a2ef00 346 { //in the future, modify the OFFSET REGISTERS to make the DMP converge faster
majik 0:964eb6a2ef00 347 //right now, I am only calculating the offset for the accelerometers
majik 0:964eb6a2ef00 348 int sum[3] = {0,0,0};
majik 0:964eb6a2ef00 349 int i;
majik 0:964eb6a2ef00 350 int16_t x,y,z;
majik 0:964eb6a2ef00 351
majik 0:964eb6a2ef00 352 int count=0;
majik 0:964eb6a2ef00 353 int t_now = t.read_ms();
majik 0:964eb6a2ef00 354 while((t.read_ms()-t_now) < 1000) //loop for one second
majik 0:964eb6a2ef00 355 {
majik 0:964eb6a2ef00 356 mpu.getAcceleration(&x,&y,&z);
majik 0:964eb6a2ef00 357 sum[0] += x;
majik 0:964eb6a2ef00 358 sum[1] += y;
majik 0:964eb6a2ef00 359 sum[2] += z;
majik 0:964eb6a2ef00 360 Thread::wait(5);
majik 0:964eb6a2ef00 361 count++;
majik 0:964eb6a2ef00 362 }
majik 0:964eb6a2ef00 363 for(i = 0; i<3; i++)
majik 0:964eb6a2ef00 364 acc_offset[i] = (sum[i]/count);
majik 0:964eb6a2ef00 365 //bt.printf("ACC_OFF:%d;%d;%d;",acc_offset[0],acc_offset[1],acc_offset[2]);//print the offset used by accelerometer
majik 0:964eb6a2ef00 366 }