272835 Digital Team2015 / Mbed 2 deprecated MPU6050IMU

Basic program to get the properly-scaled gyro and accelerometer data from a MPU-6050 6-axis motion sensor. Perform sensor fusion using Sebastian Madgwick's open-source IMU fusion filter. Running on the STM32F401 at 84 MHz achieved sensor fusion filter update rates of 5500 Hz. Additional info at https://github.com/kriswiner/MPU-6050.

Dependencies:   mbed

Fork of MPU6050IMU by Kris Winer

main.cpp@3:bf3448217248, 2015-12-05 (annotated)

Committer:
thanawatinges
Date:
Sat Dec 05 12:01:16 2015 +0000
Revision:
3:bf3448217248
Parent:
1:cea9d83b8636 
IMU

Who changed what in which revision?

UserRevisionLine numberNew contents of line
onehorse 1:cea9d83b8636 1 #include "mbed.h"
onehorse 1:cea9d83b8636 2 #include "MPU6050.h"
onehorse 0:65aa78c10981 3
onehorse 1:cea9d83b8636 4 float sum = 0;
onehorse 1:cea9d83b8636 5 uint32_t sumCount = 0;
onehorse 1:cea9d83b8636 6
thanawatinges 3:bf3448217248 7 MPU6050 mpu6050;
onehorse 1:cea9d83b8636 8
thanawatinges 3:bf3448217248 9 Timer t;
onehorse 1:cea9d83b8636 10
thanawatinges 3:bf3448217248 11 Serial pc(USBTX, USBRX); // tx, rx
thanawatinges 3:bf3448217248 12
onehorse 1:cea9d83b8636 13 int main()
onehorse 1:cea9d83b8636 14 {
thanawatinges 3:bf3448217248 15 pc.baud(9600);
onehorse 0:65aa78c10981 16
thanawatinges 3:bf3448217248 17 //Set up I2C
thanawatinges 3:bf3448217248 18 i2c.frequency(400000); // use fast (400 kHz) I2C
onehorse 0:65aa78c10981 19
thanawatinges 3:bf3448217248 20 t.start();
onehorse 0:65aa78c10981 21
onehorse 0:65aa78c10981 22
thanawatinges 3:bf3448217248 23 // Read the WHO_AM_I register, this is a good test of communication
thanawatinges 3:bf3448217248 24 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
thanawatinges 3:bf3448217248 25 pc.printf("I AM 0x%x\n\r", whoami);
thanawatinges 3:bf3448217248 26 pc.printf("I SHOULD BE 0x68\n\r");
thanawatinges 3:bf3448217248 27
thanawatinges 3:bf3448217248 28 if (whoami == 0x68) { // WHO_AM_I should always be 0x68
thanawatinges 3:bf3448217248 29 pc.printf("MPU6050 is online...");
thanawatinges 3:bf3448217248 30 wait(1);
thanawatinges 3:bf3448217248 31
thanawatinges 3:bf3448217248 32 if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
thanawatinges 3:bf3448217248 33 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
thanawatinges 3:bf3448217248 34 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
thanawatinges 3:bf3448217248 35 mpu6050.initMPU6050();
thanawatinges 3:bf3448217248 36 pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
onehorse 0:65aa78c10981 37
thanawatinges 3:bf3448217248 38 wait(2);
thanawatinges 3:bf3448217248 39 } else {
thanawatinges 3:bf3448217248 40 pc.printf("Device did not the pass self-test!\n\r");
thanawatinges 3:bf3448217248 41 }
thanawatinges 3:bf3448217248 42 } else {
thanawatinges 3:bf3448217248 43 pc.printf("Could not connect to MPU6050: \n\r");
thanawatinges 3:bf3448217248 44 pc.printf("%#x \n", whoami);
thanawatinges 3:bf3448217248 45
thanawatinges 3:bf3448217248 46 while(1) ; // Loop forever if communication doesn't happen
onehorse 0:65aa78c10981 47 }
thanawatinges 3:bf3448217248 48 while(1) {
thanawatinges 3:bf3448217248 49
thanawatinges 3:bf3448217248 50 // If data ready bit set, all data registers have new data
thanawatinges 3:bf3448217248 51 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
thanawatinges 3:bf3448217248 52 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
thanawatinges 3:bf3448217248 53 mpu6050.getAres();
onehorse 0:65aa78c10981 54
thanawatinges 3:bf3448217248 55 // Now we'll calculate the accleration value into actual g's
thanawatinges 3:bf3448217248 56 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
thanawatinges 3:bf3448217248 57 ay = (float)accelCount[1]*aRes - accelBias[1];
thanawatinges 3:bf3448217248 58 az = (float)accelCount[2]*aRes - accelBias[2];
onehorse 1:cea9d83b8636 59
thanawatinges 3:bf3448217248 60 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
thanawatinges 3:bf3448217248 61 mpu6050.getGres();
onehorse 0:65aa78c10981 62
thanawatinges 3:bf3448217248 63 // Calculate the gyro value into actual degrees per second
thanawatinges 3:bf3448217248 64 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
thanawatinges 3:bf3448217248 65 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
thanawatinges 3:bf3448217248 66 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
thanawatinges 3:bf3448217248 67
thanawatinges 3:bf3448217248 68 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
thanawatinges 3:bf3448217248 69 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
thanawatinges 3:bf3448217248 70 }
thanawatinges 3:bf3448217248 71
thanawatinges 3:bf3448217248 72 Now = t.read_us();
thanawatinges 3:bf3448217248 73 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
thanawatinges 3:bf3448217248 74 lastUpdate = Now;
thanawatinges 3:bf3448217248 75
thanawatinges 3:bf3448217248 76 sum += deltat;
thanawatinges 3:bf3448217248 77 sumCount++;
thanawatinges 3:bf3448217248 78
thanawatinges 3:bf3448217248 79 if(lastUpdate - firstUpdate > 10000000.0f) {
thanawatinges 3:bf3448217248 80 beta = 0.04; // decrease filter gain after stabilized
thanawatinges 3:bf3448217248 81 zeta = 0.015; // increasey bias drift gain after stabilized
thanawatinges 3:bf3448217248 82 }
onehorse 0:65aa78c10981 83
thanawatinges 3:bf3448217248 84 // Pass gyro rate as rad/s
thanawatinges 3:bf3448217248 85 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
onehorse 0:65aa78c10981 86
thanawatinges 3:bf3448217248 87 // Serial print and/or display at 0.5 s rate independent of data rates
thanawatinges 3:bf3448217248 88 delt_t = t.read_ms() - count;
thanawatinges 3:bf3448217248 89 if (delt_t > 500) { // update LCD once per half-second independent of read rate
thanawatinges 3:bf3448217248 90
thanawatinges 3:bf3448217248 91 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
thanawatinges 3:bf3448217248 92 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
thanawatinges 3:bf3448217248 93 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
thanawatinges 3:bf3448217248 94 pitch *= 180.0f / PI;
thanawatinges 3:bf3448217248 95 yaw *= 180.0f / PI;
thanawatinges 3:bf3448217248 96 roll *= 180.0f / PI;
thanawatinges 3:bf3448217248 97
thanawatinges 3:bf3448217248 98 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
thanawatinges 3:bf3448217248 99 // pc.printf("average rate = %f\n\r", (float) sumCount/sum);
thanawatinges 3:bf3448217248 100
thanawatinges 3:bf3448217248 101 myled= !myled;
thanawatinges 3:bf3448217248 102 count = t.read_ms();
thanawatinges 3:bf3448217248 103 sum = 0;
thanawatinges 3:bf3448217248 104 sumCount = 0;
thanawatinges 3:bf3448217248 105 }
thanawatinges 3:bf3448217248 106 }
thanawatinges 3:bf3448217248 107
thanawatinges 3:bf3448217248 108 }