Mark Vandermeulen / FYDP_Final2

This is for our FYDP project. 2 MPU6050s are used

Dependencies:   Servo mbed

DMP/DMP.cpp@4:05484073a641, 2015-03-22 (annotated)

Committer:
majik
Date:
Sun Mar 22 06:34:30 2015 +0000
Revision:
4:05484073a641
Parent:
2:7b3822eacad8 
BOTH IMUs WORK NOW. Put them in separate threads. Servo is included.

Who changed what in which revision?

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