Takujyou_Ishii / Mbed 2 deprecated test_mpu6050

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Madgwickfilter MPU6050

main.cpp@3:9424c6493a75, 2017-09-08 (annotated)

Committer:
stream3715
Date:
Fri Sep 08 18:01:40 2017 +0000
Revision:
3:9424c6493a75
Parent:
0:07431908151a
Child:
4:fdba5e452d36
????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stream3715 0:07431908151a 1
stream3715 0:07431908151a 2 /* MPU6050 Basic Example Code
stream3715 0:07431908151a 3 by: Kris Winer
stream3715 0:07431908151a 4 date: May 1, 2014
stream3715 0:07431908151a 5 license: Beerware - Use this code however you'd like. If you
stream3715 0:07431908151a 6 find it useful you can buy me a beer some time.
stream3715 0:07431908151a 7
stream3715 0:07431908151a 8 Demonstrate MPU-6050 basic functionality including initialization, accelerometer trimming, sleep mode functionality as well as
stream3715 0:07431908151a 9 parameterizing the register addresses. Added display functions to allow display to on breadboard monitor.
stream3715 0:07431908151a 10 No DMP use. We just want to get out the accelerations, temperature, and gyro readings.
stream3715 0:07431908151a 11
stream3715 0:07431908151a 12 SDA and SCL should have external pull-up resistors (to 3.3V).
stream3715 0:07431908151a 13 10k resistors worked for me. They should be on the breakout
stream3715 0:07431908151a 14 board.
stream3715 0:07431908151a 15
stream3715 0:07431908151a 16 Hardware setup:
stream3715 0:07431908151a 17 MPU6050 Breakout --------- Arduino
stream3715 0:07431908151a 18 3.3V --------------------- 3.3V
stream3715 0:07431908151a 19 SDA ----------------------- A4
stream3715 0:07431908151a 20 SCL ----------------------- A5
stream3715 0:07431908151a 21 GND ---------------------- GND
stream3715 0:07431908151a 22
stream3715 0:07431908151a 23 Note: The MPU6050 is an I2C sensor and uses the Arduino Wire library.
stream3715 0:07431908151a 24 Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
stream3715 0:07431908151a 25 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
stream3715 0:07431908151a 26 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
stream3715 0:07431908151a 27 */
stream3715 0:07431908151a 28
stream3715 0:07431908151a 29 #include "mbed.h"
stream3715 0:07431908151a 30 #include "MPU6050.h"
stream3715 0:07431908151a 31
stream3715 0:07431908151a 32 float sum = 0;
stream3715 0:07431908151a 33 uint32_t sumCount = 0;
stream3715 0:07431908151a 34
stream3715 0:07431908151a 35 MPU6050 mpu6050;
stream3715 0:07431908151a 36
stream3715 0:07431908151a 37 Timer t;
stream3715 3:9424c6493a75 38
stream3715 3:9424c6493a75 39 Serial pc(USBTX, USBRX); // tx, rx
stream3715 0:07431908151a 40
stream3715 0:07431908151a 41 int main()
stream3715 0:07431908151a 42 {
stream3715 3:9424c6493a75 43 pc.baud(9600);
stream3715 3:9424c6493a75 44
stream3715 0:07431908151a 45 //Set up I2C
stream3715 0:07431908151a 46 i2c.frequency(400000); // use fast (400 kHz) I2C
stream3715 0:07431908151a 47
stream3715 3:9424c6493a75 48 t.start();
stream3715 0:07431908151a 49
stream3715 0:07431908151a 50 // Read the WHO_AM_I register, this is a good test of communication
stream3715 0:07431908151a 51 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
stream3715 3:9424c6493a75 52 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
stream3715 0:07431908151a 53
stream3715 0:07431908151a 54 if (whoami == 0x68) // WHO_AM_I should always be 0x68
stream3715 0:07431908151a 55 {
stream3715 3:9424c6493a75 56 pc.printf("MPU6050 is online...");
stream3715 0:07431908151a 57 wait(1);
stream3715 0:07431908151a 58
stream3715 0:07431908151a 59 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
stream3715 0:07431908151a 60 pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
stream3715 0:07431908151a 61 pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
stream3715 0:07431908151a 62 pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
stream3715 0:07431908151a 63 pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
stream3715 0:07431908151a 64 pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
stream3715 0:07431908151a 65 pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
stream3715 0:07431908151a 66 wait(1);
stream3715 0:07431908151a 67
stream3715 0:07431908151a 68 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)
stream3715 0:07431908151a 69 {
stream3715 0:07431908151a 70 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
stream3715 0:07431908151a 71 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
stream3715 0:07431908151a 72 mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
stream3715 0:07431908151a 73
stream3715 0:07431908151a 74 wait(2);
stream3715 0:07431908151a 75 }
stream3715 0:07431908151a 76 else
stream3715 0:07431908151a 77 {
stream3715 0:07431908151a 78 pc.printf("Device did not the pass self-test!\n\r");
stream3715 0:07431908151a 79
stream3715 0:07431908151a 80 }
stream3715 0:07431908151a 81 }
stream3715 0:07431908151a 82 else
stream3715 0:07431908151a 83 {
stream3715 3:9424c6493a75 84 pc.printf("Could not connect to MPU6050: \n\r");
stream3715 0:07431908151a 85 pc.printf("%#x \n", whoami);
stream3715 0:07431908151a 86
stream3715 0:07431908151a 87 while(1) ; // Loop forever if communication doesn't happen
stream3715 0:07431908151a 88 }
stream3715 0:07431908151a 89
stream3715 0:07431908151a 90
stream3715 0:07431908151a 91
stream3715 0:07431908151a 92 while(1) {
stream3715 0:07431908151a 93
stream3715 0:07431908151a 94 // If data ready bit set, all data registers have new data
stream3715 0:07431908151a 95 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
stream3715 0:07431908151a 96 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
stream3715 0:07431908151a 97 mpu6050.getAres();
stream3715 0:07431908151a 98
stream3715 0:07431908151a 99 // Now we'll calculate the accleration value into actual g's
stream3715 0:07431908151a 100 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
stream3715 0:07431908151a 101 ay = (float)accelCount[1]*aRes - accelBias[1];
stream3715 0:07431908151a 102 az = (float)accelCount[2]*aRes - accelBias[2];
stream3715 0:07431908151a 103
stream3715 0:07431908151a 104 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
stream3715 0:07431908151a 105 mpu6050.getGres();
stream3715 0:07431908151a 106
stream3715 0:07431908151a 107 // Calculate the gyro value into actual degrees per second
stream3715 0:07431908151a 108 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
stream3715 0:07431908151a 109 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
stream3715 0:07431908151a 110 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
stream3715 0:07431908151a 111
stream3715 0:07431908151a 112 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
stream3715 0:07431908151a 113 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
stream3715 0:07431908151a 114 }
stream3715 0:07431908151a 115
stream3715 0:07431908151a 116 Now = t.read_us();
stream3715 0:07431908151a 117 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
stream3715 0:07431908151a 118 lastUpdate = Now;
stream3715 0:07431908151a 119
stream3715 0:07431908151a 120 sum += deltat;
stream3715 0:07431908151a 121 sumCount++;
stream3715 0:07431908151a 122
stream3715 0:07431908151a 123 if(lastUpdate - firstUpdate > 10000000.0f) {
stream3715 0:07431908151a 124 beta = 0.04; // decrease filter gain after stabilized
stream3715 0:07431908151a 125 zeta = 0.015; // increasey bias drift gain after stabilized
stream3715 0:07431908151a 126 }
stream3715 0:07431908151a 127
stream3715 0:07431908151a 128 // Pass gyro rate as rad/s
stream3715 0:07431908151a 129 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
stream3715 0:07431908151a 130
stream3715 0:07431908151a 131 // Serial print and/or display at 0.5 s rate independent of data rates
stream3715 0:07431908151a 132 delt_t = t.read_ms() - count;
stream3715 0:07431908151a 133 if (delt_t > 500) { // update LCD once per half-second independent of read rate
stream3715 0:07431908151a 134
stream3715 0:07431908151a 135 pc.printf("ax = %f", 1000*ax);
stream3715 0:07431908151a 136 pc.printf(" ay = %f", 1000*ay);
stream3715 0:07431908151a 137 pc.printf(" az = %f mg\n\r", 1000*az);
stream3715 0:07431908151a 138
stream3715 0:07431908151a 139 pc.printf("gx = %f", gx);
stream3715 0:07431908151a 140 pc.printf(" gy = %f", gy);
stream3715 0:07431908151a 141 pc.printf(" gz = %f deg/s\n\r", gz);
stream3715 0:07431908151a 142
stream3715 0:07431908151a 143 pc.printf(" temperature = %f C\n\r", temperature);
stream3715 0:07431908151a 144
stream3715 0:07431908151a 145 pc.printf("q0 = %f\n\r", q[0]);
stream3715 0:07431908151a 146 pc.printf("q1 = %f\n\r", q[1]);
stream3715 0:07431908151a 147 pc.printf("q2 = %f\n\r", q[2]);
stream3715 0:07431908151a 148 pc.printf("q3 = %f\n\r", q[3]);
stream3715 0:07431908151a 149
stream3715 0:07431908151a 150
stream3715 0:07431908151a 151 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
stream3715 0:07431908151a 152 // In this coordinate system, the positive z-axis is down toward Earth.
stream3715 0:07431908151a 153 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
stream3715 0:07431908151a 154 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
stream3715 0:07431908151a 155 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
stream3715 0:07431908151a 156 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
stream3715 0:07431908151a 157 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
stream3715 0:07431908151a 158 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
stream3715 0:07431908151a 159 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
stream3715 0:07431908151a 160 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]);
stream3715 0:07431908151a 161 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
stream3715 0:07431908151a 162 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]);
stream3715 0:07431908151a 163 pitch *= 180.0f / PI;
stream3715 0:07431908151a 164 yaw *= 180.0f / PI;
stream3715 0:07431908151a 165 roll *= 180.0f / PI;
stream3715 0:07431908151a 166
stream3715 0:07431908151a 167 // pc.printf("Yaw, Pitch, Roll: \n\r");
stream3715 0:07431908151a 168 // pc.printf("%f", yaw);
stream3715 0:07431908151a 169 // pc.printf(", ");
stream3715 0:07431908151a 170 // pc.printf("%f", pitch);
stream3715 0:07431908151a 171 // pc.printf(", ");
stream3715 0:07431908151a 172 // pc.printf("%f\n\r", roll);
stream3715 0:07431908151a 173 // pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
stream3715 0:07431908151a 174
stream3715 3:9424c6493a75 175 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
stream3715 3:9424c6493a75 176 pc.printf("average rate = %f\n\r", (float) sumCount/sum);
stream3715 0:07431908151a 177
stream3715 3:9424c6493a75 178 myled= !myled;
stream3715 0:07431908151a 179 count = t.read_ms();
stream3715 0:07431908151a 180 sum = 0;
stream3715 0:07431908151a 181 sumCount = 0;
stream3715 0:07431908151a 182 }
stream3715 0:07431908151a 183 }
stream3715 0:07431908151a 184
stream3715 0:07431908151a 185 }